[filterSfM] add support for filtering landmarks

Author: almarouk

src/software/pipeline/main_filterSfM.cpp

@@ -45,30 +45,32 @@ namespace fs = boost::filesystem;
 
 static const std::size_t MAX_LEAF_ELEMENTS = 10;
 
-template <typename DATA>
-struct PointVectorAdaptator
+struct LandmarksAdaptator
 {
-    using Derived = PointVectorAdaptator; //!< In this case the dataset class is myself.
+    using Derived = LandmarksAdaptator; //!< In this case the dataset class is myself.
     using T = double;
 
-    const DATA& _data;
-    PointVectorAdaptator(const DATA& data)
-        : _data(data)
-    {
-    }
-
     /// CRTP helper method
     inline const Derived& derived() const { return *static_cast<const Derived*>(this); }
     /// CRTP helper method
     inline Derived& derived() { return *static_cast<Derived*>(this); }
 
+    const sfmData::Landmarks& _data;
+    LandmarksAdaptator(const sfmData::Landmarks& data)
+        : _data(data)
+    {
+    }
+
     // Must return the number of data points
     inline size_t kdtree_get_point_count() const { return _data.size(); }
 
     // Returns the dim'th component of the idx'th point in the class:
-    // Since this is inlined and the "dim" argument is typically an immediate value, the
-    //  "if/else's" are actually solved at compile time.
-    inline T kdtree_get_pt(const size_t idx, int dim) const { return _data.at(idx).m[dim]; }
+    inline T kdtree_get_pt(const size_t idx, int dim) const
+    {
+        auto it = _data.begin();
+        std::advance(it, idx);
+        return it->second.X(dim);
+    }
 
     // Optional bounding-box computation: return false to default to a standard bbox computation loop.
     //   Return true if the BBOX was already computed by the class and returned in "bb" so it can be avoided to redo it
@@ -80,50 +82,52 @@ struct PointVectorAdaptator
     }
 };
 
-template <typename DATA>
 using KdTree = nanoflann::KDTreeSingleIndexAdaptor<
-    nanoflann::L2_Simple_Adaptor<double, PointVectorAdaptator>,
-    PointVectorAdaptator, 3 /* dim */
+    nanoflann::L2_Simple_Adaptor<double, LandmarksAdaptator>,
+    LandmarksAdaptator,
+    3, /* dim */
+    IndexT
 >;
 
-template <typename DATA>
-class Tree
+/**
+ * A result-set class used when performing a radius based search.
+ */
+class PixSizeSearch
 {
-    std::unique_ptr<KdTree> _tree;
-    std::unique_ptr<PointVectorAdaptator> _pointCloudRef;
-
 public:
-    Tree(const DATA& data) { initKdTree(data); }
-
-    void initKdTree(const DATA& data)
+    const double _radius_sq;
+    const std::vector<double>& _pixSize;
+    const double _pixSize_i;
+    bool found = false;
+
+    inline PixSizeSearch(double radius, const std::vector<double>& pixSize, int i)
+        : _radius_sq(radius * radius)
+        , _pixSize(pixSize)
+        , _pixSize_i(pixSize[i])
     {
-        ALICEVISION_LOG_INFO("Build nanoflann KdTree index.");
-        _pointCloudRef = std::make_unique<PointVectorAdaptator>(data);
-        _tree = std::make_unique<KdTree>(3 /*dim*/, *_pointCloudRef.get(),
-                                         nanoflann::KDTreeSingleIndexAdaptorParams(MAX_LEAF_ELEMENTS));
-        _tree->buildIndex();
-        ALICEVISION_LOG_INFO("KdTree created for " << data.size() << " points.");
     }
 
-    bool locateNearestVertex(const Point3d& p, std::size_t& index, double& sq_dist) const
+    inline bool full() const { return found; }
+
+    inline bool addPoint(double dist, IndexT index)
     {
-        index = std::numeric_limits<std::size_t>::max();
-        sq_dist = std::numeric_limits<double>::max();
-        nanoflann::KNNResultSet<double, std::size_t> resultSet(1);
-        resultSet.init(&index, &sq_dist);
-        if(!_tree->findNeighbors(resultSet, p.m, nanoflann::SearchParams()))
+        if(dist < _radius_sq && _pixSize[index] < _pixSize_i)
         {
+            found = true;
             return false;
         }
         return true;
     }
+
+    inline double worstDist() const { return _radius_sq; }
 };
 
