Skip to content

Commit

Permalink
Refactor the perspective class
Browse files Browse the repository at this point in the history
  • Loading branch information
@shinsumicco
shinsumicco committed Aug 22, 2019
1 parent 342038e commit 0d4d7e5
Show file tree
Hide file tree
Showing 2 changed files with 32 additions and 42 deletions.
55 changes: 20 additions & 35 deletions src/openvslam/camera/perspective.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -21,10 +21,8 @@ perspective::perspective(const std::string& name, const setup_type_t& setup_type
eigen_cam_matrix_ << fx_, 0, cx_, 0, fy_, cy_, 0, 0, 1;
eigen_dist_params_ << k1_, k2_, p1_, p2_, k3_;

// 画像の最大範囲を計算
img_bounds_ = compute_image_bounds();

// セルサイズの逆数を計算しておく
inv_cell_width_ = static_cast<double>(num_grid_cols_) / (img_bounds_.max_x_ - img_bounds_.min_x_);
inv_cell_height_ = static_cast<double>(num_grid_rows_) / (img_bounds_.max_y_ - img_bounds_.min_y_);
}
Expand Down Expand Up @@ -72,19 +70,18 @@ image_bounds perspective::compute_image_bounds() const {
spdlog::debug("compute image bounds");

if (k1_ == 0 && k2_ == 0 && p1_ == 0 && p2_ == 0 && k3_ == 0) {
// 歪み無し
// any distortion does not exist

return image_bounds{0.0, cols_, 0.0, rows_};
}
else {
// 歪み有り
// distortion exists

// 画像の4隅の座標
// (x, y) = (列, 行)の順で渡す
const std::vector<cv::KeyPoint> corners{cv::KeyPoint(0.0, 0.0, 1.0), // 左上
cv::KeyPoint(cols_, 0.0, 1.0), // 右上
cv::KeyPoint(0.0, rows_, 1.0), // 左下
cv::KeyPoint(cols_, rows_, 1.0)}; // 右下
// corner coordinates: (x, y) = (col, row)
const std::vector<cv::KeyPoint> corners{cv::KeyPoint(0.0, 0.0, 1.0), // left top
cv::KeyPoint(cols_, 0.0, 1.0), // right top
cv::KeyPoint(0.0, rows_, 1.0), // left bottom
cv::KeyPoint(cols_, rows_, 1.0)}; // right bottom

std::vector<cv::KeyPoint> undist_corners;
undistort_keypoints(corners, undist_corners);
Expand Down Expand Up @@ -149,57 +146,45 @@ void perspective::convert_bearings_to_keypoints(const eigen_alloc_vector<Vec3_t>
}

bool perspective::reproject_to_image(const Mat33_t& rot_cw, const Vec3_t& trans_cw, const Vec3_t& pos_w, Vec2_t& reproj, float& x_right) const {
// カメラ基準の座標に変換
// convert to camera-coordinates
const Vec3_t pos_c = rot_cw * pos_w + trans_cw;

// カメラの正面になければ不可視
// check if the point is visible
if (pos_c(2) <= 0.0) {
return false;
}

// 画像上に投影
// reproject onto the image
const auto z_inv = 1.0 / pos_c(2);
reproj(0) = fx_ * pos_c(0) * z_inv + cx_;
reproj(1) = fy_ * pos_c(1) * z_inv + cy_;
x_right = reproj(0) - focal_x_baseline_ * z_inv;

// 画像内であることを確認
if (reproj(0) < img_bounds_.min_x_ || reproj(0) > img_bounds_.max_x_) {
return false;
}
if (reproj(1) < img_bounds_.min_y_ || reproj(1) > img_bounds_.max_y_) {
return false;
}

return true;
// check if the point is visible
return (img_bounds_.min_x_ < reproj(0) && reproj(0) < img_bounds_.max_x_
&& img_bounds_.min_y_ < reproj(1) && reproj(1) < img_bounds_.max_y_);
}

bool perspective::reproject_to_bearing(const Mat33_t& rot_cw, const Vec3_t& trans_cw, const Vec3_t& pos_w, Vec3_t& reproj) const {
// カメラ基準の座標に変換
// convert to camera-coordinates
reproj = rot_cw * pos_w + trans_cw;

// カメラの正面になければ不可視
// check if the point is visible
if (reproj(2) <= 0.0) {
return false;
}

// 画像上に投影
// reproject onto the image
const auto z_inv = 1.0 / reproj(2);
const auto x = fx_ * reproj(0) * z_inv + cx_;
const auto y = fy_ * reproj(1) * z_inv + cy_;

// 画像内であることを確認
if (x < img_bounds_.min_x_ || x > img_bounds_.max_x_) {
return false;
}
if (y < img_bounds_.min_y_ || y > img_bounds_.max_y_) {
return false;
}

// bearingにする
// convert to a bearing
reproj.normalize();

return true;
// check if the point is visible
return (img_bounds_.min_x_ < x && x < img_bounds_.max_x_
&& img_bounds_.min_y_ < y && y < img_bounds_.max_y_);
}

nlohmann::json perspective::to_json() const {
Expand Down
19 changes: 12 additions & 7 deletions src/openvslam/camera/perspective.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -22,7 +22,7 @@ class perspective final : public base {

image_bounds compute_image_bounds() const override final;

inline cv::KeyPoint undistort_keypoint(const cv::KeyPoint& dist_keypt) const override final {
cv::KeyPoint undistort_keypoint(const cv::KeyPoint& dist_keypt) const override final {
// fill cv::Mat with distorted keypoints
cv::Mat mat(1, 2, CV_32F);
mat.at<float>(0, 0) = dist_keypt.pt.x;
Expand All @@ -47,7 +47,7 @@ class perspective final : public base {

void undistort_keypoints(const std::vector<cv::KeyPoint>& dist_keypts, std::vector<cv::KeyPoint>& undist_keypts) const override final;

inline Vec3_t convert_keypoint_to_bearing(const cv::KeyPoint& undist_keypt) const override final {
Vec3_t convert_keypoint_to_bearing(const cv::KeyPoint& undist_keypt) const override final {
const auto x_normalized = (undist_keypt.pt.x - cx_) / fx_;
const auto y_normalized = (undist_keypt.pt.y - cy_) / fy_;
const auto l2_norm = std::sqrt(x_normalized * x_normalized + y_normalized * y_normalized + 1.0);
Expand All @@ -56,7 +56,7 @@ class perspective final : public base {

void convert_keypoints_to_bearings(const std::vector<cv::KeyPoint>& undist_keypts, eigen_alloc_vector<Vec3_t>& bearings) const override final;

inline cv::KeyPoint convert_bearing_to_keypoint(const Vec3_t& bearing) const override final {
cv::KeyPoint convert_bearing_to_keypoint(const Vec3_t& bearing) const override final {
const auto x_normalized = bearing(0) / bearing(2);
const auto y_normalized = bearing(1) / bearing(2);

Expand All @@ -69,32 +69,37 @@ class perspective final : public base {

void convert_bearings_to_keypoints(const eigen_alloc_vector<Vec3_t>& bearings, std::vector<cv::KeyPoint>& undist_keypts) const override final;

// 画像内に再投影->true, 画像外に再投影->false
bool reproject_to_image(const Mat33_t& rot_cw, const Vec3_t& trans_cw, const Vec3_t& pos_w, Vec2_t& reproj, float& x_right) const override final;

// 画像内に再投影->true, 画像外に再投影->false
bool reproject_to_bearing(const Mat33_t& rot_cw, const Vec3_t& trans_cw, const Vec3_t& pos_w, Vec3_t& reproj) const override final;

nlohmann::json to_json() const override final;

//-------------------------
// Parameters specific to this model

//! pinhole params
const double fx_;
const double fy_;
const double cx_;
const double cy_;
const double fx_inv_;
const double fy_inv_;

//! distortion params
const double k1_;
const double k2_;
const double p1_;
const double p2_;
const double k3_;

// camera matrix
//! camera matrix in OpenCV format
cv::Mat cv_cam_matrix_;
//! camera matrix in Eigen format
Mat33_t eigen_cam_matrix_;
// distortion params
//! distortion params in OpenCV format
cv::Mat cv_dist_params_;
//! distortion params in Eigen format
Vec5_t eigen_dist_params_;
};

Expand Down

0 comments on commit 0d4d7e5

Please sign in to comment.