18 #ifndef PIC_COMPUTER_VISION_FUNDAMENTAL_HPP 19 #define PIC_COMPUTER_VISION_FUNDAMENTAL_HPP 25 #include "../base.hpp" 27 #include "../image.hpp" 29 #include "../filtering/filter_luminance.hpp" 30 #include "../filtering/filter_gaussian_2d.hpp" 32 #include "../util/math.hpp" 33 #include "../util/eigen_util.hpp" 35 #include "../features_matching/orb_descriptor.hpp" 36 #include "../features_matching/feature_matcher.hpp" 37 #include "../features_matching/binary_feature_lsh_matcher.hpp" 38 #include "../computer_vision/nelder_mead_opt_fundamental.hpp" 40 #ifndef PIC_DISABLE_EIGEN 42 #ifndef PIC_EIGEN_NOT_BUNDLED 43 #include "../externals/Eigen/Dense" 44 #include "../externals/Eigen/SVD" 45 #include "../externals/Eigen/Geometry" 47 #include <Eigen/Dense> 49 #include <Eigen/Geometry> 56 #ifndef PIC_DISABLE_EIGEN 65 std::vector< Eigen::Vector2f > &points1)
69 if((points0.size() != points1.size()) || (points0.size() < 8)) {
81 Eigen::MatrixXd A(points0.size(), 9);
84 for(
unsigned int i = 0; i < points0.size(); i++) {
87 Eigen::Vector2f p0 = points0[i];
88 Eigen::Vector2f p1 = points1[i];
90 p0[0] = (p0[0] - transform_0[0]) / transform_0[2];
91 p0[1] = (p0[1] - transform_0[1]) / transform_0[2];
93 p1[0] = (p1[0] - transform_1[0]) / transform_1[2];
94 p1[1] = (p1[1] - transform_1[1]) / transform_1[2];
96 A(i, 0) = p0[0] * p1[0];
97 A(i, 1) = p0[0] * p1[1];
99 A(i, 3) = p0[1] * p1[0];
100 A(i, 4) = p0[1] * p1[1];
108 Eigen::JacobiSVD< Eigen::MatrixXd > svd(A, Eigen::ComputeFullV);
109 Eigen::MatrixXd V = svd.matrixV();
111 int n = int(V.cols()) - 1;
126 Eigen::Matrix3d mat_1_t = Eigen::Transpose< Eigen::Matrix3d >(mat_1);
127 F = mat_1_t * F * mat_0;
130 Eigen::JacobiSVD< Eigen::MatrixXd > svdF(F, Eigen::ComputeThinU | Eigen::ComputeThinV);
131 Eigen::Matrix3d Uf = svdF.matrixU();
132 Eigen::Matrix3d Vf = svdF.matrixV();
133 Eigen::Vector3d Df = svdF.singularValues();
136 Eigen::Matrix3d F_new = Uf *
DiagonalMatrix(Df) * Eigen::Transpose< Eigen::Matrix3d >(Vf);
138 double norm =
MAX(Df[0], Df[1]);
151 std::vector< Eigen::Vector2f > &points1,
152 std::vector< unsigned int > &inliers,
153 unsigned int maxIterations = 100,
154 double threshold = 0.01,
155 unsigned int seed = 1)
157 if(points0.size() < 9) {
164 std::mt19937 m(seed);
166 unsigned int n = int(points0.size());
168 unsigned int *subSet =
new unsigned int [nSubSet];
172 for(
unsigned int i = 0; i < maxIterations; i++) {
175 std::vector< Eigen::Vector2f > sub_points0;
176 std::vector< Eigen::Vector2f > sub_points1;
178 for(
int j = 0; j < nSubSet; j++) {
179 unsigned int k = subSet[j];
180 sub_points0.push_back(points0[k]);
181 sub_points1.push_back(points1[k]);
187 std::vector< unsigned int > tmp_inliers;
189 for(
unsigned int j = 0; j < n; j++) {
190 Eigen::Vector3d p0 = Eigen::Vector3d(points0[j][0], points0[j][1], 1.0);
191 Eigen::Vector3d p1 = Eigen::Vector3d(points1[j][0], points1[j][1], 1.0);
193 Eigen::Vector3d tmpF_p0 = tmpF * p0;
194 double n0 = sqrt(tmpF_p0[0] * tmpF_p0[0] + tmpF_p0[1] * tmpF_p0[1]);
199 double err = fabs(tmpF_p0.dot(p1));
202 tmp_inliers.push_back(j);
207 if(tmp_inliers.size() > inliers.size()) {
210 inliers.assign(tmp_inliers.begin(), tmp_inliers.end());
215 if(inliers.size() > 7) {
218 printf(
"Better estimate using inliers only.\n");
221 std::vector< Eigen::Vector2f > sub_points0;
222 std::vector< Eigen::Vector2f > sub_points1;
224 for(
unsigned int i = 0; i < inliers.size(); i++) {
225 sub_points0.push_back(points0[inliers[i]]);
226 sub_points1.push_back(points1[inliers[i]]);
241 std::vector< Eigen::Vector2f > &points1,
242 std::vector< unsigned int > &inliers,
243 unsigned int maxIterationsRansac = 100,
244 double thresholdRansac = 0.01,
245 unsigned int seed = 1,
246 unsigned int maxIterationsNonLinear = 10000,
247 float thresholdNonLinear = 1e-4f
269 Eigen::JacobiSVD< Eigen::Matrix3d > svdF(F, Eigen::ComputeThinU | Eigen::ComputeThinV);
270 Eigen::Matrix3d Uf = svdF.matrixU();
271 Eigen::Matrix3d Vf = svdF.matrixV();
272 Eigen::Vector3d Df = svdF.singularValues();
275 Eigen::Matrix3d F_new = Uf *
DiagonalMatrix(Df) * Eigen::Transpose< Eigen::Matrix3d >(Vf);
277 double norm =
MAX(Df[0], Df[1]);
295 Eigen::Matrix3d M0_inv = M0_3.inverse();
299 Eigen::Matrix3d M1_inv = M1_3.inverse();
317 F = F * M1_3 * M0_inv;
319 Eigen::JacobiSVD< Eigen::Matrix3d > svdF(F, Eigen::ComputeThinU | Eigen::ComputeThinV);
320 Eigen::Vector3d Df = svdF.singularValues();
322 double norm =
MAX(Df[0],
MAX(Df[1], Df[2]));
334 std::vector< Eigen::Vector2f > &m0,
335 std::vector< Eigen::Vector2f > &m1,
336 std::vector< unsigned int > &inliers)
339 if(img0 == NULL || img1 == NULL) {
348 std::vector< Eigen::Vector2f > corners_from_img0;
349 std::vector< Eigen::Vector2f > corners_from_img1;
357 hcd.
execute(L0, &corners_from_img0);
358 hcd.
execute(L1, &corners_from_img1);
369 std::vector< unsigned int *> descs0;
370 b_desc.
getAll(L0_flt, corners_from_img0, descs0);
372 std::vector< unsigned int *> descs1;
373 b_desc.
getAll(L1_flt, corners_from_img1, descs1);
376 std::vector< Eigen::Vector3i > matches;
397 #endif // PIC_DISABLE_EIGEN 401 #endif // PIC_COMPUTER_VISION_FUNDAMENTAL_HPP Definition: filter_luminance.hpp:28
PIC_INLINE Eigen::Matrix3d getShiftScaleMatrix(Eigen::Vector3f &info)
getShiftScaleMatrix computes a shifting and scaling matrix
Definition: eigen_util.hpp:297
PIC_INLINE float * getLinearArrayFromMatrix(Eigen::Matrix3d &mat)
getLinearArray
Definition: eigen_util.hpp:404
Definition: nelder_mead_opt_fundamental.hpp:38
PIC_INLINE Eigen::Matrix3d estimateFundamentalRansac(std::vector< Eigen::Vector2f > &points0, std::vector< Eigen::Vector2f > &points1, std::vector< unsigned int > &inliers, unsigned int maxIterations=100, double threshold=0.01, unsigned int seed=1)
estimateFundamentalRansac
Definition: fundamental_matrix.hpp:150
void execute(Image *img, std::vector< Eigen::Vector2f > *corners)
execute
Definition: harris_corner_detector.hpp:137
PIC_INLINE Eigen::Matrix3d estimateFundamental(std::vector< Eigen::Vector2f > &points0, std::vector< Eigen::Vector2f > &points1)
estimateFundamental estimates the foundamental matrix between image 1 to image 2
Definition: fundamental_matrix.hpp:64
PIC_INLINE Eigen::Matrix3d estimateFundamentalFromImages(Image *img0, Image *img1, std::vector< Eigen::Vector2f > &m0, std::vector< Eigen::Vector2f > &m1, std::vector< unsigned int > &inliers)
estimateFundamentalFromImages
Definition: fundamental_matrix.hpp:332
PIC_INLINE Eigen::Matrix3d estimateFundamentalWithNonLinearRefinement(std::vector< Eigen::Vector2f > &points0, std::vector< Eigen::Vector2f > &points1, std::vector< unsigned int > &inliers, unsigned int maxIterationsRansac=100, double thresholdRansac=0.01, unsigned int seed=1, unsigned int maxIterationsNonLinear=10000, float thresholdNonLinear=1e-4f)
estimateFundamentalWithNonLinearRefinement
Definition: fundamental_matrix.hpp:240
PIC_INLINE Eigen::Vector3d getLastColumn(Eigen::Matrix34d &mat)
getLastColumn
Definition: eigen_util.hpp:169
PIC_INLINE Eigen::Matrix3d extractFundamentalMatrix(Eigen::Matrix34d &M0, Eigen::Matrix34d &M1, Eigen::VectorXd &e0, Eigen::VectorXd &e1)
extractFundamentalMatrix
Definition: fundamental_matrix.hpp:289
PIC_INLINE Eigen::Vector3f addOne(Eigen::Vector2f &x)
addOne
Definition: eigen_util.hpp:185
The LSH class.
Definition: binary_feature_lsh_matcher.hpp:34
The HarrisCornerDetector class.
Definition: harris_corner_detector.hpp:54
static Image * execute(Image *imgIn, Image *imgOut, LUMINANCE_TYPE type=LT_CIE_LUMINANCE)
execute
Definition: filter_luminance.hpp:166
virtual Scalar * run(Scalar *x_start, unsigned int n, Scalar epsilon=1e-4f, int max_iterations=1000, Scalar *x=NULL)
Definition: nelder_mead_opt_base.hpp:333
#define PIC_INLINE
Definition: base.hpp:33
PIC_INLINE Eigen::Matrix3d getSquareMatrix(Eigen::Matrix34d &mat)
getSquareMatrix
Definition: eigen_util.hpp:146
void getAllMatches(std::vector< unsigned int *> &descs0, std::vector< Eigen::Vector3i > &matches)
Definition: feature_matcher.hpp:81
The Image class stores an image as buffer of float.
Definition: image.hpp:60
void getAll(Image *img, std::vector< Eigen::Vector2f > &corners, std::vector< uint * > &descs)
getAll
Definition: brief_descriptor.hpp:226
PIC_INLINE Eigen::Matrix3d DiagonalMatrix(Eigen::Vector3d D)
DiagonalMatrix creates a diagonal matrix.
Definition: eigen_util.hpp:113
The ORBDescriptor class.
Definition: orb_descriptor.hpp:34
PIC_INLINE Eigen::Matrix3d noramalizeFundamentalMatrix(Eigen::Matrix3d F)
noramalizeFundamentalMatrix
Definition: fundamental_matrix.hpp:267
static void filterMatches(std::vector< Eigen::Vector2f > &c0, std::vector< Eigen::Vector2f > &c1, std::vector< Eigen::Vector3i > &matches, std::vector< Eigen::Vector2f > &p0, std::vector< Eigen::Vector2f > &p1)
filterMatches
Definition: feature_matcher.hpp:103
Definition: bilateral_separation.hpp:25
PIC_INLINE Eigen::Vector3f ComputeNormalizationTransform(std::vector< Eigen::Vector2f > &points)
ComputeNormalizationTransform.
Definition: eigen_util.hpp:530
#define MAX(a, b)
Definition: math.hpp:73
int getDescriptorSize()
getDescriptorSize returns the descriptor size.
Definition: brief_descriptor.hpp:244
static Image * execute(Image *imgIn, Image *imgOut, float sigma)
execute
Definition: filter_gaussian_2d.hpp:84
PIC_INLINE Eigen::MatrixXd getMatrixdFromLinearArray(float *array, int rows, int cols)
getMatrixFromLinearArray
Definition: eigen_util.hpp:469
PIC_INLINE void getRandomPermutation(std::mt19937 &m, unsigned int *perm, unsigned int nPerm, unsigned int n)
getRandomPermutation computes a random permutation.
Definition: math.hpp:434