-bool filterLandmarks(SfMData& sfmData)
+bool filterLandmarks(SfMData& sfmData, double radiusScale)
 {
     mvsUtils::MultiViewParams mp(sfmData, "", "", "", false);
-    std::vector<std::pair<double, IndexT>> landmarksPixSize(sfmData.getLandmarks().size());
+    std::vector<double> landmarksPixSize(sfmData.getLandmarks().size());
 
+    ALICEVISION_LOG_INFO("Computing pixel size: started.");
     #pragma omp parallel for
     for(auto i = 0; i < sfmData.getLandmarks().size(); i++)
     {
@@ -137,26 +141,56 @@ bool filterLandmarks(SfMData& sfmData)
         for(const auto& observationPair : landmark.observations)
         {
             const IndexT viewId = observationPair.first;
-            auto d = mp.getCamPixelSize(Point3d(landmark.X.x(), landmark.X.y(), landmark.X.z()),
+            pixSize += mp.getCamPixelSize(Point3d(landmark.X.x(), landmark.X.y(), landmark.X.z()),
                                           mp.getIndexFromViewId(viewId), observationPair.second.scale);
-            if(d < 0.)
-                d = 0.;
-            pixSize += d;
-            if(pixSize < 0.)
-                pixSize = 0.;
             n++;
         }
-        if(pixSize < 0.)
-            pixSize = 0.;
         pixSize /= n;
-        if(pixSize < 0.)
-            pixSize = 0.;
 
-        landmarksPixSize[i] = std::pair<double, IndexT>(pixSize, landmarkPair->first);
+        landmarksPixSize[i] = pixSize;
     }
+    ALICEVISION_LOG_INFO("Computing pixel size: done.");
+
+    //// sort landmarks by descending pixSize order
+    //std::stable_sort(landmarksPixSize.begin(), landmarksPixSize.end(), std::greater<>{});
 
-    // sort landmarks by descending pixSize order
-    std::stable_sort(landmarksPixSize.begin(), landmarksPixSize.end(), std::greater<>{});
+    ALICEVISION_LOG_INFO("Build nanoflann KdTree index.");
+    LandmarksAdaptator data(sfmData.getLandmarks());
+    KdTree tree(3, data, nanoflann::KDTreeSingleIndexAdaptorParams(10));
+    tree.buildIndex();
+    ALICEVISION_LOG_INFO("KdTree created for " << sfmData.getLandmarks().size() << " points.");
+    std::vector<IndexT> newIdx(sfmData.getLandmarks().size());
+    IndexT currentIdx = 0;
+
+    ALICEVISION_LOG_INFO("Identifying landmarks to remove: started.");
+    #pragma omp parallel for
+    for (auto i = 0; i < sfmData.getLandmarks().size(); i++)
+    {
+        PixSizeSearch search(landmarksPixSize[i] * radiusScale, landmarksPixSize, i);
+        bool found = tree.findNeighbors(search, sfmData.getLandmarks().at(i).X.data(), nanoflann::SearchParams());
+        if (!found)
+        {
+            newIdx[i] = -1;
+        }
+        else
+        {
+            newIdx[i] = currentIdx++;
+        }
+    }
+    ALICEVISION_LOG_INFO("Identifying landmarks to remove: done.");
+
+    ALICEVISION_LOG_INFO("Removing landmarks: started.");
+    Landmarks filteredLandmarks;
+    #pragma omp parallel for
+    for (auto i = 0; i < sfmData.getLandmarks().size(); i++)
+    {
+        if(newIdx[i] != -1)
+        {
+            filteredLandmarks[newIdx[i]] = sfmData.getLandmarks().at(i);
+        }
+    }
+    sfmData.getLandmarks() = filteredLandmarks;
+    ALICEVISION_LOG_INFO("Removing landmarks: done.");
 
     //// take only best observations
     //observationScores.resize(maxNbObservationsPerLandmark);
@@ -230,6 +264,7 @@ int aliceVision_main(int argc, char *argv[])
     std::string inputSfmFilename;
     std::string outputSfmFilename;
     int maxNbObservationsPerLandmark = 5;
+    double radiusScale = 2;
 
     po::options_description requiredParams("Required parameters");
     requiredParams.add_options()
@@ -241,7 +276,9 @@ int aliceVision_main(int argc, char *argv[])
     po::options_description optionalParams("Optional parameters");
     optionalParams.add_options()
         ("maxNbObservationsPerLandmark", po::value<int>(&maxNbObservationsPerLandmark)->default_value(maxNbObservationsPerLandmark),
-         "Maximum number of allowed observations per landmark.");
+         "Maximum number of allowed observations per landmark.")
+        ("radiusScale", po::value<double>(&radiusScale)->default_value(radiusScale),
+         "Scale factor applied to pixel size based radius filter applied to landmarks.");
 
     CmdLine cmdline("AliceVision SfM filtering.");
     cmdline.add(requiredParams);
@@ -267,8 +304,12 @@ int aliceVision_main(int argc, char *argv[])
     }
 
     // filter SfM data
-    bool success2 = filterLandmarks(sfmData);
+    ALICEVISION_LOG_INFO("Filtering landmarks: started.");
+    bool success2 = filterLandmarks(sfmData, radiusScale);
+    ALICEVISION_LOG_INFO("Filtering landmarks: done.");
+    ALICEVISION_LOG_INFO("Filtering observations: started.");
     bool success1 = filterObservations(sfmData, maxNbObservationsPerLandmark);
+    ALICEVISION_LOG_INFO("Filtering observations: done.");
 
     if(success1)
     {