Merge pull request #1961 from alicevision/dev/localbundle

Local bundle adjustment's covisibility estimation optimization

Author: servantftransperfect

src/aliceVision/sfm/pipeline/expanding/ConnexityGraph.cpp

@@ -5,12 +5,13 @@
 // You can obtain one at https://mozilla.org/MPL/2.0/.
 
 #include "ConnexityGraph.hpp"
-//#include <aliceVision/stl/Counter.hpp>
+#include <aliceVision/stl/stl.hpp>
 #include <lemon/bfs.h>
 
 namespace aliceVision {
 namespace sfm {
 
+
 struct viewIdScored 
 {
     viewIdScored() = default;
@@ -33,77 +34,166 @@ struct viewIdScored
     size_t card = 0;        
 };
 
-bool ConnexityGraph::build(const sfmData::SfMData & sfmData, const std::set<IndexT> & viewsOfInterest)
+bool ConnexityGraph::updateCocardinalities(const sfmData::SfMData & sfmData, 
+                              const track::TracksPerView& tracksPerViews)
 {
-    lemon::ListGraph graph;
-    std::map<IndexT, lemon::ListGraph::Node> _nodePerViewId;
-    std::map<lemon::ListGraph::Node, IndexT> viewIdPerNode;
-
     //Create a list of reconstructed views
-    std::vector<IndexT> views;
-    for (const auto & pv : sfmData.getViews())
+    std::set<IndexT> setViews;
+    for (const auto & [viewId, view] : sfmData.getViews())
     {
-        if (sfmData.isPoseAndIntrinsicDefined(pv.first))
+        if (sfmData.isPoseAndIntrinsicDefined(viewId))
         {
-            views.push_back(pv.first);
-
-            lemon::ListGraph::Node newNode = graph.addNode();
-            _nodePerViewId[pv.first] = newNode;
-            viewIdPerNode[newNode] = pv.first;
+            setViews.insert(viewId);
         }
     }
 
+    //Get incoming views
+    std::set<IndexT> newViews;
+    std::set_difference(setViews.begin(), setViews.end(), 
+                        _previousViews.begin(), _previousViews.end(), 
+                        std::inserter(newViews, newViews.begin()));
+    
+    ALICEVISION_LOG_INFO("Update cocardinalities with " << newViews.size() << " new views");
 
-    //Build a list of landmarks index per view
-    std::map<IndexT, std::set<IndexT>> landmarksPerView;
-    for (const auto & pl : sfmData.getLandmarks())
+    //Retrieve landmarks
+    const sfmData::Landmarks & landmarks = sfmData.getLandmarks();
+    
+    //Get All landmark ids (keys)
+    std::set<IndexT> landmarkIds;
+    std::transform(landmarks.begin(), landmarks.end(), 
+                    std::inserter(landmarkIds, landmarkIds.begin()), 
+                    stl::RetrieveKey());
+
+    //For all incoming views only
+    for (const IndexT newView: newViews)
     {
-        for (const auto & po : pl.second.getObservations())
+        const track::TrackIdSet & trackIds = tracksPerViews.at(newView);
+        
+        //Build a list of landmarks observed by this view
+        std::vector<IndexT> newViewLandmarks;
+        newViewLandmarks.reserve(trackIds.size());
+        std::set_intersection(trackIds.begin(), trackIds.end(), 
+                              landmarkIds.begin(), landmarkIds.end(), 
+                              std::back_inserter(newViewLandmarks));
+
+        //Loop over all landmarks
+        std::map<IndexT, size_t> commonLandmarksCountPerView;
+        for (const IndexT & idLandmark : newViewLandmarks)
+        {   
+            //Loop over all view observing the landmark
+            const sfmData::Landmark & landmark = landmarks.at(idLandmark);
+            for (const auto & [idView, _] : landmark.getObservations())
+            {
+                if (idView == newView)
+                {
+                    continue;
+                }
+
+                //Update the coobservation counter
+                auto it = commonLandmarksCountPerView.find(idView);
+                if (it != commonLandmarksCountPerView.end())
+                {
+                    it->second++;
+                }
+                else
+                {
+                    commonLandmarksCountPerView[idView] = 1;
+                }
+            }
+        }
+
+        //Update the cocardinalities sparse matrix 
+        for (const auto [otherView, card]: commonLandmarksCountPerView)
         {
-            landmarksPerView[po.first].insert(pl.first);
+            Pair p;
+            p.first = std::min(newView, otherView);
+            p.second = std::max(newView, otherView);
+            _cocardinalities[p] = card;
         }
     }
+
 
-    //For all possible unique pairs    
-    std::map<IndexT, std::vector<viewIdScored>> covisibility;
-    for (int idref = 0; idref < views.size(); idref++)
+
+    //Get Removed views
+    std::set<IndexT> removedViews;
+    std::set_difference(_previousViews.begin(), _previousViews.end(),
+                        setViews.begin(), setViews.end(), 
+                        std::inserter(removedViews, removedViews.begin()));
+
+    ALICEVISION_LOG_INFO("Update cocardinalities with " << removedViews.size() << " removed views");
+
+    for (const IndexT removedView : removedViews)
     {
-        IndexT viewRef = views[idref];
+        std::erase_if(_cocardinalities, 
+                    [removedView](const auto & item)
+                    {
+                        const auto & pair = item.first;
+                        return ((pair.first == removedView) || (pair.second == removedView));
+                    }
+        );
+    }
 
-        const auto & ptref = landmarksPerView[viewRef];
-        
-        for (int idcur = idref + 1; idcur < views.size(); idcur++)
-        {
-            IndexT viewCur = views[idcur];
+    //Copy for next iteration
+    _previousViews = setViews;
+    
+    return true;
+}
 
-            const auto & ptcur = landmarksPerView[viewCur];
+bool ConnexityGraph::build(const sfmData::SfMData & sfmData, 
+                        const track::TracksPerView& tracksPerViews, 
+                        const std::set<IndexT> & viewsOfInterest)
+{
+    lemon::ListGraph graph;
+    std::map<IndexT, lemon::ListGraph::Node> nodePerViewId;
+    std::map<lemon::ListGraph::Node, IndexT> viewIdPerNode;
 
-            std::vector<IndexT> intersection;
-            std::set_intersection(ptref.begin(), ptref.end(), ptcur.begin(), ptcur.end(), 
-                                    std::back_inserter(intersection));
+    if (!updateCocardinalities(sfmData, tracksPerViews))
+    {
+        return false;
+    }
 
-            size_t s = intersection.size();
-            if (s == 0)
-            {
-                continue;
-            }
+    ALICEVISION_LOG_INFO("Connexity graph begin");
+    //Reset result
+    _distancesPerPoseId.clear();
 
-            covisibility[viewRef].push_back({viewCur, s});
-            covisibility[viewCur].push_back({viewRef, s});
+    //Create a list of reconstructed views
+    std::vector<IndexT> views;
+    for (const auto & [viewId, view] : sfmData.getViews())
+    {
+        if (sfmData.isPoseAndIntrinsicDefined(viewId))
+        {
+            views.push_back(viewId);
+
+            lemon::ListGraph::Node newNode = graph.addNode();
+            nodePerViewId[viewId] = newNode;
+            viewIdPerNode[newNode] = viewId;
         }
     }
 
+    
+    //For all possible unique pairs    
+    std::map<IndexT, std::vector<viewIdScored>> covisibility;
+    for (const auto & [pair, s]: _cocardinalities)
+    {
+        covisibility[pair.first].push_back({pair.second, s});
+        covisibility[pair.second].push_back({pair.first, s});
+    }
+
     //Filter out connexions without enough information
     for (auto & item : covisibility)
     {
         auto & vec = item.second;
+
+        //Just skip filtering if we don't have more than _minLinksPerView links
         if (vec.size() < _minLinksPerView)
         {
             continue;
         }
 
+        //Sort the vector by descending order of shared observations
         std::sort(vec.begin(), vec.end(), std::greater<>());
 
+        //Count the number of items with enough observations
         size_t pos = 0;
         for (; pos < vec.size(); pos++)
         {
@@ -113,6 +203,7 @@ bool ConnexityGraph::build(const sfmData::SfMData & sfmData, const std::set<Inde
             }
         }
 
+        //Keep at LEAST _minLinksPerView
         pos = std::max(pos, _minLinksPerView);
         vec.resize(pos);
     }
@@ -130,8 +221,8 @@ bool ConnexityGraph::build(const sfmData::SfMData & sfmData, const std::set<Inde
         {
             IndexT viewId2 = part.viewId;
 
-            const lemon::ListGraph::Node & node1 = _nodePerViewId[viewId1];
-            const lemon::ListGraph::Node & node2 = _nodePerViewId[viewId2];
+            const lemon::ListGraph::Node & node1 = nodePerViewId[viewId1];
+            const lemon::ListGraph::Node & node2 = nodePerViewId[viewId2];
 
             graph.addEdge(node1, node2);
         }
@@ -159,8 +250,8 @@ bool ConnexityGraph::build(const sfmData::SfMData & sfmData, const std::set<Inde
 
             if (intrinsicIdRef != intrinsicIdCur) continue;
 
-            const lemon::ListGraph::Node & node1 = _nodePerViewId[viewRef];
-            const lemon::ListGraph::Node & node2 = _nodePerViewId[viewCur];
+            const lemon::ListGraph::Node & node1 = nodePerViewId[viewRef];
+            const lemon::ListGraph::Node & node2 = nodePerViewId[viewCur];
 
             graph.addEdge(node1, node2);
         }
@@ -174,15 +265,15 @@ bool ConnexityGraph::build(const sfmData::SfMData & sfmData, const std::set<Inde
 
     for (auto id : viewsOfInterest)
     {
-        auto it = _nodePerViewId.find(id);
-        if (it != _nodePerViewId.end())
+        auto it = nodePerViewId.find(id);
+        if (it != nodePerViewId.end())
         {
             bfs.addSource(it->second);
         }
     }
 
     bfs.start();
-    for (const auto & x : _nodePerViewId)
+    for (const auto & x : nodePerViewId)
     {   
         //Retrieve the poseId associated to this view
         IndexT poseId = sfmData.getView(x.first).getPoseId();
@@ -210,6 +301,8 @@ bool ConnexityGraph::build(const sfmData::SfMData & sfmData, const std::set<Inde
         }
     }
 
+    ALICEVISION_LOG_INFO("Connexity graph end");
+
     return true;
 }
 

src/aliceVision/sfm/pipeline/expanding/ConnexityGraph.hpp

@@ -8,20 +8,26 @@
 
 #include <aliceVision/sfmData/SfMData.hpp>
 #include <lemon/list_graph.h>
+#include <aliceVision/track/Track.hpp>
 
 namespace aliceVision {
 namespace sfm {
 
 class ConnexityGraph
 {
 public:
+
     /**
      * Compute distances of all the sfmData views to the viewsOfInterest views
      * @param sfmData the sfmData containing all the views
-     * @param viewsOfInterest the list of views to compute the distance from. Those views must also be in the sfmData !
+     * @param tracksPerView list of track ids indexed per view
+     * @param viewsOfInterest the list of views to compute the distance from. 
+     * Those views must also be in the sfmData !
      * @return false if an error occurred
     */
-    bool build(const sfmData::SfMData & sfmData, const std::set<IndexT> & viewsOfInterest);
+    bool build(const sfmData::SfMData & sfmData,  
+                const track::TracksPerView& tracksPerViews,
+                const std::set<IndexT> & viewsOfInterest);
 
     /**
      * Get the distance for a particular poseId to one view of interest
@@ -31,8 +37,20 @@ class ConnexityGraph
     int getDistance(IndexT poseId) const;
 
 
+private:
+    /**
+     * update the coCardinalities
+     * @param sfmData the sfmData containing all the views
+     * @param tracksPerView list of track ids indexed per view
+     * @return false if an error occurred
+    */
+    bool updateCocardinalities(const sfmData::SfMData & sfmData, 
+                              const track::TracksPerView& tracksPerViews);
+
 private:
     std::map<IndexT, int> _distancesPerPoseId;
+    std::set<IndexT> _previousViews;
+    std::map<Pair, size_t> _cocardinalities;
 
 private:
     size_t _minLinksPerView = 10;

src/aliceVision/sfm/pipeline/expanding/ExpansionChunk.cpp

@@ -32,6 +32,12 @@ bool ExpansionChunk::process(sfmData::SfMData & sfmData, const track::TracksHand
         return false;
     }
 
+    //Check if historyHandler exists
+    if (!_historyHandler)
+    {
+        return false;
+    }
+
     //How many views to resect ?
     size_t countToProcess = 0;
     for (const IndexT viewId : viewsChunk)
@@ -52,6 +58,7 @@ bool ExpansionChunk::process(sfmData::SfMData & sfmData, const track::TracksHand
     };
 
     std::vector<IntermediateResectionInfo> intermediateInfos;
+    std::set<IndexT> addedViews;
 
     ALICEVISION_LOG_INFO("Resection start");
     #pragma omp parallel for
@@ -119,6 +126,7 @@ bool ExpansionChunk::process(sfmData::SfMData & sfmData, const track::TracksHand
         }
 
         addPose(sfmData, item.viewId, item.pose);
+        addedViews.insert(item.viewId);
     }
 
     // Get a list of valid views
@@ -149,11 +157,8 @@ bool ExpansionChunk::process(sfmData::SfMData & sfmData, const track::TracksHand
     {
         setConstraints(sfmData, tracksHandler, validViewIds);
     }
-
-    if (_historyHandler)
-    {
-        _historyHandler->saveState(sfmData);
-    }
+    
+    _historyHandler->saveState(sfmData);
 
     ALICEVISION_LOG_INFO("ExpansionChunk::process end");
 

src/aliceVision/sfm/pipeline/expanding/ExpansionHistory.hpp

@@ -52,6 +52,10 @@ class ExpansionHistory
         return _epoch;
     }
 
+    /**
+     * Save the state after a chunk
+     * @param sfmData the current sfmData
+    */
     void saveState(const sfmData::SfMData & sfmData);
 
     /**
@@ -73,6 +77,12 @@ class ExpansionHistory
 private:
     // History of focals per intrinsics
     std::map<IndexT, std::vector<std::pair<size_t, double>>> _focalHistory;
+
+    // History of added views
+    std::vector<std::set<IndexT>> _addedViews;
+
+    // History of removed views
+    std::vector<std::set<IndexT>> _removedViews;
 
     // epoch ID
     std::size_t _epoch = 0;