Merge pull request #1990 from alicevision/dev/fixBootstrap

Fix sfm bootstrap

Author: servantftransperfect

.gitignore

@@ -77,5 +77,7 @@ Production
 **/.vscode
 **/.vs
 
-vcpkg_installed/
-export/
+/vcpkg_installed/
+/export/
+/.cache/
+/compile_commands.json

meshroom/aliceVision/RelativePoseEstimating.py

@@ -1,4 +1,4 @@
-__version__ = "3.0"
+__version__ = "3.1"
 
 from meshroom.core import desc
 from meshroom.core.utils import DESCRIBER_TYPES, VERBOSE_LEVEL
@@ -48,6 +48,14 @@ class RelativePoseEstimating(desc.AVCommandLineNode):
             range=(1, 1000, 1),
             advanced=True,
         ),
+        desc.FloatParam(
+            name="distanceThreshold",
+            label="Distance Threshold",
+            description="Threshold on geometric distance (epipolar distance or reprojection distance for pure rotation)",
+            value=4.0,
+            range=(0.0, 50.0, 1.0),
+            advanced=True,
+        ),
         desc.File(
             name="imagePairsList",
             label="Image Pairs",

src/aliceVision/multiview/triangulation/triangulationDLT.cpp

@@ -34,22 +34,63 @@ void TriangulateDLT(const Mat34& P1, const Vec2& x1, const Mat34& P2, const Vec2
     homogeneousToEuclidean(X_homogeneous, X_euclidean);
 }
 
-// Solve:
-// [cross(x0,P0) X = 0]
-// [cross(x1,P1) X = 0]
+/**
+* let v = R.transpose() * x; (to get a world coordinates direction)
+* then lambda * v = X - center
+* (v) cross (X - center) = 0
+* [v]x*X - [V]x*center) = 0
+*/
 void TriangulateSphericalDLT(const Mat4& T1, const Vec3& x1, const Mat4& T2, const Vec3& x2, Vec4& X_homogeneous)
 {
-    Mat design(6, 4);
-    for (int i = 0; i < 4; ++i)
-    {
-        design(0, i) = -x1[2] * T1(1, i) + x1[1] * T1(2, i);
-        design(1, i) = x1[2] * T1(0, i) - x1[0] * T1(2, i);
-        design(2, i) = -x1[1] * T1(0, i) + x1[0] * T1(1, i);
+    const Mat3 R1 = T1.block<3, 3>(0, 0);
+    const Mat3 R2 = T2.block<3, 3>(0, 0);
+    const Vec3 t1 = T1.block<3, 1>(0, 3);
+    const Vec3 t2 = T2.block<3, 1>(0, 3);
+    const Vec3 c1 = -R1.transpose() * t1;
+    const Vec3 c2 = -R2.transpose() * t2;
 
-        design(3, i) = -x2[2] * T2(1, i) + x2[1] * T2(2, i);
-        design(4, i) = x2[2] * T2(0, i) - x2[0] * T2(2, i);
-        design(5, i) = -x2[1] * T2(0, i) + x2[0] * T2(1, i);
-    }
+    const Vec3 v1 = R1.transpose() * x1;
+    const Vec3 v2 = R2.transpose() * x2;
+
+    Mat4 design;
+    
+    //We keep last 2 rows of the equation for each point
+    // [ 0  -z   y]
+    // [ z   0  -x]
+    // [-y   x   0]
+    // --> 
+    // [ z   0  -x]
+    // [-y   x   0]
+
+    // [ z   0  -x] * [cx cy cz]^t = z * cx - x * cz
+    // [-y   x   0] * [cx cy cz]^t = -y * cx + x * cy
+
+    //[v]x
+    design(0, 0) = v1.z();
+    design(0, 1) = 0;
+    design(0, 2) = -v1.x();    
+    design(1, 0) = - v1.y();
+    design(1, 1) = v1.x();
+    design(1, 2) = 0;
+
+    //-[V]x*center
+    design(0, 3) = - (v1.z() * c1.x() - v1.x() * c1.z());
+    design(1, 3) = - (-v1.y() * c1.x() + v1.x() * c1.y());
+
+    //[v]x
+    design(2, 0) = v2.z();
+    design(2, 1) = 0;
+    design(2, 2) = -v2.x();    
+    design(3, 0) = - v2.y();
+    design(3, 1) = v2.x();
+    design(3, 2) = 0;
+
+    //-[V]x*center
+    design(2, 3) = - (v2.z() * c2.x() - v2.x() * c2.z());
+    design(3, 3) = - (-v2.y() * c2.x() + v2.x() * c2.y());
+
+    // Solve AX=0, where X is the homogeneous coordinates vector of the 3d point
+    // in the reference frame
     Nullspace(design, X_homogeneous);
 }
 

src/aliceVision/multiview/triangulation/triangulationDLT.hpp

@@ -50,9 +50,9 @@ void TriangulateSphericalDLT(const Mat4& T1, const Vec3& x1, const Mat4& T2, con
  * Triangulate a point given a set of bearing vectors
  * and associated projection matrices (in meters)
  * @param T1 a projection matrix (R | t)
- * @param x1 a unit bearing vector
+ * @param x1 a unit bearing vector (on the unit sphere)
  * @param T2 a projection matrix K (R | t)
- * @param x2 a unit bearing vector
+ * @param x2 a unit bearing vector (on the unit sphere)
  * @param X_homogeneous a 3d point
  */
 void TriangulateSphericalDLT(const Mat4& T1, const Vec3& x1, const Mat4& T2, const Vec3& x2, Vec3& X_euclidean);

src/aliceVision/multiview/triangulation/triangulationDLT_test.cpp

@@ -9,6 +9,7 @@
 #include <aliceVision/numeric/projection.hpp>
 #include <aliceVision/multiview/NViewDataSet.hpp>
 #include <aliceVision/multiview/triangulation/triangulationDLT.hpp>
+#include <aliceVision/geometry/lie.hpp>
 
 #define BOOST_TEST_MODULE triangulationDLT
 
@@ -32,3 +33,45 @@ BOOST_AUTO_TEST_CASE(Triangulation_TriangulateDLT)
         BOOST_CHECK_SMALL(DistanceLInfinity(X_estimated, X_gt), 1e-8);
     }
 }
