[WIP][filterSfM] fix option to keep propagated observations

- create a fake observation by projecting the landmark in the view
alicevision · Mar 18, 2024 · 7aae1a0 · 7aae1a0
1 parent 982fa24
commit 7aae1a0
Showing 1 changed file with 10 additions and 6 deletions.
diff --git a/src/software/pipeline/main_filterSfM.cpp b/src/software/pipeline/main_filterSfM.cpp
@@ -926,13 +926,17 @@ bool filterObservations3D(SfMData& sfmData, const FilterParams::FilterObservatio
         {
             // add observation only if it's an original observation and not augmented
             const auto& viewId = viewIds[idx[j]];
-            if(params.observationsPropagationKeep)
-                filteredObservations[viewId] = landmark.observations[viewId];
-            else
+            const auto& obsIt = landmark.observations.find(viewId);
+            if(obsIt != landmark.observations.end())
+                filteredObservations[viewId] = obsIt->second;
+            else if (params.observationsPropagationKeep)
             {
-                const auto& obsIt = landmark.observations.find(viewId);
-                if(obsIt != landmark.observations.end())
-                    filteredObservations[viewId] = landmark.observations[viewId];
+                // project landmark in view to find observation coords
+                const sfmData::View* view = sfmData.getViews().at(viewId).get();
+                const geometry::Pose3 pose = sfmData.getPose(*view).getTransform();
+                const camera::IntrinsicBase* intrinsic = sfmData.getIntrinsics().at(view->getIntrinsicId()).get();
+                const Vec2& x = intrinsic->project(pose, landmark.X.homogeneous());
+                filteredObservations[viewId] = Observation(x, UndefinedIndexT, 0.0);
             }
         }
         landmark.observations = std::move(filteredObservations);