DepthMap based priors in sfm

Author: Fabien Servant

src/aliceVision/sfm/CMakeLists.txt

@@ -27,6 +27,7 @@ set(sfm_files_headers
     pipeline/bootstrapping/EstimateAngle.hpp
     pipeline/bootstrapping/PairsScoring.hpp
     pipeline/bootstrapping/Bootstrap.hpp
+    pipeline/bootstrapping/TracksDepths.hpp
     pipeline/expanding/SfmTriangulation.hpp
     pipeline/expanding/SfmResection.hpp
     pipeline/expanding/SfmBundle.hpp
@@ -71,6 +72,7 @@ set(sfm_files_sources
     pipeline/bootstrapping/EstimateAngle.cpp
     pipeline/bootstrapping/PairsScoring.cpp
     pipeline/bootstrapping/Bootstrap.cpp
+    pipeline/bootstrapping/TracksDepths.cpp
     pipeline/expanding/SfmTriangulation.cpp
     pipeline/expanding/SfmResection.cpp
     pipeline/expanding/SfmBundle.cpp

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

@@ -8,6 +8,7 @@
 #include <aliceVision/track/tracksUtils.hpp>
 #include <aliceVision/multiview/triangulation/triangulationDLT.hpp>
 #include <aliceVision/sfm/pipeline/expanding/SfmResection.hpp>
+#include <aliceVision/sfm/pipeline/bootstrapping/TracksDepths.hpp>
 #include <vector>
 #include <random>
 
@@ -166,5 +167,96 @@ bool bootstrapMesh(sfmData::SfMData & sfmData,
     return true;
 }
 
+bool bootstrapDepth(sfmData::SfMData & sfmData, 
+                    const IndexT referenceViewId,
+                    const IndexT nextViewId,
+                    const track::TracksMap& tracksMap, 
+                    const track::TracksPerView & tracksPerView)
+{
+    std::mt19937 randomNumberGenerator;
+
+    const sfmData::View & viewReference = sfmData.getView(referenceViewId);
+    const sfmData::View & viewNext = sfmData.getView(nextViewId);
+
+    camera::IntrinsicBase::sptr camReference = sfmData.getIntrinsicSharedPtr(viewReference.getIntrinsicId());
+    camera::IntrinsicBase::sptr camNext = sfmData.getIntrinsicSharedPtr(viewNext.getIntrinsicId());
+
+    sfmData::CameraPose & poseReference = sfmData.getPoses()[viewReference.getPoseId()];
+    sfmData::CameraPose & poseNext = sfmData.getPoses()[viewNext.getPoseId()];
+
+            
+    sfmData::SfMData miniSfm;
+    if (!buildSfmDataFromDepthMap(miniSfm, sfmData, tracksMap, tracksPerView, referenceViewId))
+    {
+        return EXIT_FAILURE;
+    }
+
+    sfm::SfmResection resection(50000, std::numeric_limits<double>::infinity());
+
+    Eigen::Matrix4d pose;
+    double newThreshold;
+    size_t inliersCount;
+        
+    if (!resection.processView(miniSfm, 
+                            tracksMap, tracksPerView, 
+                            randomNumberGenerator,
+                            nextViewId, pose, newThreshold, inliersCount))
+    {
+        return EXIT_FAILURE;
+    }
+
+
+    geometry::Pose3 pose3(pose);
+    poseNext.setTransform(pose3);
+
+    const auto & landmarks = miniSfm.getLandmarks();
+    auto & outLandmarks = sfmData.getLandmarks();
+    
+    for (const auto & [landmarkId, landmark] : landmarks)
+    {
+        //Retrieve track object
+        const auto & track = tracksMap.at(landmarkId);
+
+        const track::TrackItem & itemReference = track.featPerView.at(referenceViewId);
+
+        //Maybe this track is not observed in the next view
+        if (track.featPerView.find(nextViewId) == track.featPerView.end())
+        {
+            continue;
+        }
+
+        //Compute error
+        const track::TrackItem & item = track.featPerView.at(nextViewId);
+        const Vec2 pt = item.coords;
+        const Vec2 estpt = camNext->transformProject(pose3, landmark.X.homogeneous(), true);
+        double err = (pt - estpt).norm();
+
+        //If error is ok, then we add it to the sfmData
+        if (err <= newThreshold)
+        {
+            sfmData::Observation obs;
+            obs.setFeatureId(item.featureId);
+            obs.setScale(item.scale);
+            obs.setCoordinates(item.coords);
+
+            sfmData::Observation obsReference;
+            obsReference.setFeatureId(itemReference.featureId);
+            obsReference.setScale(itemReference.scale);
+            obsReference.setCoordinates(itemReference.coords);
+
+            //Add landmark to sfmData
+            outLandmarks[landmarkId] = landmark;
+            outLandmarks[landmarkId].setParallaxRobust(true);
+
+            //Add observation to landmark
+            sfmData::Observations & observations = outLandmarks[landmarkId].getObservations();
+            observations[referenceViewId] = obsReference;
+            observations[nextViewId] = obs;
+        }
+    }
+
+    return true;
+}
+
 }
 }

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

