18 #ifndef PIC_COMPUTER_VISION_ESSENTIAL_MATRIX_HPP 19 #define PIC_COMPUTER_VISION_ESSENTIAL_MATRIX_HPP 25 #include "../base.hpp" 27 #include "../util/math.hpp" 28 #include "../util/eigen_util.hpp" 30 #include "../computer_vision/triangulation.hpp" 31 #include "../computer_vision/camera_matrix.hpp" 33 #ifndef PIC_DISABLE_EIGEN 35 #ifndef PIC_EIGEN_NOT_BUNDLED 36 #include "../externals/Eigen/Dense" 37 #include "../externals/Eigen/SVD" 38 #include "../externals/Eigen/Geometry" 40 #include <Eigen/Dense> 42 #include <Eigen/Geometry> 49 #ifndef PIC_DISABLE_EIGEN 61 Eigen::Matrix3d K2t = Eigen::Transpose<Eigen::Matrix3d>(K2);
94 Eigen::JacobiSVD< Eigen::MatrixXd > svd(E, Eigen::ComputeThinU | Eigen::ComputeThinV);
95 Eigen::Matrix3d U = svd.matrixU();
96 Eigen::Matrix3d V = svd.matrixV();
112 Eigen::Matrix3d Vt = Eigen::Transpose<Eigen::Matrix3d>(V);
113 Eigen::Matrix3d Wt = Eigen::Transpose<Eigen::Matrix3d>(W);
115 Eigen::Matrix3d UVt = U * Vt;
116 double detUVt = UVt.determinant();
118 R1 = detUVt * U * W * Vt;
119 R2 = detUVt * U * Wt * Vt;
125 Eigen::Matrix3d Ut = Eigen::Transpose<Eigen::Matrix3d>(U);
126 Eigen::Matrix3d S = U * Z * Ut;
147 std::vector< Eigen::Vector2f > &points0, std::vector< Eigen::Vector2f > &points1,
148 Eigen::Matrix3d &R, Eigen::Vector3d &t)
150 if(points0.size() != points1.size()) {
154 Eigen::Matrix3d R0, R1;
163 for(
unsigned int j = 0; j < 4; j++) {
165 Eigen::Matrix3d tmp_R = (j < 2) ? R0 : R1;
166 Eigen::Vector3d tmp_T;
178 for(
unsigned int i = 0; i < points0.size(); i++) {
180 Eigen::Vector3d point_0 = Eigen::Vector3d(points0[i][0], points0[i][1], 1.0);
181 Eigen::Vector3d point_1 = Eigen::Vector3d(points1[i][0], points1[i][1], 1.0);
184 Eigen::Vector3d p0_euc = Eigen::Vector3d(p0[0], p0[1], p0[2]);
187 if((p0[2] >= 0.0) && (p1[2] >= 0.0)) {
194 printf(
"Front: %d %d\n",tmp_counter,j);
197 if(tmp_counter > counter) {
199 counter = tmp_counter;
205 R = (type < 2) ? R0 : R1;
207 if((type % 2) == 0) {
223 #endif // PIC_DISABLE_EIGEN 227 #endif // PIC_COMPUTER_VISION_ESSENTIAL_MATRIX_HPP PIC_INLINE Eigen::Matrix34d getCameraMatrixIdentity(Eigen::Matrix3d &K)
getCameraMatrixIdentity
Definition: camera_matrix.hpp:118
PIC_INLINE Eigen::Matrix34d getCameraMatrix(Eigen::Matrix3d &K, Eigen::Matrix3d &R, Eigen::Vector3d &t)
getCameraMatrix
Definition: camera_matrix.hpp:132
PIC_INLINE Eigen::Vector4d triangulationHartleySturm(Eigen::Vector3d &point_0, Eigen::Vector3d &point_1, Eigen::Matrix34d &M0, Eigen::Matrix34d &M1, int maxIter=100)
triangulationHartl Sturm
Definition: triangulation.hpp:83
#define PIC_INLINE
Definition: base.hpp:33
PIC_INLINE void decomposeEssentialMatrix(Eigen::Matrix3d &E, Eigen::Matrix3d &R1, Eigen::Matrix3d &R2, Eigen::Vector3d &t)
decomposeEssentialMatrix decomposes an essential matrix E.
Definition: essential_matrix.hpp:91
PIC_INLINE Eigen::Vector3d rigidTransform(Eigen::Vector3d &point, Eigen::Matrix3d &R, Eigen::Vector3d &t)
rigidTransform computes a rigidi transformation in 3D.
Definition: eigen_util.hpp:333
Definition: bilateral_separation.hpp:25
PIC_INLINE Eigen::Matrix3d computeEssentialMatrix(Eigen::Matrix3d &F, Eigen::Matrix3d &K1, Eigen::Matrix3d &K2)
computeEssentialMatrix computes the essential matrix, E, from the fundamental matrix, F12, and the two intrics matrices K1 and K2
Definition: essential_matrix.hpp:59
PIC_INLINE Eigen::Matrix3d RotationMatrixRefinement(Eigen::Matrix3d &R)
RotationMatrixRefinement.
Definition: eigen_util.hpp:343
PIC_INLINE bool decomposeEssentialMatrixWithConfiguration(Eigen::Matrix3d &E, Eigen::Matrix3d &K0, Eigen::Matrix3d &K1, std::vector< Eigen::Vector2f > &points0, std::vector< Eigen::Vector2f > &points1, Eigen::Matrix3d &R, Eigen::Vector3d &t)
decomposeEssentialMatrixWithConfiguration decomposes an essential matrix E.
Definition: essential_matrix.hpp:146