18 #ifndef PIC_COMPUTER_VISION_CAMERA_MARTIX_HPP 19 #define PIC_COMPUTER_VISION_CAMERA_MARTIX_HPP 25 #include "../base.hpp" 27 #include "../util/math.hpp" 28 #include "../util/eigen_util.hpp" 30 #ifndef PIC_DISABLE_EIGEN 32 #ifndef PIC_EIGEN_NOT_BUNDLED 33 #include "../externals/Eigen/Dense" 34 #include "../externals/Eigen/SVD" 35 #include "../externals/Eigen/Geometry" 36 #include "../externals/Eigen/Geometry" 37 #include "../externals/Eigen/QR" 39 #include <Eigen/Dense> 41 #include <Eigen/Geometry> 49 #ifndef PIC_DISABLE_EIGEN 58 Eigen::JacobiSVD< Eigen::Matrix3d > svdF(F, Eigen::ComputeFullV);
59 Eigen::Matrix3d V = svdF.matrixV();
81 Eigen::Matrix3d K_inv = K.inverse();
83 Eigen::Matrix3d H_p = K_inv * H;
85 Eigen::Vector3d r_0(H_p(0, 0), H_p(1, 0), H_p(2, 0));
86 Eigen::Vector3d r_1(H_p(0, 1), H_p(1, 1), H_p(2, 1));
90 Eigen::Vector3d r_2 = r_0.cross(r_1);
92 Eigen::Vector3d t(H_p(0, 2), H_p(1, 2), H_p(2, 2));
167 Eigen::Matrix3d matrix = P.block<3, 3>(0, 0).inverse();
171 Eigen::HouseholderQR<Eigen::Matrix3d> qr(matrix.rows(), matrix.cols());
174 Eigen::Matrix3d Q = qr.householderQ();
175 Eigen::Matrix3d U = qr.matrixQR().triangularView<Eigen::Upper>();
178 Eigen::Vector3d d = U_d;
179 for(
int i = 0; i < 3; i++) {
181 d[i] = U_d[i] > 0.0 ? 1.0 : -1.0;
190 auto Q_t = Eigen::Transpose< Eigen::Matrix3d >(Q);
191 auto s = Q.determinant();
194 t = s * U * P.col(3);
212 Eigen::Vector3d proj = M * p;
216 return Eigen::Vector2i(
int(proj[0]),
int(proj[1]));
228 Eigen::Vector4d p4d(p[0], p[1], p[2], 1.0);
245 Eigen::Vector4d p_t = Eigen::Vector4d(p[0], p[1], p[2], 1.0);
247 Eigen::Vector3d proj = M * p_t;
251 double x_cx = (proj[0] - cx);
252 double y_cy = (proj[1] - cy);
254 double dx = x_cx / fx;
255 double dy = y_cy / fy;
256 double rho_sq = dx * dx + dy * dy;
258 double factor = 1.0 / (1.0 + rho_sq * lambda);
260 proj[0] = x_cx * factor + cx;
261 proj[1] = y_cy * factor + cy;
263 out[0] = int(proj[0]);
264 out[1] = int(proj[1]);
276 Eigen::Matrix3d Q = P.block<3, 3>(0, 0);
277 auto Q_inv = Q.inverse();
278 return - Q_inv * P.col(3);
295 Eigen::Matrix3d &K1, Eigen::Matrix3d &R1, Eigen::Vector3d &t1,
296 Eigen::Matrix34d &P0_out, Eigen::Matrix34d &P1_out,
297 Eigen::Matrix3d &T0, Eigen::Matrix3d &T1)
318 Eigen::Vector3d x_axis = c1 - c0;
319 Eigen::Vector3d tmp = R1.row(2);
320 Eigen::Vector3d y_axis = tmp.cross(x_axis);
321 Eigen::Vector3d z_axis = x_axis.cross(y_axis);
346 K(0, 2) = (K0(0, 2) + K1(0, 2)) * 0.5;
347 K(1, 2) = (K0(1, 2) + K1(1, 2)) * 0.5;
350 Eigen::Vector3d t0n = -R * c0;
353 Eigen::Vector3d t1n = -R * c1;
357 auto Q0o = P0_in.block<3, 3>(0, 0);
358 auto Q0n = P0_out.block<3, 3>(0, 0);
359 T0 = Q0n * Q0o.inverse();
361 auto Q1o = P1_in.block<3, 3>(0, 0);
362 auto Q1n = P1_out.block<3, 3>(0, 0);
363 T1 = Q1n * Q1o.inverse();
376 Eigen::Matrix34d &P0_out, Eigen::Matrix34d &P1_out,
377 Eigen::Matrix3d &T0, Eigen::Matrix3d &T1)
379 Eigen::Matrix3d K0, K1, R0, R1;
380 Eigen::Vector3d t0, t1;
392 #endif // PIC_DISABLE_EIGEN 396 #endif // PIC_COMPUTER_VISION_CAMERA_MARTIX_HPP PIC_INLINE Eigen::Matrix34d getCameraMatrixIdentity(Eigen::Matrix3d &K)
getCameraMatrixIdentity
Definition: camera_matrix.hpp:118
PIC_INLINE Eigen::Vector3d getDiagonalFromMatrix(Eigen::Matrix3d &mat)
getDiagonalFromMatrix
Definition: eigen_util.hpp:130
PIC_INLINE Eigen::Matrix34d getCameraMatrix(Eigen::Matrix3d &K, Eigen::Matrix3d &R, Eigen::Vector3d &t)
getCameraMatrix
Definition: camera_matrix.hpp:132
PIC_INLINE Eigen::Vector2i cameraMatrixProjection(Eigen::Matrix34d &M, Eigen::Vector3d &p, double cx, double cy, double fx, double fy, double lambda)
cameraMatrixProjection
Definition: camera_matrix.hpp:243
PIC_INLINE Eigen::Vector2i cameraMatrixProject(Eigen::Matrix34d &M, Eigen::Vector4d &p)
cameraMatrixProject projects a point, p, using the camera matrix, M.
Definition: camera_matrix.hpp:210
PIC_INLINE Eigen::Vector3d computeEpipole(Eigen::Matrix3d &F)
computeEpipole computes the epipole of a fundamental matrix F.
Definition: camera_matrix.hpp:56
#define PIC_INLINE
Definition: base.hpp:33
PIC_INLINE Eigen::Vector3d getOpticalCenter(Eigen::Matrix34d &P)
getOpticalCenter
Definition: camera_matrix.hpp:274
PIC_INLINE Eigen::Matrix3d DiagonalMatrix(Eigen::Vector3d D)
DiagonalMatrix creates a diagonal matrix.
Definition: eigen_util.hpp:113
PIC_INLINE void decomposeCameraMatrix(Eigen::Matrix34d &P, Eigen::Matrix3d &K, Eigen::Matrix3d &R, Eigen::Vector3d &t)
decomposeCameraMatrix
Definition: camera_matrix.hpp:162
PIC_INLINE void cameraRectify(Eigen::Matrix3d &K0, Eigen::Matrix3d &R0, Eigen::Vector3d &t0, Eigen::Matrix3d &K1, Eigen::Matrix3d &R1, Eigen::Vector3d &t1, Eigen::Matrix34d &P0_out, Eigen::Matrix34d &P1_out, Eigen::Matrix3d &T0, Eigen::Matrix3d &T1)
cameraRectify
Definition: camera_matrix.hpp:294
Definition: bilateral_separation.hpp:25
PIC_INLINE Eigen::Matrix34d getCameraMatrixFromHomography(Eigen::Matrix3d &H, Eigen::Matrix3d &K)
getCameraMatrixFromHomography
Definition: camera_matrix.hpp:76