Use Eigen hnormalized() and homogeneous() function openMVG#709

src/openMVG/cameras/Camera_Intrinsics.hpp

@@ -88,11 +88,11 @@ struct IntrinsicBase : public Clonable<IntrinsicBase>
     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
-

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();
   }
 };
 

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<double>(width*height) );
+  const double dNorm = 1.0 / sqrt( static_cast<double>(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,

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<double, 2, 4> 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<double, 2, 4> 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();

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
 */

src/openMVG/multiview/solver_essential_kernel.hpp

@@ -72,11 +72,25 @@ class EssentialKernel :
    public two_view::kernel::Kernel<SolverArg,ErrorArg, ModelArg>
 {
 public:
-  EssentialKernel(const Mat &x1, const Mat &x2,
-                  const Mat3 &K1, const Mat3 &K2):
-  two_view::kernel::Kernel<SolverArg,ErrorArg, ModelArg>(x1,x2),
-                                                         K1_(K1), K2_(K2) {}
-  void Fit(const std::vector<size_t> &samples, std::vector<ModelArg> *models) const {
+  EssentialKernel
+  (
+    const Mat &x1,
+    const Mat &x2,
+    const Mat3 &K1,
+    const Mat3 &K2
+   ):
+    two_view::kernel::Kernel<SolverArg,ErrorArg, ModelArg>(x1,x2),
+    K1_(K1),
+    K2_(K2)
+  {}
+
+  void Fit
+  (
+    const std::vector<size_t> &samples,
+    std::vector<ModelArg> *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<EightPointRelativePoseSolver,
-  fundamental::kernel::SampsonError, Mat3>  EightPointKernel;
+using EightPointKernel = essential::kernel::EssentialKernel<EightPointRelativePoseSolver,
+  fundamental::kernel::SampsonError, Mat3>;
 
 
 //-- Solver kernel for the 5pt Essential Matrix Estimation
-typedef essential::kernel::EssentialKernel<FivePointSolver,
-  fundamental::kernel::SampsonError, Mat3>  FivePointKernel;
+using FivePointKernel = essential::kernel::EssentialKernel<FivePointSolver,
+  fundamental::kernel::SampsonError, Mat3>;
 
 
 }  // namespace kernel

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();
   }
 };
 

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();
   }
 };
 

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<SolverArg,ErrorArg, ModelArg>(pt2D,pt3D){}
 
-  void Fit(const std::vector<size_t> &samples, std::vector<ModelArg> *models) const {
-    Mat pt2d = ExtractColumns(this->x1_, samples);
-    Mat pt3d = ExtractColumns(this->x2_, samples);
+  void Fit
+  (
+    const std::vector<size_t> &samples,
+    std::vector<ModelArg> *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<size_t> &samples, std::vector<Model> *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();
   }