+
+BOOST_AUTO_TEST_CASE(Triangulation_TriangulateSphericalDLT_base)
+{
+    Vec3 X_gt;
+
+    X_gt << 2, -3, 4;
+
+    Mat4 T1 = Mat4::Identity();
+    Mat4 T2 = Mat4::Identity();
+
+    T1.block<3, 3>(0, 0) = SO3::expm({0.1, 0.2, -0.5});
+    T2.block<3, 3>(0, 0) = SO3::expm({0.01, -0.3, -0.2});
+    T1.block<3, 1>(0, 3) << 0.5, 0.6, -0.2;
+    T2.block<3, 1>(0, 3) << -0.1, 1.6, 2.2;
+
+    Vec3 x1 = (T1 * X_gt.homogeneous()).head(3).normalized();
+    Vec3 x2 = (T2 * X_gt.homogeneous()).head(3).normalized();
+
+    Vec3 X_estimated;
+    multiview::TriangulateSphericalDLT(T1, x1, T2, x2, X_estimated);
+
+    BOOST_CHECK_SMALL(DistanceLInfinity(X_estimated, X_gt), 1e-8);
+}
+
+BOOST_AUTO_TEST_CASE(Triangulation_TriangulateSphericalDLT_Zero)
+{
+    Vec3 X_gt;
+
+    X_gt << 2, -3, 4;
+
+    Mat4 T1 = Mat4::Identity();
+    Mat4 T2 = Mat4::Identity();
+
+    Vec3 x1 = (T1 * X_gt.homogeneous()).head(3).normalized();
+    Vec3 x2 = (T2 * X_gt.homogeneous()).head(3).normalized();
+
+    Vec3 X_estimated;
+    multiview::TriangulateSphericalDLT(T1, x1, T2, x2, X_estimated);
+    
+    //Result should be null vector
+    BOOST_CHECK(X_estimated.norm() < 1e-3);
+}

src/aliceVision/sfm/pipeline/bootstrapping/EstimateAngle.cpp

