From b7ff339a7110a9a7c47c09969981beedf3833c8e Mon Sep 17 00:00:00 2001
From: Abdelrahman AL MAROUK <abdelrahman.almarouk@gmail.com>
Date: Tue, 12 Sep 2023 17:07:22 +0200
Subject: [PATCH] [WIP] fix KdTree search

a point is neighbor to itself if it participated in creating the tree
---
 src/software/pipeline/main_filterSfM.cpp         | 9 ++++++---
 src/software/pipeline/main_prepareDenseScene.cpp | 5 +++--
 2 files changed, 9 insertions(+), 5 deletions(-)

diff --git a/src/software/pipeline/main_filterSfM.cpp b/src/software/pipeline/main_filterSfM.cpp
index 58cc4a8774..5fe1fe5f80 100644
--- a/src/software/pipeline/main_filterSfM.cpp
+++ b/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_)
         {
diff --git a/src/software/pipeline/main_prepareDenseScene.cpp b/src/software/pipeline/main_prepareDenseScene.cpp
index 267df62fe6..8e08f317b4 100644
--- a/src/software/pipeline/main_prepareDenseScene.cpp
+++ b/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);
                     }