[WIP] stash commit

Author: almarouk

src/software/pipeline/CMakeLists.txt

@@ -392,8 +392,10 @@ if(ALICEVISION_BUILD_SFM)
           aliceVision_cmdline
           aliceVision_sfmData
           aliceVision_sfmDataIO
+          aliceVision_mvsUtils
           Boost::program_options
           Boost::filesystem
+          nanoflann
   )
 
 endif() # if(ALICEVISION_BUILD_SFM)

src/software/pipeline/main_filterSfM.cpp

@@ -11,6 +11,9 @@
 #include <aliceVision/system/main.hpp>
 #include <aliceVision/config.hpp>
 #include <aliceVision/sfmDataIO/viewIO.hpp>
+#include <aliceVision/mvsUtils/MultiViewParams.hpp>
+
+#include "nanoflann.hpp"
 
 #include <boost/program_options.hpp>
 #include <boost/filesystem.hpp>
@@ -40,7 +43,137 @@ using namespace aliceVision::sfmDataIO;
 namespace po = boost::program_options;
 namespace fs = boost::filesystem;
 
-bool filterSfMData(SfMData& sfmData, int maxNbObservationsPerLandmark)
+static const std::size_t MAX_LEAF_ELEMENTS = 10;
+
+template <typename DATA>
+struct PointVectorAdaptator
+{
+    using Derived = PointVectorAdaptator; //!< 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); }
+
+    // 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]; }
+
+    // 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
+    //   again. Look at bb.size() to find out the expected dimensionality (e.g. 2 or 3 for point clouds)
+    template <class BBOX>
+    bool kdtree_get_bbox(BBOX& bb) const
+    {
+        return false;
+    }
+};
+
+template <typename DATA>
+using KdTree = nanoflann::KDTreeSingleIndexAdaptor<
+    nanoflann::L2_Simple_Adaptor<double, PointVectorAdaptator>,
+    PointVectorAdaptator, 3 /* dim */
+>;
+
+template <typename DATA>
+class Tree
+{
+    std::unique_ptr<KdTree> _tree;
+    std::unique_ptr<PointVectorAdaptator> _pointCloudRef;
+
+public:
+    Tree(const DATA& data) { initKdTree(data); }
+
+    void initKdTree(const DATA& data)
+    {
+        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
+    {
+        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()))
+        {
+            return false;
+        }
+        return true;
+    }
+};
+
+bool filterLandmarks(SfMData& sfmData)
+{
+    mvsUtils::MultiViewParams mp(sfmData, "", "", "", false);
+    std::vector<std::pair<double, IndexT>> landmarksPixSize(sfmData.getLandmarks().size());
+
+    #pragma omp parallel for
+    for(auto i = 0; i < sfmData.getLandmarks().size(); i++)
+    {
+        auto landmarkPair = sfmData.getLandmarks().begin();
+        std::advance(landmarkPair, i);
+        const sfmData::Landmark& landmark = landmarkPair->second;
+
+        // compute landmark pixSize
+        double pixSize = 0.;
+        int n = 0;
+        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()),
+                                          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);
+    }
+
+    // sort landmarks by descending pixSize order
+    std::stable_sort(landmarksPixSize.begin(), landmarksPixSize.end(), std::greater<>{});
+
+    //// take only best observations
+    //observationScores.resize(maxNbObservationsPerLandmark);
+
+
+
+    //// replace the observations
+    //Observations filteredObservations;
+    //for(auto observationScorePair : observationScores)
+    //{
+    //    filteredObservations[observationScorePair.second] = landmark.observations[observationScorePair.second];
+    //}
+    //landmark.observations = filteredObservations;
+    return true;
+}
+
+bool filterObservations(SfMData& sfmData, int maxNbObservationsPerLandmark)
 {
     #pragma omp parallel for
     for(auto i = 0; i < sfmData.getLandmarks().size(); i++)
@@ -134,7 +267,10 @@ int aliceVision_main(int argc, char *argv[])
     }
 
     // filter SfM data
-    if(filterSfMData(sfmData, maxNbObservationsPerLandmark))
+    bool success2 = filterLandmarks(sfmData);
+    bool success1 = filterObservations(sfmData, maxNbObservationsPerLandmark);
+
+    if(success1)
     {
         sfmDataIO::Save(sfmData, outputSfmFilename, sfmDataIO::ESfMData::ALL);
         return EXIT_SUCCESS;