@@ -16,6 +16,7 @@ bool estimatePairAngle(const sfmData::SfMData & sfmData,
                        const IndexT referenceViewId,
                        const IndexT otherViewId,
                        const geometry::Pose3 & otherTreference,
+                       const double & errorMax,
                        const track::TracksMap& tracksMap, 
                        const track::TracksPerView & tracksPerView, 
                        double & resultAngle, 
@@ -33,7 +34,9 @@ bool estimatePairAngle(const sfmData::SfMData & sfmData,
     track::getCommonTracksInImagesFast({referenceViewId, otherViewId}, tracksMap, tracksPerView, mapTracksCommon);
 
     const Mat4 T1 = Eigen::Matrix4d::Identity();
-    const Mat4 T2 = otherTreference.getHomogeneous();
+    Mat4 T2 = otherTreference.getHomogeneous();
+    const Mat3 R2 = otherTreference.rotation();
+    const Mat3 E = CrossProductMatrix(otherTreference.translation()) * R2;
 
     const Eigen::Vector3d c = otherTreference.center();
 
@@ -52,6 +55,13 @@ bool estimatePairAngle(const sfmData::SfMData & sfmData,
         const Vec3 pt3d1 = refIntrinsics->toUnitSphere(refIntrinsics->removeDistortion(refIntrinsics->ima2cam(refpt)));
         const Vec3 pt3d2 = nextIntrinsics->toUnitSphere(nextIntrinsics->removeDistortion(nextIntrinsics->ima2cam(nextpt)));
 
+        const Vec3 x = (E * pt3d1).normalized();
+        double error = std::abs(std::asin(pt3d2.dot(x)));
+        if (error > errorMax)
+        {
+            continue;
+        }
+
 
         Vec3 X;
         multiview::TriangulateSphericalDLT(T1, pt3d1, T2, pt3d2, X);

src/aliceVision/sfm/pipeline/bootstrapping/EstimateAngle.hpp

@@ -18,6 +18,7 @@ namespace sfm {
  * @param referenceViewId the reference view id
  * @param otherViewId the other view id
  * @param otherTreference the relative pose
+ * @param errorMax the maximal tolerated error
  * @param tracksMap the input map of tracks
  * @param tracksPerView tracks grouped by views
  * @param resultAngle the output median angle
@@ -28,6 +29,7 @@ bool estimatePairAngle(const sfmData::SfMData & sfmData,
                        const IndexT referenceViewId,
                        const IndexT otherViewId,
                        const geometry::Pose3 & otherTreference,
+                       const double & errorMax,
                        const track::TracksMap& tracksMap, 
                        const track::TracksPerView & tracksPerView, 
                        double & resultAngle, 

src/aliceVision/sfm/pipeline/bootstrapping/PairsScoring.cpp

@@ -70,7 +70,7 @@ IndexT findBestPair(const sfmData::SfMData & sfmData,
 
         double angle = 0.0;
         std::vector<size_t> usedTracks;
-        if (!sfm::estimatePairAngle(sfmData, pair.reference, pair.next, pair.pose, tracksMap, tracksPerView, angle, usedTracks))
+        if (!sfm::estimatePairAngle(sfmData, pair.reference, pair.next, pair.pose, pair.errorMax, tracksMap, tracksPerView, angle, usedTracks))
         {
             continue;
         }

src/aliceVision/sfm/pipeline/relativePoses.hpp

@@ -20,6 +20,7 @@ struct ReconstructedPair
     IndexT next;
     geometry::Pose3 pose;
     double score;
+    double errorMax;
 };
 
 void tag_invoke(const boost::json::value_from_tag&, boost::json::value& jv, sfm::ReconstructedPair const& input)
@@ -28,7 +29,9 @@ void tag_invoke(const boost::json::value_from_tag&, boost::json::value& jv, sfm:
           {"next", input.next},
           {"R", boost::json::value_from(SO3::logm(input.pose.rotation()))},
           {"t", boost::json::value_from(input.pose.translation())},
-          {"score", boost::json::value_from(input.score)}};
+          {"score", boost::json::value_from(input.score)},
+          {"errorMax", boost::json::value_from(input.errorMax)}
+          };
 }
 
 ReconstructedPair tag_invoke(boost::json::value_to_tag<ReconstructedPair>, boost::json::value const& jv)
@@ -42,6 +45,7 @@ ReconstructedPair tag_invoke(boost::json::value_to_tag<ReconstructedPair>, boost
     ret.pose.setRotation(SO3::expm(boost::json::value_to<Vec3>(obj.at("R"))));
     ret.pose.setTranslation(boost::json::value_to<Vec3>(obj.at("t")));
     ret.score = boost::json::value_to<double>(obj.at("score"));
+    ret.errorMax = boost::json::value_to<double>(obj.at("errorMax"));
 
     return ret;
 }