Merge pull request #1718 from alicevision/dev/sfmExpanding

Add SfM Expanding application

src/aliceVision/camera/Equidistant.cpp

@@ -481,5 +481,12 @@ double Equidistant::getHorizontalFov() const
 
 double Equidistant::getVerticalFov() const { return getHorizontalFov(); }
 
+double Equidistant::pixelProbability() const
+{
+    return 1.0 / double(w());
+}
+
+
+
 }  // namespace camera
 }  // namespace aliceVision

src/aliceVision/camera/Equidistant.hpp

@@ -143,6 +143,13 @@ class Equidistant : public IntrinsicScaleOffsetDisto
      */
     double getVerticalFov() const override;
 
+
+    /**
+     * @brief how a one pixel change relates to an angular change
+     * @return a value in radians
+    */
+    double pixelProbability() const override;
+
   protected:
     double _circleRadius{0.0};
     Vec2 _circleCenter{0.0, 0.0};

src/aliceVision/camera/IntrinsicBase.hpp

@@ -363,6 +363,12 @@ class IntrinsicBase
      */
     virtual double imagePlaneToCameraPlaneError(double value) const = 0;
 
+    /**
+     * @brief What is the probability of a pixel wrt the whole fov
+     * @return a value in radians
+    */
+    virtual double pixelProbability() const = 0;
+
     /**
      * @brief Return true if the intrinsic is valid
      * @return True if the intrinsic is valid

src/aliceVision/camera/Pinhole.cpp

@@ -327,5 +327,12 @@ double Pinhole::getVerticalFov() const
     return 2.0 * atan2(sensorHeight() / 2.0, focalLengthMM);
 }
 
+double Pinhole::pixelProbability() const
+{
+    const double focalLengthMM = sensorHeight() * getScale().y() / double(h());
+    const double pixToMm = 1.0 / _scale(0);
+    return std::atan2(pixToMm, focalLengthMM) / getVerticalFov();
+}
+
 }  // namespace camera
 }  // namespace aliceVision

src/aliceVision/camera/Pinhole.hpp

@@ -114,6 +114,12 @@ class Pinhole : public IntrinsicScaleOffsetDisto
      * @return Vertical FOV in radians
      */
     double getVerticalFov() const override;
+
+    /**
+     * @brief how a one pixel change relates to an angular change
+     * @return a value in radians
+    */
+    double pixelProbability() const override;
 };
 
 }  // namespace camera

src/aliceVision/multiview/CMakeLists.txt

