DepthMap based priors in sfm

Conflicts:
	src/aliceVision/sfm/pipeline/bootstrapping/PairsScoring.cpp
	src/aliceVision/sfmData/Landmark.hpp
	src/software/pipeline/main_sfmBootstrapping.cpp

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,6 +8,8 @@
 #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 {
@@ -116,5 +118,100 @@ IndexT findBestPair(const sfmData::SfMData & sfmData,
     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

@@ -37,5 +37,18 @@ IndexT findBestPair(const sfmData::SfMData & sfmData,
                     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);
+
 }
 }

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

@@ -0,0 +1,85 @@
+// This file is part of the AliceVision project.
+// Copyright (c) 2025 AliceVision contributors.
+// This Source Code Form is subject to the terms of the Mozilla Public License,
+// v. 2.0. If a copy of the MPL was not distributed with this file,
+// You can obtain one at https://mozilla.org/MPL/2.0/.
+
+#include <aliceVision/sfm/pipeline/bootstrapping/TracksDepths.hpp>
+
+namespace aliceVision {
+namespace sfm {
+
+bool buildSfmDataFromDepthMap(sfmData::SfMData & output,
+                            const sfmData::SfMData & sfmData, 
+                            const track::TracksMap& tracksMap, 
+                            const track::TracksPerView & tracksPerView, 
+                            IndexT viewId)
+{
+    output.clear();
+
+    const sfmData::View & view = sfmData.getView(viewId);
+    const camera::IntrinsicBase & intrinsic = sfmData.getIntrinsic(view.getIntrinsicId());
+
+    if (tracksPerView.find(viewId) == tracksPerView.end())
+    {
+        return false;
+    }
+
+    sfmData::Landmarks & landmarks = output.getLandmarks();
+
+    //Copy all intrinsics, because it's light.
+    for (const auto & [intrinsicId, intrinsic] : sfmData.getIntrinsics())
+    {
+        output.getIntrinsics().insert(
+            std::make_pair(intrinsicId, 
+                camera::IntrinsicBase::sptr(intrinsic->clone()))
+            );
+    }
+
+    std::set<IndexT> usedViewIds;
+    const auto & trackIds = tracksPerView.at(viewId);
+    for (const auto & trackId : trackIds)
+    {
+        const auto & track = tracksMap.at(trackId);
+        const auto & feat = track.featPerView.at(viewId);
+        const double & iZ = feat.idepth;
+
+        if (iZ < 0.0)
+        {
+            continue;
+        }
+
+        const double Z = 1.0 / iZ;
+        
+        const Vec2 meters = intrinsic.removeDistortion(intrinsic.ima2cam(feat.coords.cast<double>()));
+        
+        sfmData::Landmark & landmark = landmarks[trackId];
+        landmark.X.x() = meters.x() * Z;
+        landmark.X.y() = meters.y() * Z;
+        landmark.X.z() = Z;
+
+        landmark.descType = track.descType;
+
+        sfmData::Observations & observations = landmark.getObservations();
+        for (const auto & [otherViewId, otherFeat] : track.featPerView)
+        {
+            usedViewIds.insert(otherViewId);
+        }
+    }
+
+    // Copy only used views
+    for (const auto & usedViewId : usedViewIds)
+    {
+        const auto & iview = sfmData.getViewSharedPtr(usedViewId);
+
+        output.getViews().insert(
+            std::make_pair(usedViewId, 
+                sfmData::View::sptr(iview->clone()))
+            );
+    }
+
+    return true;
+}
+
+}
+}

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

@@ -0,0 +1,30 @@
+// This file is part of the AliceVision project.
+// Copyright (c) 2025 AliceVision contributors.
+// This Source Code Form is subject to the terms of the Mozilla Public License,
+// v. 2.0. If a copy of the MPL was not distributed with this file,
+// You can obtain one at https://mozilla.org/MPL/2.0/.
+#pragma once
+
+#include <aliceVision/track/Track.hpp>
+#include <aliceVision/sfmData/SfMData.hpp>
+
+namespace aliceVision {
+namespace sfm {
+
+/**
+ * @brief Create a new sfmData cleared from any existing landmarks and poses
+ * But with new landmarks created from tracks if depth exists
+ * @param output the result sfmData
+ * @param sfmData the input sfmData, used for views and intrinsics
+ * @param tracksMap the input map of tracks
+ * @param tracksPerView tracks grouped by views 
+ * @param viewId the view of interest identifier
+ * @return false if an error occured
+*/
+bool buildSfmDataFromDepthMap(sfmData::SfMData & output,
+                            const sfmData::SfMData & sfmData, 
+                            const track::TracksMap& tracksMap, 
+                            const track::TracksPerView & tracksPerView, 
+                            IndexT viewId);
+}
+}