CamRadtan.h
Go to the documentation of this file.
1 /*
2  * OpenVINS: An Open Platform for Visual-Inertial Research
3  * Copyright (C) 2018-2023 Patrick Geneva
4  * Copyright (C) 2018-2023 Guoquan Huang
5  * Copyright (C) 2018-2023 OpenVINS Contributors
6  * Copyright (C) 2018-2019 Kevin Eckenhoff
7  *
8  * This program is free software: you can redistribute it and/or modify
9  * it under the terms of the GNU General Public License as published by
10  * the Free Software Foundation, either version 3 of the License, or
11  * (at your option) any later version.
12  *
13  * This program is distributed in the hope that it will be useful,
14  * but WITHOUT ANY WARRANTY; without even the implied warranty of
15  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
16  * GNU General Public License for more details.
17  *
18  * You should have received a copy of the GNU General Public License
19  * along with this program. If not, see <https://www.gnu.org/licenses/>.
20  */
21 
22 #ifndef OV_CORE_CAM_RADTAN_H
23 #define OV_CORE_CAM_RADTAN_H
24 
25 #include "CamBase.h"
26 
27 namespace ov_core {
28 
82 class CamRadtan : public CamBase {
83 
84 public:
90  CamRadtan(int width, int height) : CamBase(width, height) {}
91 
93 
99  Eigen::Vector2f undistort_f(const Eigen::Vector2f &uv_dist) override {
100 
101  // Determine what camera parameters we should use
102  cv::Matx33d camK = camera_k_OPENCV;
103  cv::Vec4d camD = camera_d_OPENCV;
104 
105  // Convert to opencv format
106  cv::Mat mat(1, 2, CV_32F);
107  mat.at<float>(0, 0) = uv_dist(0);
108  mat.at<float>(0, 1) = uv_dist(1);
109  mat = mat.reshape(2); // Nx1, 2-channel
110 
111  // Undistort it!
112  cv::undistortPoints(mat, mat, camK, camD);
113 
114  // Construct our return vector
115  Eigen::Vector2f pt_out;
116  mat = mat.reshape(1); // Nx2, 1-channel
117  pt_out(0) = mat.at<float>(0, 0);
118  pt_out(1) = mat.at<float>(0, 1);
119  return pt_out;
120  }
121 
127  Eigen::Vector2f distort_f(const Eigen::Vector2f &uv_norm) override {
128 
129  // Get our camera parameters
130  Eigen::MatrixXd cam_d = camera_values;
131 
132  // Calculate distorted coordinates for radial
133  double r = std::sqrt(uv_norm(0) * uv_norm(0) + uv_norm(1) * uv_norm(1));
134  double r_2 = r * r;
135  double r_4 = r_2 * r_2;
136  double x1 = uv_norm(0) * (1 + cam_d(4) * r_2 + cam_d(5) * r_4) + 2 * cam_d(6) * uv_norm(0) * uv_norm(1) +
137  cam_d(7) * (r_2 + 2 * uv_norm(0) * uv_norm(0));
138  double y1 = uv_norm(1) * (1 + cam_d(4) * r_2 + cam_d(5) * r_4) + cam_d(6) * (r_2 + 2 * uv_norm(1) * uv_norm(1)) +
139  2 * cam_d(7) * uv_norm(0) * uv_norm(1);
140 
141  // Return the distorted point
142  Eigen::Vector2f uv_dist;
143  uv_dist(0) = (float)(cam_d(0) * x1 + cam_d(2));
144  uv_dist(1) = (float)(cam_d(1) * y1 + cam_d(3));
145  return uv_dist;
146  }
147 
154  void compute_distort_jacobian(const Eigen::Vector2d &uv_norm, Eigen::MatrixXd &H_dz_dzn, Eigen::MatrixXd &H_dz_dzeta) override {
155 
156  // Get our camera parameters
157  Eigen::MatrixXd cam_d = camera_values;
158 
159  // Calculate distorted coordinates for radial
160  double r = std::sqrt(uv_norm(0) * uv_norm(0) + uv_norm(1) * uv_norm(1));
161  double r_2 = r * r;
162  double r_4 = r_2 * r_2;
163 
164  // Jacobian of distorted pixel to normalized pixel
165  H_dz_dzn = Eigen::MatrixXd::Zero(2, 2);
166  double x = uv_norm(0);
167  double y = uv_norm(1);
168  double x_2 = uv_norm(0) * uv_norm(0);
169  double y_2 = uv_norm(1) * uv_norm(1);
170  double x_y = uv_norm(0) * uv_norm(1);
171  H_dz_dzn(0, 0) = cam_d(0) * ((1 + cam_d(4) * r_2 + cam_d(5) * r_4) + (2 * cam_d(4) * x_2 + 4 * cam_d(5) * x_2 * r_2) +
172  2 * cam_d(6) * y + (2 * cam_d(7) * x + 4 * cam_d(7) * x));
173  H_dz_dzn(0, 1) = cam_d(0) * (2 * cam_d(4) * x_y + 4 * cam_d(5) * x_y * r_2 + 2 * cam_d(6) * x + 2 * cam_d(7) * y);
174  H_dz_dzn(1, 0) = cam_d(1) * (2 * cam_d(4) * x_y + 4 * cam_d(5) * x_y * r_2 + 2 * cam_d(6) * x + 2 * cam_d(7) * y);
175  H_dz_dzn(1, 1) = cam_d(1) * ((1 + cam_d(4) * r_2 + cam_d(5) * r_4) + (2 * cam_d(4) * y_2 + 4 * cam_d(5) * y_2 * r_2) +
176  2 * cam_d(7) * x + (2 * cam_d(6) * y + 4 * cam_d(6) * y));
177 
178  // Calculate distorted coordinates for radtan
179  double x1 = uv_norm(0) * (1 + cam_d(4) * r_2 + cam_d(5) * r_4) + 2 * cam_d(6) * uv_norm(0) * uv_norm(1) +
180  cam_d(7) * (r_2 + 2 * uv_norm(0) * uv_norm(0));
181  double y1 = uv_norm(1) * (1 + cam_d(4) * r_2 + cam_d(5) * r_4) + cam_d(6) * (r_2 + 2 * uv_norm(1) * uv_norm(1)) +
182  2 * cam_d(7) * uv_norm(0) * uv_norm(1);
183 
184  // Compute the Jacobian in respect to the intrinsics
185  H_dz_dzeta = Eigen::MatrixXd::Zero(2, 8);
186  H_dz_dzeta(0, 0) = x1;
187  H_dz_dzeta(0, 2) = 1;
188  H_dz_dzeta(0, 4) = cam_d(0) * uv_norm(0) * r_2;
189  H_dz_dzeta(0, 5) = cam_d(0) * uv_norm(0) * r_4;
190  H_dz_dzeta(0, 6) = 2 * cam_d(0) * uv_norm(0) * uv_norm(1);
191  H_dz_dzeta(0, 7) = cam_d(0) * (r_2 + 2 * uv_norm(0) * uv_norm(0));
192  H_dz_dzeta(1, 1) = y1;
193  H_dz_dzeta(1, 3) = 1;
194  H_dz_dzeta(1, 4) = cam_d(1) * uv_norm(1) * r_2;
195  H_dz_dzeta(1, 5) = cam_d(1) * uv_norm(1) * r_4;
196  H_dz_dzeta(1, 6) = cam_d(1) * (r_2 + 2 * uv_norm(1) * uv_norm(1));
197  H_dz_dzeta(1, 7) = 2 * cam_d(1) * uv_norm(0) * uv_norm(1);
198  }
199 };
200 
201 } // namespace ov_core
202 
203 #endif /* OV_CORE_CAM_RADTAN_H */
ov_core::CamRadtan::~CamRadtan
~CamRadtan()
Definition: CamRadtan.h:92
ov_core::CamRadtan
Radial-tangential / Brown–Conrady model pinhole camera model class.
Definition: CamRadtan.h:82
ov_core::CamRadtan::undistort_f
Eigen::Vector2f undistort_f(const Eigen::Vector2f &uv_dist) override
Given a raw uv point, this will undistort it based on the camera matrices into normalized camera coor...
Definition: CamRadtan.h:99
ov_core::CamBase
Base pinhole camera model class.
Definition: CamBase.h:39
ov_core::CamBase::camera_d_OPENCV
cv::Vec4d camera_d_OPENCV
Camera distortion in OpenCV format.
Definition: CamBase.h:186
ov_core::CamBase::camera_k_OPENCV
cv::Matx33d camera_k_OPENCV
Camera intrinsics in OpenCV format.
Definition: CamBase.h:183
CamBase.h
ov_core::CamRadtan::CamRadtan
CamRadtan(int width, int height)
Default constructor.
Definition: CamRadtan.h:90
ov_core::CamBase::camera_values
Eigen::MatrixXd camera_values
Raw set of camera intrinic values (f_x & f_y & c_x & c_y & k_1 & k_2 & k_3 & k_4)
Definition: CamBase.h:180
ov_core::CamRadtan::compute_distort_jacobian
void compute_distort_jacobian(const Eigen::Vector2d &uv_norm, Eigen::MatrixXd &H_dz_dzn, Eigen::MatrixXd &H_dz_dzeta) override
Computes the derivative of raw distorted to normalized coordinate.
Definition: CamRadtan.h:154
ov_core
Core algorithms for OpenVINS.
Definition: CamBase.h:30
ov_core::CamRadtan::distort_f
Eigen::Vector2f distort_f(const Eigen::Vector2f &uv_norm) override
Given a normalized uv coordinate this will distort it to the raw image plane.
Definition: CamRadtan.h:127


ov_core
Author(s): Patrick Geneva , Kevin Eckenhoff , Guoquan Huang
autogenerated on Mon Dec 16 2024 03:06:46