18 #ifndef PIC_COMPUTER_VISION_HOMOGRAPHY_HPP 19 #define PIC_COMPUTER_VISION_HOMOGRAPHY_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" 37 #include <Eigen/Dense> 39 #include <Eigen/Geometry> 44 #include "../computer_vision/nelder_mead_opt_homography.hpp" 48 #ifndef PIC_DISABLE_EIGEN 57 std::vector< Eigen::Vector2f > &points1)
61 if((points0.size() != points1.size()) || (points0.size() < 4)) {
72 int n = int(points0.size());
73 Eigen::MatrixXd A(n * 2, 9);
76 for(
int i = 0; i < n; i++) {
78 Eigen::Vector2f p0 = points0[i];
79 Eigen::Vector2f p1 = points1[i];
81 p0[0] = (p0[0] - transform_0[0]) / transform_0[2];
82 p0[1] = (p0[1] - transform_0[1]) / transform_0[2];
84 p1[0] = (p1[0] - transform_1[0]) / transform_1[2];
85 p1[1] = (p1[1] - transform_1[1]) / transform_1[2];
94 A(j, 6) = -p1[1] * p0[0];
95 A(j, 7) = -p1[1] * p0[1];
106 A(j, 6) = -p1[0] * p0[0];
107 A(j, 7) = -p1[0] * p0[1];
112 Eigen::JacobiSVD< Eigen::MatrixXd > svd(A, Eigen::ComputeFullV);
113 Eigen::MatrixXd V = svd.matrixV();
115 n = int(V.cols()) - 1;
130 H = mat_1.inverse() * H * mat_0;
143 std::vector< Eigen::Vector2f > &points1,
144 std::vector< unsigned int > &inliers,
145 unsigned int maxIterations = 100,
146 double threshold = 4.0,
147 unsigned int seed = 1)
149 if(points0.size() < 5) {
156 std::mt19937 m(seed);
158 unsigned int n = int(points0.size());
160 unsigned int *subSet =
new unsigned int [nSubSet];
164 for(
unsigned int i = 0; i < maxIterations; i++) {
168 std::vector< Eigen::Vector2f > sub_points0;
169 std::vector< Eigen::Vector2f > sub_points1;
171 for(
int j = 0; j < nSubSet; j++) {
172 sub_points0.push_back(points0[subSet[j]]);
173 sub_points1.push_back(points1[subSet[j]]);
179 std::vector< unsigned int > tmp_inliers;
181 for(
unsigned int j = 0; j < n; j++) {
182 Eigen::Vector3d point_hom = Eigen::Vector3d(points0[j][0], points0[j][1], 1.0);
183 Eigen::Vector3d pp = tmpH * point_hom;
186 double dx = points1[j][0] - pp[0];
187 double dy = points1[j][1] - pp[1];
188 double squared_diff = (dx * dx) + (dy * dy);
190 if(squared_diff < threshold) {
191 tmp_inliers.push_back(j);
196 if(tmp_inliers.size() > inliers.size()) {
199 inliers.assign(tmp_inliers.begin(), tmp_inliers.end());
204 if(inliers.size() > 3) {
206 printf(
"Better estimate using inliers only.\n");
209 std::vector< Eigen::Vector2f > sub_points0;
210 std::vector< Eigen::Vector2f > sub_points1;
212 for(
unsigned int i = 0; i < inliers.size(); i++) {
213 sub_points0.push_back(points0[inliers[i]]);
214 sub_points1.push_back(points1[inliers[i]]);
232 std::vector< Eigen::Vector2f > &points0,
233 std::vector< Eigen::Vector2f > &points1,
234 std::vector< unsigned int > &inliers,
235 unsigned int maxIterationsRansac = 10000,
236 double thresholdRansac = 2.5,
237 unsigned int seedRansac = 1,
238 unsigned int maxIterationsNonLinear = 10000,
239 float thresholdNonLinear = 1e-5f
243 maxIterationsRansac, thresholdRansac,
248 nmoh.
run(H_array, 8, thresholdNonLinear, maxIterationsNonLinear, H_array);
253 #endif // PIC_DISABLE_EIGEN 257 #endif // PIC_COMPUTER_VISION_HOMOGRAPHY_HPP PIC_INLINE Eigen::Matrix3d estimateHomography(std::vector< Eigen::Vector2f > &points0, std::vector< Eigen::Vector2f > &points1)
estimateHomography estimates an homography matrix H between image 1 to image 2
Definition: homography_matrix.hpp:56
float * run(float *x_start, unsigned int n, float epsilon=1e-4f, int max_iterations=1000, float *x=NULL)
run
Definition: nelder_mead_opt_homography.hpp:115
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
PIC_INLINE Eigen::Matrix3d estimateHomographyWithNonLinearRefinement(std::vector< Eigen::Vector2f > &points0, std::vector< Eigen::Vector2f > &points1, std::vector< unsigned int > &inliers, unsigned int maxIterationsRansac=10000, double thresholdRansac=2.5, unsigned int seedRansac=1, unsigned int maxIterationsNonLinear=10000, float thresholdNonLinear=1e-5f)
estimateHomographyRansac computes the homography such that: points1 = H * points0 ...
Definition: homography_matrix.hpp:231
PIC_INLINE Eigen::Matrix3d estimateHomographyRansac(std::vector< Eigen::Vector2f > &points0, std::vector< Eigen::Vector2f > &points1, std::vector< unsigned int > &inliers, unsigned int maxIterations=100, double threshold=4.0, unsigned int seed=1)
estimateHomographyRansac computes the homography such that: points1 = H * points0 ...
Definition: homography_matrix.hpp:142
#define PIC_INLINE
Definition: base.hpp:33
Definition: bilateral_separation.hpp:25
PIC_INLINE Eigen::Vector3f ComputeNormalizationTransform(std::vector< Eigen::Vector2f > &points)
ComputeNormalizationTransform.
Definition: eigen_util.hpp:530
PIC_INLINE Eigen::Matrix3d getMatrix3dFromLinearArray(float *array)
getMatrix3dFromLinearArray
Definition: eigen_util.hpp:488
Definition: nelder_mead_opt_homography.hpp:36
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