@@ -46,5 +46,21 @@ bool bootstrapMesh(sfmData::SfMData & sfmData,
                     const track::TracksMap& tracksMap, 
                     const track::TracksPerView & tracksPerView);
 
+
+/**
+ * @brief Create a minimal SfmData with poses and landmarks for two views
+ * @param sfmData the input sfmData which contains camera information
+ * @param referenceViewId the reference view id
+ * @param otherViewId the other view id
+ * @param otherTreference the relative pose
+ * @param tracksMap the input map of tracks
+ * @param tracksPerView tracks grouped by views
+ * @return true
+*/
+bool bootstrapDepth(sfmData::SfMData & sfmData, 
+                    const IndexT referenceViewId,
+                    const IndexT otherViewId,
+                    const track::TracksMap& tracksMap, 
+                    const track::TracksPerView & tracksPerView);
 }
 }

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

@@ -8,62 +8,27 @@
 #include <aliceVision/sfm/pipeline/bootstrapping/EstimateAngle.hpp>
 #include <aliceVision/sfm/pipeline/expanding/ExpansionPolicyLegacy.hpp>
 #include <aliceVision/multiview/triangulation/triangulationDLT.hpp>
+#include <aliceVision/sfm/pipeline/bootstrapping/TracksDepths.hpp>
+#include <aliceVision/sfm/pipeline/expanding/SfmResection.hpp>
 
 namespace aliceVision {
 namespace sfm {
 
-IndexT findBestPair(const sfmData::SfMData & sfmData, 
+sfm::ReconstructedPair findBestPair(const sfmData::SfMData & sfmData, 
                 const std::vector<sfm::ReconstructedPair> & pairs,
                 const track::TracksMap& tracksMap, 
                 const track::TracksPerView & tracksPerView, 
-                const std::set<IndexT> & filterIn,
-                const std::set<IndexT> & filterOut,
                 double hardMinAngle,
                 double softMinAngle,
                 double maxAngle)
 {
-    IndexT bestPair = UndefinedIndexT;
+    sfm::ReconstructedPair bestPair;
+    bestPair.reference = UndefinedIndexT;
     double maxScore = std::numeric_limits<double>::lowest();
 
     for (IndexT pairId = 0; pairId < pairs.size(); pairId++)
     {
         const sfm::ReconstructedPair & pair = pairs[pairId];
-
-        if (!filterIn.empty())
-        {
-            bool found = false;
-
-            for (auto item : filterIn)
-            {
-                if (pair.reference == item || pair.next == item)
-                {
-                    found = true;
-                }
-            }
-
-            if (!found)
-            {
-                continue;
-            }
-        }
-
-        if (!filterOut.empty())
-        {
-            bool found = false;
-
-            for (auto item : filterOut)
-            {
-                if (pair.reference == item || pair.next == item)
-                {
-                    found = true;
-                }
-            }
-
-            if (found)
-            {
-                continue;
-            }
-        }
 
         double angle = 0.0;
         std::vector<size_t> usedTracks;
@@ -103,12 +68,107 @@ IndexT findBestPair(const sfmData::SfMData & sfmData,
         if (score > maxScore)
         {
             maxScore = score;
-            bestPair = pairId;
+            bestPair = pair;
         }
     }
 
     return bestPair;
 }
 
+sfm::ReconstructedPair findBestPairFromTrackDepths(const sfmData::SfMData & sfmData, 
+                                    const std::vector<sfm::ReconstructedPair> & pairs,
+                                    const track::TracksMap& tracksMap, 
+                                    const track::TracksPerView & tracksPerView)
+{
+    std::mt19937 randomNumberGenerator;
+
+    std::set<IndexT> views;
+    for (IndexT pairId = 0; pairId < pairs.size(); pairId++)
+    {
+        views.insert(pairs[pairId].reference);
+        views.insert(pairs[pairId].next);
+    }
+
+    sfm::ReconstructedPair bestPair;
+    bestPair.reference = UndefinedIndexT;
+    
+    size_t bestCount = 0;
+
+    for (const auto & idView: views)
+    {
+        sfmData::SfMData miniSfm;
+        if (!buildSfmDataFromDepthMap(miniSfm, sfmData, tracksMap, tracksPerView, idView))
+        {
+            continue;
+        }
+
+        sfm::SfmResection resection(1024, std::numeric_limits<double>::infinity());
+
+
+        sfm::ReconstructedPair bestPairLocal;
+        size_t maxInliers = 0;
+        size_t totalInliers = 0;
+
+        for (IndexT pairId = 0; pairId < pairs.size(); pairId++)
+        {
+            const auto & pairSource = pairs[pairId];
+            sfm::ReconstructedPair pair;
+            pair.reference = idView;
+
+            if (pairSource.reference == idView)
+            {
+                pair.next = pairSource.next;
+            }
+            else if (pairSource.next == idView)
+            {
+                pair.next = pairSource.reference;
+            }
+            else
+            {
+                continue;
+            }
+
+            if (miniSfm.getViews().find(pair.next) == miniSfm.getViews().end())
+            {
+                continue;
+            }
+
+            Eigen::Matrix4d pose;
+            double newThreshold;
+            size_t inliersCount;
+            
+            if (!resection.processView(miniSfm, 
+                                    tracksMap, 
+                                    tracksPerView, 
+                                    randomNumberGenerator,
+                                    pair.next, 
+                                    pose, newThreshold, inliersCount))
+            {
+                continue;
+            }    
+
+            pair.pose = geometry::Pose3(pose);
+            pair.score = inliersCount;
+
+            if (inliersCount > maxInliers)
+            {
+                maxInliers = inliersCount;
+                bestPairLocal = pair;
+            }
+
+            totalInliers += inliersCount;
+        }
+
+        if (totalInliers > bestCount)
+        {
+            bestPair = bestPairLocal;
+            bestCount = totalInliers;
+        }
+    }
+
+
+    return bestPair;
+}
+
 }
 }

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

@@ -20,22 +20,31 @@ namespace sfm {
 * @param pairs the input list of reconstructed pairs
 * @param tracksMap the input map of tracks
 * @param tracksPerView tracks grouped by views
-* @param filterIn pair must contains one of the views inside this set (if non empty)
-* @param filterout pair must NOT contains one of the views inside this set (if non empty)
 * @param hardMinAngle minimal angle allowed (rejected)
 * @param softMinAngle minimal angle allowed (tolerated, but heavily downgraded)
 * @param maxAngle maximal angle allowed
-* @return index in "pairs" of the best pair or UndefinedIndexT if no pair found
+* @return The best pair (pair.reference is UndefinedIndexT if nothing found)
 */
-IndexT findBestPair(const sfmData::SfMData & sfmData, 
-                    const std::vector<sfm::ReconstructedPair> & pairs,
-                    const track::TracksMap& tracksMap, 
-                    const track::TracksPerView & tracksPerView, 
-                    const std::set<IndexT> & filterIn,
-                    const std::set<IndexT> & filterOut,
-                    double hardMinAngle,
-                    double softMinAngle,
-                    double maxAngle);
+sfm::ReconstructedPair findBestPair(const sfmData::SfMData & sfmData, 
+                        const std::vector<sfm::ReconstructedPair> & pairs,
+                        const track::TracksMap& tracksMap, 
+                        const track::TracksPerView & tracksPerView, 
+                        double hardMinAngle,
+                        double softMinAngle,
+                        double maxAngle);
+
+/**
+* @brief Get best pair from track Depths with highest score
+* @param sfmData the input sfmData which contains camera information
+* @param pairs the input list of reconstructed pairs
+* @param tracksMap the input map of tracks
+* @param tracksPerView tracks grouped by views
+* @return The best pair (pair.reference is UndefinedIndexT if nothing found)
+*/
+sfm::ReconstructedPair findBestPairFromTrackDepths(const sfmData::SfMData & sfmData, 
+                            const std::vector<sfm::ReconstructedPair> & pairs,
+                            const track::TracksMap& tracksMap, 
+                            const track::TracksPerView & tracksPerView);
 
 }
 }