@@ -20,6 +20,8 @@ set(multiview_files_headers
   relativePose/HomographyKernel.hpp
   relativePose/Rotation3PSolver.hpp
   relativePose/ISolverErrorRelativePose.hpp
+  relativePose/RelativeSphericalKernel.hpp
+  relativePose/RotationSphericalKernel.hpp
   resection/EPnPSolver.hpp
   resection/EPnPKernel.hpp
   resection/P3PSolver.hpp
@@ -76,6 +78,7 @@ alicevision_add_library(aliceVision_multiview
   PUBLIC_LINKS
     aliceVision_numeric
     aliceVision_robustEstimation
+    aliceVision_camera
     ${CERES_LIBRARIES}
   PRIVATE_LINKS
     aliceVision_system

src/aliceVision/multiview/essential.cpp

@@ -41,7 +41,6 @@ void motionFromEssential(const Mat3& E, std::vector<Mat3>* Rs, std::vector<Vec3>
 {
     Eigen::JacobiSVD<Mat3> USV(E, Eigen::ComputeFullU | Eigen::ComputeFullV);
     Mat3 U = USV.matrixU();
-    // Vec3 d =  USV.singularValues();
     Mat3 Vt = USV.matrixV().transpose();
 
     // Last column of U is undetermined since d = (a a 0).
@@ -73,6 +72,48 @@ void motionFromEssential(const Mat3& E, std::vector<Mat3>* Rs, std::vector<Vec3>
     (*ts)[3] = -U.col(2);
 }
 
+// HZ 9.7 page 259 (Result 9.19)
+void motionFromEssential(const Mat3& E, std::vector<Mat4> & Ts)
+{
+    Eigen::JacobiSVD<Mat3> USV(E, Eigen::ComputeFullU | Eigen::ComputeFullV);
+    Mat3 U = USV.matrixU();
+    Mat3 Vt = USV.matrixV().transpose();
+
+    // Last column of U is undetermined since d = (a a 0).
+    if (U.determinant() < 0)
+    {
+        U.col(2) *= -1;
+    }
+    // Last row of Vt is undetermined since d = (a a 0).
+    if (Vt.determinant() < 0)
+    {
+        Vt.row(2) *= -1;
+    }
+
+    Mat3 W;
+    W << 0, -1, 0, 1, 0, 0, 0, 0, 1;
+
+    Mat3 U_W_Vt = U * W * Vt;
+    Mat3 U_Wt_Vt = U * W.transpose() * Vt;
+
+    Ts.resize(4);
+    Ts[0].setIdentity();
+    Ts[0].block<3, 3>(0, 0) = U_W_Vt;
+    Ts[0].block<3, 1>(0, 3) = U.col(2);
+
+    Ts[1].setIdentity();
+    Ts[1].block<3, 3>(0, 0) = U_W_Vt;
+    Ts[1].block<3, 1>(0, 3) = -U.col(2);
+
+    Ts[2].setIdentity();
+    Ts[2].block<3, 3>(0, 0) = U_Wt_Vt;
+    Ts[2].block<3, 1>(0, 3) = U.col(2);
+
+    Ts[3].setIdentity();
+    Ts[3].block<3, 3>(0, 0) = U_Wt_Vt;
+    Ts[3].block<3, 1>(0, 3) = -U.col(2);
+}
+
 // HZ 9.6 pag 259 (9.6.3 Geometrical interpretation of the 4 solutions)
 int motionFromEssentialChooseSolution(const std::vector<Mat3>& Rs,
                                       const std::vector<Vec3>& ts,
@@ -124,4 +165,76 @@ bool motionFromEssentialAndCorrespondence(const Mat3& E, const Mat3& K1, const V
     }
 }
 
+bool estimateTransformStructureFromEssential(Mat4 & T,
+                                std::vector<Vec3>& structure,
+                                std::vector<size_t>& newVecInliers,
+                                const Mat3& E,
+                                const std::vector<size_t>& vecInliers,
+                                const camera::IntrinsicBase & cam1,
+                                const camera::IntrinsicBase & cam2,
+                                const std::vector<Vec2> & x1,
+                                const std::vector<Vec2> & x2)
+{
+    // Find set of analytical solutions
+    std::vector<Mat4> Ts;
+    motionFromEssential(E, Ts);
+
+    // Check each possible solution and keep the best one
+    size_t bestCoundValid = 0;
+    for (int it = 0; it < Ts.size(); it++)
+    {
+        const Mat4 T1 = Eigen::Matrix4d::Identity();
+        const Mat4 & T2 = Ts[it];
+
+        std::vector<Vec3> points;
+        std::vector<size_t> updatedInliers;
+
+        // Triangulate each point
+        size_t countValid = 0;
+        for (size_t k = 0; k < vecInliers.size(); ++k)
+        {
+            const Vec2& pt1 = x1[vecInliers[k]];
+            const Vec2& pt2 = x2[vecInliers[k]];
+
+            const Vec3 pt3d1 = cam1.toUnitSphere(cam1.removeDistortion(cam1.ima2cam(pt1)));
+            const Vec3 pt3d2 = cam2.toUnitSphere(cam2.removeDistortion(cam2.ima2cam(pt2)));
+
+            Vec3 X;
+            multiview::TriangulateSphericalDLT(T1, pt3d1, T2, pt3d2, X);
+
+            Vec2 ptValid1 = cam1.project(T1, X.homogeneous(), true);
+            Vec2 ptValid2 = cam2.project(T2, X.homogeneous(), true);
+
+            Eigen::Vector3d dirX1 = (T1 * X.homogeneous()).head(3).normalized();
+            Eigen::Vector3d dirX2 = (T2 * X.homogeneous()).head(3).normalized();
+
+            // Check that the points are logically 
+            if (!(dirX1.dot(pt3d1) > 0.0 && dirX2.dot(pt3d2)  > 0.0))
+            {
+                continue;
+            }
+
+            updatedInliers.push_back(vecInliers[k]);
+            points.push_back(X);
+            countValid++;
+        }
+
+        if (countValid > bestCoundValid)
+        {
+            bestCoundValid = countValid;
+            structure = points;
+            newVecInliers = updatedInliers;
+            T = Ts[it];
+        }
+    }
+
+    // Check result validity
+    if (newVecInliers.size() < 10)
+    {
+        return false;
+    }
+
+    return true;
+}
+
 }  // namespace aliceVision

src/aliceVision/multiview/essential.hpp

@@ -9,7 +9,7 @@
 #pragma once
 
 #include <aliceVision/numeric/numeric.hpp>
-
+#include <aliceVision/camera/IntrinsicBase.hpp>
 #include <vector>
 
 namespace aliceVision {
@@ -57,4 +57,34 @@ int motionFromEssentialChooseSolution(const std::vector<Mat3>& Rs,
  */
 void motionFromEssential(const Mat3& E, std::vector<Mat3>* Rs, std::vector<Vec3>* ts);
 
+/**
+ * @brief HZ 9.7 page 259 (Result 9.19)
+ */
+void motionFromEssential(const Mat3& E, std::vector<Mat4> & Ts);
+
+/**
+ * @brief Estimate the relative transformation of the camera and
+ * the associated 3d structure of the observed points using an 
+ * essential matrix as input prior
+ * @param T the output estimated pose
+ * @param structure the output estimated structure
+ * @param newVecInliers the updated inliers list (set of indices in the coordinates vectors)
+ * @param E the input Essential matrix prior
+ * @param vecInliers the input list of inliers (set of indices in the coordinates vectors)
+ * @param cam1 the first camera intrinsic object
+ * @param cam2 the second camera intrinsic object
+ * @param x1 the observed points coordinates in the first camera
+ * @param x2 the observed points coordinates in the second camera
+ * @return true if estimation succeeded
+*/
+bool estimateTransformStructureFromEssential(Mat4 & T,
+                                std::vector<Vec3>& structure,
+                                std::vector<size_t>& newVecInliers,
+                                const Mat3& E,
+                                const std::vector<size_t>& vecInliers,
+                                const camera::IntrinsicBase & cam1,
+                                const camera::IntrinsicBase & cam2,
+                                const std::vector<Vec2> & x1,
+                                const std::vector<Vec2> & x2);
+
 }  // namespace aliceVision

src/aliceVision/multiview/relativePose/Essential5PSolver.cpp

@@ -71,6 +71,18 @@ Mat fivePointsNullspaceBasis(const Mat2X& x1, const Mat2X& x2)
     return svd.matrixV().topRightCorner<9, 4>();
 }
 
+/**
+ * @brief Compute the nullspace of the linear constraints given by the matches.
+ */
+Mat fivePointsNullspaceBasisSpherical(const Mat& x1, const Mat& x2)
+{
+    Eigen::Matrix<double, 9, 9> A;
+    A.setZero();  // make A square until Eigen supports rectangular SVD.
+    encodeEpipolarSphericalEquation(x1, x2, &A);
+    Eigen::JacobiSVD<Eigen::Matrix<double, 9, 9>> svd(A, Eigen::ComputeFullV);
+    return svd.matrixV().topRightCorner<9, 4>();
+}
+
 /**
  * @brief Builds the polynomial constraint matrix M.
  */
@@ -139,13 +151,13 @@ Mat fivePointsPolynomialConstraints(const Mat& EBasis)
 
 void Essential5PSolver::solve(const Mat& x1, const Mat& x2, std::vector<robustEstimation::Mat3Model>& models) const
 {
-    assert(2 == x1.rows());
+    assert(2 == x1.rows() || 3 == x1.rows());
     assert(5 <= x1.cols());
     assert(x1.rows() == x2.rows());
     assert(x1.cols() == x2.cols());
 
     // step 1: Nullspace Extraction.
-    const Eigen::Matrix<double, 9, 4> EBasis = fivePointsNullspaceBasis(x1, x2);
+    const Eigen::Matrix<double, 9, 4> EBasis = (x1.rows()==2)?fivePointsNullspaceBasis(x1, x2):fivePointsNullspaceBasisSpherical(x1, x2);
 
     // step 2: Constraint Expansion.
     const Eigen::Matrix<double, 10, 20> EConstraints = fivePointsPolynomialConstraints(EBasis);