Fix LocalBA intrinsics edges management

src/aliceVision/matchingImageCollection/geometricFilterUtils_test.cpp

@@ -49,7 +49,7 @@ BOOST_AUTO_TEST_CASE(matchingImageCollection_centerMatrix)
 BOOST_AUTO_TEST_CASE(matchingImageCollection_similarityEstimation)
 {
   // same point should give the identity matrix
-  const feature::SIOPointFeature feat1 {1.0, 5.0, 1, 0.1};
+  const feature::SIOPointFeature feat1 {1.0f, 5.0f, 1.0f, 0.1f};
   Mat3 S;
   matchingImageCollection::computeSimilarity(feat1, feat1, S);
 

src/aliceVision/sfm/LocalBundleAdjustmentGraph.cpp

@@ -11,6 +11,7 @@
 #include <lemon/bfs.h>
 
 #include <fstream>
+#include <algorithm>
 
 namespace fs = boost::filesystem;
 
@@ -132,23 +133,66 @@ void LocalBundleAdjustmentGraph::exportIntrinsicsHistory(const std::string& fold
   os.close();
 }
 
-bool LocalBundleAdjustmentGraph::removeViewsToTheGraph(const std::set<IndexT>& removedViewsId)
+bool LocalBundleAdjustmentGraph::removeViews(const sfmData::SfMData& sfmData, const std::set<IndexT>& removedViewsId)
 {
   std::size_t numRemovedNode = 0;
+  std::map<IndexT, std::vector<int>> removedEdgesByIntrinsic;
+
   for(const IndexT& viewId : removedViewsId)
   {
     auto it = _nodePerViewId.find(viewId);
-    if(it != _nodePerViewId.end())
+    if(it == _nodePerViewId.end())
     {
-      _graph.erase(it->second); // this function erase a node with its incident arcs
-      _nodePerViewId.erase(it->first);
-      _viewIdPerNode.erase(it->second);
+      ALICEVISION_LOG_WARNING("The view id: " << viewId << " does not exist in the graph, cannot remove it.");
+      continue;
+    }
 
-      numRemovedNode++;
-      ALICEVISION_LOG_DEBUG("The view #" << viewId << " has been successfully removed to the distance graph.");
+    // keep track of node incident edges that are going to be removed
+    // in order to update _intrinsicEdgesId accordingly
+    { 
+      IndexT intrinsicId = sfmData.getView(viewId).getIntrinsicId();
+      auto intrinsicIt = _intrinsicEdgesId.find(intrinsicId);
+      if(intrinsicIt != _intrinsicEdgesId.end())
+      {
+        // store incident edge ids before removal
+        for(lemon::ListGraph::IncEdgeIt e(_graph, it->second); e != lemon::INVALID; ++e)
+        {
+          removedEdgesByIntrinsic[intrinsicId].push_back(_graph.id(lemon::ListGraph::Edge(e)));
+        }
+      }
+    }
+
+    _graph.erase(it->second); // this function erase a node with its incident arcs
+    _viewIdPerNode.erase(it->second);
+    _nodePerViewId.erase(it->first);
+
+    numRemovedNode++;
+    ALICEVISION_LOG_DEBUG("The view #" << viewId << " has been successfully removed to the distance graph.");
+  }
+
+  // remove erased edges from _intrinsicsEdgesId
+  for(auto& edgesIt : removedEdgesByIntrinsic)
+  {
+    const IndexT intrinsicId =  edgesIt.first;
+    std::vector<int>& edgeIds = _intrinsicEdgesId[intrinsicId];
+    std::vector<int>& removedEdges = edgesIt.second;
+
+    std::vector<int> newEdgeIds;
+    // sort before using set_difference
+    std::sort(edgeIds.begin(), edgeIds.end());
+    std::sort(removedEdges.begin(), removedEdges.end());
+
+    std::set_difference(
+      edgeIds.begin(), edgeIds.end(), 
+      removedEdges.begin(), removedEdges.end(), 
+      std::back_inserter(newEdgeIds)
+    );
+    std::swap(edgeIds, newEdgeIds);
+
+    if(edgeIds.empty())
+    {
+      _intrinsicEdgesId.erase(intrinsicId);
     }
-    else
-      ALICEVISION_LOG_WARNING("The view id: " << viewId << " does not exist in the graph, cannot remove it.");
   }
   return numRemovedNode == removedViewsId.size();
 }
@@ -252,7 +296,7 @@ void LocalBundleAdjustmentGraph::updateGraphWithNewViews(
     for(const Pair& edge: newEdges)
       _graph.addEdge(_nodePerViewId.at(edge.first), _nodePerViewId.at(edge.second));
 
-    addIntrinsicEdgesToTheGraph(sfmData, addedViewsId);
+    numAddedEdges += addIntrinsicEdgesToTheGraph(sfmData, addedViewsId);
   }
 
   ALICEVISION_LOG_DEBUG("The distances graph has been completed with " << nbAddedNodes<< " nodes & " << numAddedEdges << " edges.");
@@ -602,7 +646,7 @@ void LocalBundleAdjustmentGraph::drawGraph(const sfmData::SfMData& sfmData, cons
 
 std::size_t LocalBundleAdjustmentGraph::addIntrinsicEdgesToTheGraph(const sfmData::SfMData& sfmData, const std::set<IndexT>& newReconstructedViews)
 {
-  std::size_t numAddedEdges = 0;
+  std::map<Pair, IndexT> newIntrinsicEdges;
 
   for(IndexT newViewId : newReconstructedViews) // for each new view
   {
@@ -613,33 +657,38 @@ std::size_t LocalBundleAdjustmentGraph::addIntrinsicEdgesToTheGraph(const sfmDat
 
     for(const auto& x : _nodePerViewId) // for each reconstructed view in the graph
     {
-      if(newViewId == x.first)  // do not compare a view with itself
-        continue;
-
-      const IndexT reconstructedViewIntrinsicId = sfmData.getViews().at(x.first)->getIntrinsicId();
-
+      const auto& otherViewId = x.first;
+
       // if the new view share the same intrinsic than previously reconstructed views
-      if(reconstructedViewIntrinsicId == newViewIntrinsicId)
+      if(newViewId != otherViewId  // do not compare a view with itself
+         && newViewIntrinsicId == sfmData.getViews().at(otherViewId)->getIntrinsicId())
       {
-        const IndexT minId = std::min(x.first, newViewId);
-        const IndexT maxId = std::max(x.first, newViewId);
-
-        lemon::ListGraph::Edge edge = _graph.addEdge(_nodePerViewId[minId], _nodePerViewId[maxId]);
-        _intrinsicEdgesId[newViewIntrinsicId].push_back(_graph.id(edge));
-        numAddedEdges++;
+        // register a new intrinsic edge between those views
+        newIntrinsicEdges[std::minmax(otherViewId, newViewId)] = newViewIntrinsicId;
       }
     }
   }
-  return numAddedEdges;
+
+  // create registered intrinsic edges in lemon graph 
+  // and update _intrinsicEdgesId accordingly
+  for(const auto& newEdge : newIntrinsicEdges)
+  {
+    lemon::ListGraph::Edge edge = _graph.addEdge(_nodePerViewId[newEdge.first.first], _nodePerViewId[newEdge.first.second]);
+    _intrinsicEdgesId[newEdge.second].push_back(_graph.id(edge));
+  }
+  return newIntrinsicEdges.size();
 }
 
 void LocalBundleAdjustmentGraph::removeIntrinsicEdgesFromTheGraph(IndexT intrinsicId)
 {
   if(_intrinsicEdgesId.count(intrinsicId) == 0)
     return;
   for(int edgeId : _intrinsicEdgesId.at(intrinsicId))
-    _graph.erase(_graph.fromId(edgeId, lemon::ListGraph::Edge()));
-
+  {
+    const auto& edge = _graph.edgeFromId(edgeId);
+    assert(_graph.valid(edge));
+    _graph.erase(edge);
+  }
   _intrinsicEdgesId.erase(intrinsicId);
 }
 
@@ -653,7 +702,9 @@ std::size_t LocalBundleAdjustmentGraph::updateRigEdgesToTheGraph(const sfmData::
   {
     for(int edgeId : edgesPerRid.second)
     {
-      _graph.erase(_graph.fromId(edgeId, lemon::ListGraph::Edge()));
+      const auto& edge = _graph.edgeFromId(edgeId);
+      assert(_graph.valid(edge));
+      _graph.erase(edge);
     }
   }
   _rigEdgesId.clear();
@@ -692,5 +743,21 @@ std::size_t LocalBundleAdjustmentGraph::updateRigEdgesToTheGraph(const sfmData::
   return numAddedEdges;
 }
 
+unsigned int LocalBundleAdjustmentGraph::countNodes() const
+{
+  unsigned int count = 0;
+  for(lemon::ListGraph::NodeIt n(_graph); n != lemon::INVALID; ++n)
+    ++count;
+  return count;
+}
+
+unsigned int LocalBundleAdjustmentGraph::countEdges() const
+{
+  unsigned int count = 0;
+  for(lemon::ListGraph::EdgeIt e(_graph); e != lemon::INVALID; ++e)
+    ++count;
+  return count;
+}
+
 } // namespace sfm
 } // namespace aliceVision

src/aliceVision/sfm/LocalBundleAdjustmentGraph.hpp

@@ -149,7 +149,7 @@ class LocalBundleAdjustmentGraph
    * @param[in] removedViewsId Set of views index to remove
    * @return true if the number of removed node is equal to the size of \c removedViewsId
    */
-  bool removeViewsToTheGraph(const std::set<IndexT>& removedViewsId);
+  bool removeViews(const sfmData::SfMData& sfmData, const std::set<IndexT>& removedViewsId);
 
   /**
    * @brief Complete the graph with the newly resected views or all the posed views if the graph is empty.
@@ -200,7 +200,19 @@ class LocalBundleAdjustmentGraph
    * @return
    */
   std::size_t updateRigEdgesToTheGraph(const sfmData::SfMData& sfmData);
-
+
+  /**
+   * @brief Count and return the number of nodes in the underlying lemon graph.
+   * @return The number of nodes in the graph.
+   */
+  unsigned int countNodes() const;
+
+  /**
+   * @brief Count and return the number of edges in the underlying lemon graph.
+   * @return The number of edges in the graph.
+   */
+  unsigned int countEdges() const;
+
 private:
 
   /**

src/aliceVision/sfm/bundleAdjustment_test.cpp

@@ -208,6 +208,9 @@ BOOST_AUTO_TEST_CASE(LOCAL_BUNDLE_ADJUSTMENT_EffectiveMinimization_Pinhole_Camer
   // 3. Use the graph-distances to assign a LBA state (Refine, Constant & Ignore) for each parameter (poses, intrinsics & landmarks)
   localBAGraph->convertDistancesToStates(sfmData);
 
+  BOOST_CHECK_EQUAL(localBAGraph->countNodes(), 4); // 4 views => 4 nodes
+  BOOST_CHECK_EQUAL(localBAGraph->countEdges(), 6); // landmarks connections: 6 edges created (see scheme)
+
   BOOST_CHECK_EQUAL(localBAGraph->getNbPosesPerState(BundleAdjustment::EParameterState::REFINED), 2);     // v0 & v1
   BOOST_CHECK_EQUAL(localBAGraph->getNbPosesPerState(BundleAdjustment::EParameterState::CONSTANT), 1);    // v2
   BOOST_CHECK_EQUAL(localBAGraph->getNbPosesPerState(BundleAdjustment::EParameterState::IGNORED), 1);     // v3

src/aliceVision/sfm/pipeline/sequential/ReconstructionEngine_sequentialSfM.cpp

@@ -695,9 +695,9 @@ bool ReconstructionEngine_sequentialSfM::bundleAdjustment(std::set<IndexT>& newR
 
     if(_params.useLocalBundleAdjustment && !removedViewsIdIteration.empty())
     {
-      // remove removed views to the graph
-      _localStrategyGraph->removeViewsToTheGraph(removedViewsIdIteration);
-      ALICEVISION_LOG_DEBUG("Removed views to the local strategy graph: " << removedViewsIdIteration);
+      // remove views from localBA graph
+      _localStrategyGraph->removeViews(_sfmData, removedViewsIdIteration);
+      ALICEVISION_LOG_DEBUG("Removed views from local strategy graph: " << removedViewsIdIteration);
     }
 
     ALICEVISION_LOG_INFO("Bundle adjustment iteration: " << iteration << " took " << std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::steady_clock::now() - chronoItStart).count() << " msec.");