From f1bafff595b28016466008d89a17a964f0c67f9e Mon Sep 17 00:00:00 2001 From: pmoulon Date: Sun, 25 Dec 2016 03:34:52 +0100 Subject: [PATCH] Use Eigen hnormalized() and homogeneous() function #709 --- src/openMVG/cameras/Camera_Intrinsics.hpp | 5 +- .../lInfinityCV/resection_kernel.hpp | 3 +- src/openMVG/multiview/conditioning.cpp | 18 ++----- src/openMVG/multiview/projection.cpp | 46 ++++++++--------- src/openMVG/multiview/projection.hpp | 9 +--- .../multiview/solver_essential_kernel.hpp | 49 +++++++++++++------ .../multiview/solver_fundamental_kernel.hpp | 31 +++++------- .../multiview/solver_homography_kernel.hpp | 6 +-- .../multiview/solver_resection_kernel.hpp | 33 ++++++++----- src/openMVG/multiview/triangulation_nview.cpp | 19 +++---- src/openMVG/multiview/triangulation_nview.hpp | 46 ++++++++--------- .../multiview/triangulation_nview_test.cpp | 15 +++--- src/openMVG/multiview/two_view_kernel.hpp | 11 +++-- src/openMVG/numeric/numeric.cpp | 32 ++++++------ src/software/opencv/accv12_demo/main.cpp | 16 +++--- 15 files changed, 165 insertions(+), 174 deletions(-) diff --git a/src/openMVG/cameras/Camera_Intrinsics.hpp b/src/openMVG/cameras/Camera_Intrinsics.hpp index 011bc6d90c..30ae1a9095 100644 --- a/src/openMVG/cameras/Camera_Intrinsics.hpp +++ b/src/openMVG/cameras/Camera_Intrinsics.hpp @@ -88,11 +88,11 @@ struct IntrinsicBase : public Clonable const Vec3 X = pose( pt3D ); // apply pose if ( this->have_disto() ) // apply disto & intrinsics { - return this->cam2ima( this->add_disto( X.head<2>() / X( 2 ) ) ); + return this->cam2ima( this->add_disto( X.hnormalized() ) ); } else // apply intrinsics { - return this->cam2ima( X.head<2>() / X( 2 ) ); + return this->cam2ima( X.hnormalized() ); } } @@ -290,4 +290,3 @@ inline double AngleBetweenRay( } // namespace openMVG #endif // #ifndef OPENMVG_CAMERA_INTRINSICS_H - diff --git a/src/openMVG/linearProgramming/lInfinityCV/resection_kernel.hpp b/src/openMVG/linearProgramming/lInfinityCV/resection_kernel.hpp index 8c24497649..c5ba559aad 100644 --- a/src/openMVG/linearProgramming/lInfinityCV/resection_kernel.hpp +++ b/src/openMVG/linearProgramming/lInfinityCV/resection_kernel.hpp @@ -34,8 +34,7 @@ struct l1SixPointResectionSolver { // Compute the residual of the projection distance(pt2D, Project(P,pt3D)) static double Error(const Mat34 & P, const Vec2 & pt2D, const Vec3 & pt3D) { - Vec2 x = Project(P, pt3D); - return (x-pt2D).norm(); + return ( Project(P, pt3D) - pt2D ).norm(); } }; diff --git a/src/openMVG/multiview/conditioning.cpp b/src/openMVG/multiview/conditioning.cpp index 58546de2c1..fa49be712e 100644 --- a/src/openMVG/multiview/conditioning.cpp +++ b/src/openMVG/multiview/conditioning.cpp @@ -43,14 +43,14 @@ void PreconditionerFromPoints(const Mat &points, Mat3 *T) { if (variance(1) < 1e-8) yfactor = mean(1) = 1.0; - *T << xfactor, 0, -xfactor * mean(0), - 0, yfactor, -yfactor * mean(1), - 0, 0, 1; + (*T) << xfactor, 0, -xfactor * mean(0), + 0, yfactor, -yfactor * mean(1), + 0, 0, 1; } void PreconditionerFromPoints(int width, int height, Mat3 *T) { // Build the normalization matrix - double dNorm = 1.0 / sqrt( static_cast(width*height) ); + const double dNorm = 1.0 / sqrt( static_cast(width*height) ); (*T) = Mat3::Identity(); (*T)(0,0) = (*T)(1,1) = dNorm; @@ -61,15 +61,7 @@ void PreconditionerFromPoints(int width, int height, Mat3 *T) { void ApplyTransformationToPoints(const Mat &points, const Mat3 &T, Mat *transformed_points) { - const Mat::Index n = points.cols(); - transformed_points->resize(2,n); - for (Mat::Index i = 0; i < n; ++i) { - Vec3 in, out; - in << points(0, i), points(1, i), 1; - out = T * in; - (*transformed_points)(0, i) = out(0)/out(2); - (*transformed_points)(1, i) = out(1)/out(2); - } + (*transformed_points) = (T * points.colwise().homogeneous()).colwise().hnormalized(); } void NormalizePoints(const Mat &points, diff --git a/src/openMVG/multiview/projection.cpp b/src/openMVG/multiview/projection.cpp index 6b0236da5c..2594b6df50 100644 --- a/src/openMVG/multiview/projection.cpp +++ b/src/openMVG/multiview/projection.cpp @@ -131,35 +131,33 @@ void KRt_From_P(const Mat34 &P, Mat3 *Kp, Mat3 *Rp, Vec3 *tp) { Mat3 F_from_P(const Mat34 & P1, const Mat34 & P2) { - Mat3 F12; + Mat3 F12; - typedef Eigen::Matrix Mat24; - Mat24 X1 = P1.block<2, 4>(1, 0); - Mat24 X2; X2 << P1.row(2), P1.row(0); - Mat24 X3 = P1.block<2, 4>(0, 0); - Mat24 Y1 = P2.block<2, 4>(1, 0); - Mat24 Y2; Y2 << P2.row(2), P2.row(0); - Mat24 Y3 = P2.block<2, 4>(0, 0); + typedef Eigen::Matrix Mat24; + Mat24 X1 = P1.block<2, 4>(1, 0); + Mat24 X2; X2 << P1.row(2), P1.row(0); + Mat24 X3 = P1.block<2, 4>(0, 0); + Mat24 Y1 = P2.block<2, 4>(1, 0); + Mat24 Y2; Y2 << P2.row(2), P2.row(0); + Mat24 Y3 = P2.block<2, 4>(0, 0); - Mat4 X1Y1, X2Y1, X3Y1, X1Y2, X2Y2, X3Y2, X1Y3, X2Y3, X3Y3; - X1Y1 << X1, Y1; X2Y1 << X2, Y1; X3Y1 << X3, Y1; - X1Y2 << X1, Y2; X2Y2 << X2, Y2; X3Y2 << X3, Y2; - X1Y3 << X1, Y3; X2Y3 << X2, Y3; X3Y3 << X3, Y3; + Mat4 X1Y1, X2Y1, X3Y1, X1Y2, X2Y2, X3Y2, X1Y3, X2Y3, X3Y3; + X1Y1 << X1, Y1; X2Y1 << X2, Y1; X3Y1 << X3, Y1; + X1Y2 << X1, Y2; X2Y2 << X2, Y2; X3Y2 << X3, Y2; + X1Y3 << X1, Y3; X2Y3 << X2, Y3; X3Y3 << X3, Y3; - F12 << X1Y1.determinant(), X2Y1.determinant(), X3Y1.determinant(), - X1Y2.determinant(), X2Y2.determinant(), X3Y2.determinant(), - X1Y3.determinant(), X2Y3.determinant(), X3Y3.determinant(); + F12 << + X1Y1.determinant(), X2Y1.determinant(), X3Y1.determinant(), + X1Y2.determinant(), X2Y2.determinant(), X3Y2.determinant(), + X1Y3.determinant(), X2Y3.determinant(), X3Y3.determinant(); - return F12; + return F12; } Vec2 Project(const Mat34 &P, const Vec3 &X) { - Vec4 HX; - HX << X, 1.0; - Vec3 hx = P * HX; - return hx.head<2>() / hx(2); + return Vec3(P * X.homogeneous()).hnormalized(); } void Project(const Mat34 &P, const Mat3X &X, Mat2X *x) { @@ -172,8 +170,8 @@ void Project(const Mat34 &P, const Mat3X &X, Mat2X *x) { void Project(const Mat34 &P, const Mat4X &X, Mat2X *x) { x->resize(2, X.cols()); for (Mat4X::Index c = 0; c < X.cols(); ++c) { - Vec3 hx = P * X.col(c); - x->col(c) = hx.head<2>() / hx(2); + const Vec3 hx = P * X.col(c); + x->col(c) = hx.hnormalized(); } } @@ -206,10 +204,6 @@ double Depth(const Mat3 &R, const Vec3 &t, const Vec3 &X) { return (R*X)[2] + t[2]; } -Vec3 EuclideanToHomogeneous(const Vec2 &x) { - return Vec3(x(0), x(1), 1.0); -} - void HomogeneousToEuclidean(const Mat &H, Mat *X) { Mat::Index d = H.rows() - 1; Mat::Index n = H.cols(); diff --git a/src/openMVG/multiview/projection.hpp b/src/openMVG/multiview/projection.hpp index 8c676b9a75..e5910bb7d9 100644 --- a/src/openMVG/multiview/projection.hpp +++ b/src/openMVG/multiview/projection.hpp @@ -128,14 +128,7 @@ void HomogeneousToEuclidean( const Vec4 &H, Vec3 *X ); void EuclideanToHomogeneous( const Mat &X, Mat *H ); /** -* @brief Change euclidean coordinates to homogeneous -* @param x Input 2d point -* @return homogeneous 3d point -*/ -Vec3 EuclideanToHomogeneous( const Vec2 &x ); - -/** -* @brief Change hoogeneous to euclidean +* @brief Change homogeneous to euclidean * @param H Input homogeneous Points * @param[out] Output euclidean points */ diff --git a/src/openMVG/multiview/solver_essential_kernel.hpp b/src/openMVG/multiview/solver_essential_kernel.hpp index f8505aa0ab..4d77d33fca 100644 --- a/src/openMVG/multiview/solver_essential_kernel.hpp +++ b/src/openMVG/multiview/solver_essential_kernel.hpp @@ -72,11 +72,25 @@ class EssentialKernel : public two_view::kernel::Kernel { public: - EssentialKernel(const Mat &x1, const Mat &x2, - const Mat3 &K1, const Mat3 &K2): - two_view::kernel::Kernel(x1,x2), - K1_(K1), K2_(K2) {} - void Fit(const std::vector &samples, std::vector *models) const { + EssentialKernel + ( + const Mat &x1, + const Mat &x2, + const Mat3 &K1, + const Mat3 &K2 + ): + two_view::kernel::Kernel(x1,x2), + K1_(K1), + K2_(K2) + {} + + void Fit + ( + const std::vector &samples, + std::vector *models + ) + const + { const Mat x1 = ExtractColumns(this->x1_, samples); const Mat x2 = ExtractColumns(this->x2_, samples); @@ -86,31 +100,34 @@ class EssentialKernel : assert(x1.cols() == x2.cols()); // Normalize the data (image coords to camera coords). - const Mat3 K1Inverse = K1_.inverse(); - const Mat3 K2Inverse = K2_.inverse(); - Mat x1_normalized, x2_normalized; - ApplyTransformationToPoints(x1, K1Inverse, &x1_normalized); - ApplyTransformationToPoints(x2, K2Inverse, &x2_normalized); + const Mat x1_normalized = (K1_.inverse() * x1.colwise().homogeneous()).colwise().hnormalized(); + const Mat x2_normalized = (K2_.inverse() * x2.colwise().homogeneous()).colwise().hnormalized(); SolverArg::Solve(x1_normalized, x2_normalized, models); } - double Error(size_t sample, const ModelArg &model) const { + double Error + ( + size_t sample, + const ModelArg &model + ) + const + { Mat3 F; FundamentalFromEssential(model, K1_, K2_, &F); return ErrorArg::Error(F, this->x1_.col(sample), this->x2_.col(sample)); } protected: - Mat3 K1_, K2_; // The two camera calibrated camera matrix + Mat3 K1_, K2_; // The two calibrated camera matrices }; //-- Solver kernel for the 8pt Essential Matrix Estimation -typedef essential::kernel::EssentialKernel EightPointKernel; +using EightPointKernel = essential::kernel::EssentialKernel; //-- Solver kernel for the 5pt Essential Matrix Estimation -typedef essential::kernel::EssentialKernel FivePointKernel; +using FivePointKernel = essential::kernel::EssentialKernel; } // namespace kernel diff --git a/src/openMVG/multiview/solver_fundamental_kernel.hpp b/src/openMVG/multiview/solver_fundamental_kernel.hpp index aa313aa06b..35c7194ea9 100644 --- a/src/openMVG/multiview/solver_fundamental_kernel.hpp +++ b/src/openMVG/multiview/solver_fundamental_kernel.hpp @@ -98,38 +98,33 @@ inline void EncodeEpipolarEquation(const TMatX &x1, const TMatX &x2, TMatA *A) { /// Compute SampsonError related to the Fundamental matrix and 2 correspondences struct SampsonError { - static double Error(const Mat3 &F, const Vec2 &x1, const Vec2 &x2) { - Vec3 x(x1(0), x1(1), 1.0); - Vec3 y(x2(0), x2(1), 1.0); + static double Error(const Mat3 &F, const Vec2 &x, const Vec2 &y) { // See page 287 equation (11.9) of HZ. - Vec3 F_x = F * x; - Vec3 Ft_y = F.transpose() * y; - return Square(y.dot(F_x)) / ( F_x.head<2>().squaredNorm() + const Vec3 F_x = F * x.homogeneous(); + const Vec3 Ft_y = F.transpose() * y.homogeneous(); + return Square(y.homogeneous().dot(F_x)) / ( F_x.head<2>().squaredNorm() + Ft_y.head<2>().squaredNorm()); } }; struct SymmetricEpipolarDistanceError { - static double Error(const Mat3 &F, const Vec2 &x1, const Vec2 &x2) { - Vec3 x(x1(0), x1(1), 1.0); - Vec3 y(x2(0), x2(1), 1.0); + static double Error(const Mat3 &F, const Vec2 &x, const Vec2 &y) { // See page 288 equation (11.10) of HZ. - Vec3 F_x = F * x; - Vec3 Ft_y = F.transpose() * y; - return Square(y.dot(F_x)) * ( 1.0 / F_x.head<2>().squaredNorm() - + 1.0 / Ft_y.head<2>().squaredNorm()) + const Vec3 F_x = F * x.homogeneous(); + const Vec3 Ft_y = F.transpose() * y.homogeneous(); + return Square(y.homogeneous().dot(F_x)) * + ( 1.0 / F_x.head<2>().squaredNorm() + + 1.0 / Ft_y.head<2>().squaredNorm()) / 4.0; // The divide by 4 is to make this match the Sampson distance. } }; struct EpipolarDistanceError { - static double Error(const Mat3 &F, const Vec2 &x1, const Vec2 &x2) { + static double Error(const Mat3 &F, const Vec2 &x, const Vec2 &y) { // Transfer error in image 2 // See page 287 equation (11.9) of HZ. - Vec3 x(x1(0), x1(1), 1.0); - Vec3 y(x2(0), x2(1), 1.0); - Vec3 F_x = F * x; - return Square(F_x.dot(y)) / F_x.head<2>().squaredNorm(); + const Vec3 F_x = F * x.homogeneous(); + return Square(F_x.dot(y.homogeneous())) / F_x.head<2>().squaredNorm(); } }; diff --git a/src/openMVG/multiview/solver_homography_kernel.hpp b/src/openMVG/multiview/solver_homography_kernel.hpp index d5b1ea4799..0ae2362737 100644 --- a/src/openMVG/multiview/solver_homography_kernel.hpp +++ b/src/openMVG/multiview/solver_homography_kernel.hpp @@ -57,10 +57,8 @@ struct FourPointSolver { // Should be distributed as Chi-squared with k = 2. struct AsymmetricError { - static double Error(const Mat &H, const Vec2 &x1, const Vec2 &x2) { - Vec3 x2h_est = H * EuclideanToHomogeneous(x1); - Vec2 x2_est = x2h_est.head<2>() / x2h_est[2]; - return (x2 - x2_est).squaredNorm(); + static double Error(const Mat &H, const Vec2 &x, const Vec2 &y) { + return (y - Vec3( H * x.homogeneous()).hnormalized() ).squaredNorm(); } }; diff --git a/src/openMVG/multiview/solver_resection_kernel.hpp b/src/openMVG/multiview/solver_resection_kernel.hpp index 2cf76c6101..475a01760b 100644 --- a/src/openMVG/multiview/solver_resection_kernel.hpp +++ b/src/openMVG/multiview/solver_resection_kernel.hpp @@ -53,8 +53,7 @@ struct SixPointResectionSolver { // Compute the residual of the projection distance(pt2D, Project(P,pt3D)) static double Error(const Mat34 & P, const Vec2 & pt2D, const Vec3 & pt3D){ - Vec2 x = Project(P, pt3D); - return (x-pt2D).norm(); + return (pt2D - Project(P, pt3D)).norm(); } }; @@ -70,9 +69,15 @@ class ResectionKernel : ResectionKernel(const Mat &pt2D, const Mat &pt3D): two_view::kernel::Kernel(pt2D,pt3D){} - void Fit(const std::vector &samples, std::vector *models) const { - Mat pt2d = ExtractColumns(this->x1_, samples); - Mat pt3d = ExtractColumns(this->x2_, samples); + void Fit + ( + const std::vector &samples, + std::vector *models + ) + const + { + const Mat pt2d = ExtractColumns(this->x1_, samples); + const Mat pt3d = ExtractColumns(this->x2_, samples); assert(2 == pt2d.rows()); assert(3 == pt3d.rows()); @@ -83,7 +88,13 @@ class ResectionKernel : } // Error : re-projection error of the sample - double Error(size_t sample, const ModelArg &model) const { + double Error + ( + size_t sample, + const ModelArg &model + ) + const + { return ErrorArg::Error(model, this->x1_.col(sample), this->x2_.col(sample)); } }; @@ -149,7 +160,7 @@ struct EpnpSolver { class ResectionKernel_K { public: - typedef Mat34 Model; + using Model = Mat34; enum { MINIMUM_SAMPLES = 6 }; ResectionKernel_K(const Mat2X &x_camera, const Mat3X &X) : x_camera_(x_camera), X_(X) { @@ -167,8 +178,8 @@ class ResectionKernel_K { } void Fit(const std::vector &samples, std::vector *models) const { - Mat2X x = ExtractColumns(x_camera_, samples); - Mat3X X = ExtractColumns(X_, samples); + const Mat2X x = ExtractColumns(x_camera_, samples); + const Mat3X X = ExtractColumns(X_, samples); Mat34 P; Mat3 R; Vec3 t; @@ -180,8 +191,8 @@ class ResectionKernel_K { } double Error(size_t sample, const Model &model) const { - Mat3X X = X_.col(sample); - Mat2X error = Project(model, X) - x_image_.col(sample); + const Mat3X X = X_.col(sample); + const Mat2X error = Project(model, X) - x_image_.col(sample); return error.col(0).norm(); } diff --git a/src/openMVG/multiview/triangulation_nview.cpp b/src/openMVG/multiview/triangulation_nview.cpp index 33da3fd379..cbf492a4dd 100644 --- a/src/openMVG/multiview/triangulation_nview.cpp +++ b/src/openMVG/multiview/triangulation_nview.cpp @@ -77,8 +77,7 @@ namespace openMVG { { const Mat34& PMat = views[i].first; const Vec2 & xy = views[i].second; - const Vec2 p = Project(PMat, X); - squared_reproj_error += (xy-p).norm(); + squared_reproj_error += (xy - Project(PMat, X)).norm(); } return squared_reproj_error; } @@ -92,7 +91,7 @@ namespace openMVG { // Iterative weighted linear least squares Mat3 AtA; Vec3 Atb, X; - std::vector weights(nviews,double(1.0)); + Vec weights = Vec::Constant(nviews,1.0); for (int it=0;it::max(); zmax = - std::numeric_limits::max(); - err = 0; + err = 0.0; for (int i=0;i() / z; - if (zzmax) zmax = z; - err += (p-x).norm(); + if (z < zmin) zmin = z; + else if (z > zmax) zmax = z; + err += (p - xProj.hnormalized()).norm(); // residual error weights[i] = 1.0 / z; } } @@ -147,4 +145,3 @@ namespace openMVG { } // namespace openMVG - diff --git a/src/openMVG/multiview/triangulation_nview.hpp b/src/openMVG/multiview/triangulation_nview.hpp index 1339fc9d30..423f8c7be4 100644 --- a/src/openMVG/multiview/triangulation_nview.hpp +++ b/src/openMVG/multiview/triangulation_nview.hpp @@ -50,39 +50,39 @@ namespace openMVG { //Iterated linear method class Triangulation - { - public: + { + public: - size_t size() const { return views.size();} + size_t size() const { return views.size();} - void clear() { views.clear();} + void clear() { views.clear();} - void add(const Mat34& projMatrix, const Vec2 & p) { - views.push_back( std::pair(projMatrix,p) ); - } + void add(const Mat34& projMatrix, const Vec2 & p) { + views.emplace_back( projMatrix, p ); + } // Return squared L2 sum of error - double error(const Vec3 &X) const; + double error(const Vec3 &X) const; - Vec3 compute(int iter = 3) const; + Vec3 compute(int iter = 3) const; - ///////////////////////////////////////////////////////////////////////////////////////////////////////////////// - // Accessors + ///////////////////////////////////////////////////////////////////////////////////////////////////////////////// + // Accessors - // These values are defined after a successful call to compute - double minDepth() const { return zmin; } - double maxDepth() const { return zmax; } - double error() const { return err; } + // These values are defined after a successful call to compute + double minDepth() const { return zmin; } + double maxDepth() const { return zmax; } + double error() const { return err; } - ///////////////////////////////////////////////////////////////////////////////////////////////////////////////// - // Data members + ///////////////////////////////////////////////////////////////////////////////////////////////////////////////// + // Data members - protected: - mutable double zmin; // min depth, mutable since modified in compute(...) const; - mutable double zmax; // max depth, mutable since modified in compute(...) const; - mutable double err; // re-projection error, mutable since modified in compute(...) const; - std::vector< std::pair > views; // Proj matrix and associated image point - }; + protected: + mutable double zmin; // min depth, mutable since modified in compute(...) const; + mutable double zmax; // max depth, mutable since modified in compute(...) const; + mutable double err; // re-projection error, mutable since modified in compute(...) const; + std::vector< std::pair > views; // Proj matrix and associated image point + }; } // namespace openMVG diff --git a/src/openMVG/multiview/triangulation_nview_test.cpp b/src/openMVG/multiview/triangulation_nview_test.cpp index 717a16cec8..d2db95d94d 100644 --- a/src/openMVG/multiview/triangulation_nview_test.cpp +++ b/src/openMVG/multiview/triangulation_nview_test.cpp @@ -54,9 +54,8 @@ TEST(Triangulate_NView, FiveViews) { // Check reprojection error. Should be nearly zero. for (int j = 0; j < nviews; ++j) { - Vec3 x_reprojected = Ps[j]*X; - x_reprojected /= x_reprojected(2); - const double error = (x_reprojected.head(2) - xs.col(j)).norm(); + const Vec3 x_reprojected = Ps[j]*X; + const double error = (x_reprojected.hnormalized() - xs.col(j)).norm(); EXPECT_NEAR(error, 0.0, 1e-9); } } @@ -84,9 +83,8 @@ TEST(Triangulate_NViewAlgebraic, FiveViews) { // Check reprojection error. Should be nearly zero. for (int j = 0; j < nviews; ++j) { - Vec3 x_reprojected = Ps[j]*X; - x_reprojected /= x_reprojected(2); - const double error = (x_reprojected.head<2>() - xs.col(j)).norm(); + const Vec3 x_reprojected = Ps[j]*X; + const double error = (x_reprojected.hnormalized() - xs.col(j)).norm(); EXPECT_NEAR(error, 0.0, 1e-9); } } @@ -107,9 +105,8 @@ TEST(Triangulate_NViewIterative, FiveViews) { // Check reprojection error. Should be nearly zero. EXPECT_NEAR(triangulationObj.error(X), 0.0, 1e-9); for (int j = 0; j < nviews; ++j) { - Vec3 x_reprojected = d.P(j) * Vec4(X(0), X(1), X(2), 1.0); - x_reprojected /= x_reprojected(2); - const double error = (x_reprojected.head<2>() - d._x[j].col(i)).norm(); + const Vec3 x_reprojected = d.P(j) * X.homogeneous(); + const double error = (x_reprojected.hnormalized() - d._x[j].col(i)).norm(); EXPECT_NEAR(error, 0.0, 1e-9); } } diff --git a/src/openMVG/multiview/two_view_kernel.hpp b/src/openMVG/multiview/two_view_kernel.hpp index d3cd611289..bb8196c38b 100644 --- a/src/openMVG/multiview/two_view_kernel.hpp +++ b/src/openMVG/multiview/two_view_kernel.hpp @@ -69,9 +69,9 @@ template &samples, vector *models) const { - Mat x1 = ExtractColumns(x1_, samples), - x2 = ExtractColumns(x2_, samples); + const Mat + x1 = ExtractColumns(x1_, samples), + x2 = ExtractColumns(x2_, samples); Solver::Solve(x1, x2, models); } /// Return the error associated to the model and sample^nth point diff --git a/src/openMVG/numeric/numeric.cpp b/src/openMVG/numeric/numeric.cpp index 17e61328a1..f36b1d9d1a 100644 --- a/src/openMVG/numeric/numeric.cpp +++ b/src/openMVG/numeric/numeric.cpp @@ -54,8 +54,8 @@ Mat3 RotationAroundZ(double angle) { double getRotationMagnitude(const Mat3 & R2) { const Mat3 R1 = Mat3::Identity(); - double cos_theta = (R1.array() * R2.array()).sum() / 3.0; - cos_theta = clamp(cos_theta, -1.0, 1.0); + double cos_theta = (R1.array() * R2.array()).sum() / 3.0; + cos_theta = clamp(cos_theta, -1.0, 1.0); return std::acos(cos_theta); } @@ -116,21 +116,19 @@ Mat3 LookAt2(const Vec3 &eyePosition3D, void MeanAndVarianceAlongRows(const Mat &A, Vec *mean_pointer, Vec *variance_pointer) { - Vec &mean = *mean_pointer; - Vec &variance = *variance_pointer; - Mat::Index n = A.rows(); - double m = static_cast(A.cols()); - mean = variance = Vec::Zero(n); - - for (Mat::Index i = 0; i < n; ++i) { - mean(i) += A.row(i).array().sum(); - variance(i) += (A.row(i).array() * A.row(i).array()).array().sum(); - } - - mean /= m; - for (Mat::Index i = 0; i < n; ++i) { - variance(i) = variance(i) / m - Square(mean(i)); - } + const Mat::Index n = A.rows(); + const double m = static_cast(A.cols()); + (*mean_pointer) = Vec::Zero(n); + (*variance_pointer) = Vec::Zero(n); + + for (Mat::Index i = 0; i < n; ++i) { + (*mean_pointer)(i) += A.row(i).array().sum(); + (*variance_pointer)(i) += (A.row(i).array() * A.row(i).array()).array().sum(); + } + (*mean_pointer) /= m; + for (Mat::Index i = 0; i < n; ++i) { + (*variance_pointer)(i) = (*variance_pointer)(i) / m - Square((*mean_pointer)(i)); + } } bool exportMatToTextFile(const Mat & mat, const std::string & filename, diff --git a/src/software/opencv/accv12_demo/main.cpp b/src/software/opencv/accv12_demo/main.cpp index 1eccf8c37f..e154d75991 100644 --- a/src/software/opencv/accv12_demo/main.cpp +++ b/src/software/opencv/accv12_demo/main.cpp @@ -243,14 +243,14 @@ int main(int, char**) cb_RANSAC_inlierPourcent.push_back(vec_inliersRansac.size()/float(vec_matches.size())); //Draw warp Vec2 x0(0,0), x1(imgA.cols, 0), x2(imgA.cols, imgA.rows), x3(0, imgA.rows); - Vec3 x00 = Hransac * Vec3(x0(0), x0(1), 1); - Vec3 x11 = Hransac * Vec3(x1(0), x1(1), 1); - Vec3 x22 = Hransac * Vec3(x2(0), x2(1), 1); - Vec3 x33 = Hransac * Vec3(x3(0), x3(1), 1); - x0 = x00.head<2>() / x00(2); - x1 = x11.head<2>() / x11(2); - x2 = x22.head<2>() / x22(2); - x3 = x33.head<2>() / x33(2); + Vec3 x00 = Hransac * x0.homogeneous(); + Vec3 x11 = Hransac * x1.homogeneous(); + Vec3 x22 = Hransac * x2.homogeneous(); + Vec3 x33 = Hransac * x3.homogeneous(); + x0 = x00.hnormalized(); + x1 = x11.hnormalized(); + x2 = x22.hnormalized(); + x3 = x33.hnormalized(); line(sidebyside, Point(x0(0)+imgA.cols, x0(1)), Point(x1(0)+imgA.cols, x1(1)), RColor, Lthickness); line(sidebyside, Point(x1(0)+imgA.cols, x1(1)), Point(x2(0)+imgA.cols, x2(1)), RColor, Lthickness);