[WIP] fix KdTree search

a point is neighbor to itself if it participated in creating the tree

Author: almarouk

src/software/pipeline/main_filterSfM.cpp

@@ -368,9 +368,12 @@ bool filterObservations2(SfMData& sfmData, int maxNbObservationsPerLandmark, int
     {
         const sfmData::Landmark& landmark = landmarksData[i];
         auto& [indices_, weights_] = neighborsData[i];
-        indices_.resize(nbNeighbors);
-        weights_.resize(nbNeighbors);
-        tree.knnSearch(landmark.X.data(), nbNeighbors, &indices_[0], &weights_[0]);
+        // a landmark is a neighbor to itself with zero distance
+        indices_.resize(nbNeighbors + 1);
+        weights_.resize(nbNeighbors + 1);
+        tree.knnSearch(landmark.X.data(), nbNeighbors + 1, &indices_[0], &weights_[0]);
+        indices_.erase(indices_.begin());
+        weights_.erase(weights_.begin());
         double total = 0.;
         for(auto& w : weights_)
         {

src/software/pipeline/main_prepareDenseScene.cpp

@@ -358,7 +358,8 @@ bool prepareDenseScene(const SfMData& sfmData,
                     tree.buildIndex();
                     ALICEVISION_LOG_INFO("KdTree created for " << observations.size() << " points.");
 
-                    int n = std::min(nbNeighborObservations, static_cast<int>(observations.size()));
+                    int n = std::min(nbNeighborObservations, static_cast<int>(observations.size() - 1)) + 1;
+                    // note that the observation is a neighbor to itself with zero distance, hence the +/- 1
                     std::vector<double> means(observations.size());
                     const std::size_t cacheSize = 1000;
                     accumulator_set<double, stats<tag::tail_quantile<right>, tag::mean>> acc(
@@ -373,7 +374,7 @@ bool prepareDenseScene(const SfMData& sfmData,
 
                         std::transform(distances_.begin(), distances_.end(), distances_.begin(),
                                        static_cast<double (*)(double)>(std::sqrt));
-                        const auto& mean = std::accumulate(distances_.begin(), distances_.end(), 0.0) / n;
+                        const auto& mean = std::accumulate(distances_.begin(), distances_.end(), 0.0) / (n - 1);
                         means[j++] = mean; 
                         acc(mean);
                     }