From d76e1660f6f9757b581af6fcb0f56c51bdecb6c9 Mon Sep 17 00:00:00 2001 From: Abdelrahman AL MAROUK Date: Mon, 16 Oct 2023 16:48:57 +0200 Subject: [PATCH] [WIP][filterSfM] fix option to keep propagated observations - create a fake observation by projecting the landmark in the view --- src/software/pipeline/main_filterSfM.cpp | 16 ++++++++++------ 1 file changed, 10 insertions(+), 6 deletions(-) diff --git a/src/software/pipeline/main_filterSfM.cpp b/src/software/pipeline/main_filterSfM.cpp index 8252f3f9d4..be88b0bc93 100644 --- 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);