From a1279c5da0825b78ebaf6e34ecf6d9854f4b8bf5 Mon Sep 17 00:00:00 2001 From: mariusud Date: Mon, 13 May 2024 14:55:55 +0200 Subject: [PATCH 1/4] add outlier removal --- .../bundle_adjustment_in_the_large.cc | 60 ++++++++++++++++++- 1 file changed, 59 insertions(+), 1 deletion(-) diff --git a/symforce/examples/bundle_adjustment_in_the_large/bundle_adjustment_in_the_large.cc b/symforce/examples/bundle_adjustment_in_the_large/bundle_adjustment_in_the_large.cc index b3b07799b..b57c9173f 100644 --- a/symforce/examples/bundle_adjustment_in_the_large/bundle_adjustment_in_the_large.cc +++ b/symforce/examples/bundle_adjustment_in_the_large/bundle_adjustment_in_the_large.cc @@ -3,7 +3,9 @@ * This source code is under the Apache 2.0 license found in the LICENSE file. * ---------------------------------------------------------------------------- */ +#include #include +#include #include @@ -15,6 +17,60 @@ #include "./gen/keys.h" #include "./gen/snavely_reprojection_factor.h" +/** + * Calculate median position + */ +Eigen::Vector3d CalculateMedianPosition(const sym::Valuesd& values, int num_cameras) { + if (num_cameras == 0) { + return Eigen::Vector3d::Zero(); + } + + std::vector positions; + positions.reserve(num_cameras); + for (int i = 0; i < num_cameras; i++) { + positions.push_back(values.At(sym::Keys::CAM_T_WORLD.WithSuper(i)).Position()); + } + + std::nth_element( + positions.begin(), positions.begin() + positions.size() / 2, positions.end(), + [](const Eigen::Vector3d& a, const Eigen::Vector3d& b) { return a.norm() < b.norm(); }); + + return positions[positions.size() / 2]; +} + +/** + * Remove cameras that are far from the median + * See https://github.com/symforce-org/symforce/issues/387 + */ +void FilterCameraOutliers(std::vector& factors, sym::Valuesd& values, + int& num_cameras, double threshold = 100.0) { + std::vector camera_poses; + std::vector filtered_factors; + + Eigen::Vector3d mean = CalculateMedianPosition(values, num_cameras); + + int num_filtered_cameras = 0; + for (int i = 0; i < num_cameras; i++) { + sym::Pose3d pose = values.At(sym::Keys::CAM_T_WORLD.WithSuper(i)); + double distance = (pose.Position() - mean).norm(); + + if (distance <= threshold) { + values.Set(sym::Keys::CAM_T_WORLD.WithSuper(num_filtered_cameras), pose); + values.Set(sym::Keys::INTRINSICS.WithSuper(num_filtered_cameras), + values.At(sym::Keys::INTRINSICS.WithSuper(i))); + + filtered_factors.push_back(factors[i]); + + num_filtered_cameras++; + } else { + spdlog::info("Removing camera outlier: {}", i); + } + } + + num_cameras = num_filtered_cameras; + factors = std::move(filtered_factors); +} + using namespace sym::Keys; /** @@ -109,6 +165,8 @@ Problem ReadProblem(const std::string& filename) { values.Set(EPSILON, sym::kDefaultEpsilond); + FilterCameraOutliers(factors, values, num_cameras); + return {std::move(factors), std::move(values), num_cameras, num_points, num_observations}; } @@ -128,7 +186,7 @@ int main(int argc, char** argv) { auto params = sym::DefaultOptimizerParams(); params.verbose = true; params.lambda_update_type = sym::lambda_update_type_t::DYNAMIC; - sym::Optimizerd optimizer{params, std::move(problem.factors)}; + k sym::Optimizerd optimizer{params, std::move(problem.factors)}; const auto stats = optimizer.Optimize(optimized_values); spdlog::info("Finished in {} iterations", stats.iterations.size()); From c9acf0b99ee84f71af9a03f5bb348d974f5017a5 Mon Sep 17 00:00:00 2001 From: mariusud Date: Mon, 13 May 2024 16:10:48 +0200 Subject: [PATCH 2/4] typo --- .../bundle_adjustment_in_the_large.cc | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/symforce/examples/bundle_adjustment_in_the_large/bundle_adjustment_in_the_large.cc b/symforce/examples/bundle_adjustment_in_the_large/bundle_adjustment_in_the_large.cc index b57c9173f..1f7fafbca 100644 --- a/symforce/examples/bundle_adjustment_in_the_large/bundle_adjustment_in_the_large.cc +++ b/symforce/examples/bundle_adjustment_in_the_large/bundle_adjustment_in_the_large.cc @@ -62,8 +62,6 @@ void FilterCameraOutliers(std::vector& factors, sym::Valuesd& valu filtered_factors.push_back(factors[i]); num_filtered_cameras++; - } else { - spdlog::info("Removing camera outlier: {}", i); } } @@ -186,7 +184,7 @@ int main(int argc, char** argv) { auto params = sym::DefaultOptimizerParams(); params.verbose = true; params.lambda_update_type = sym::lambda_update_type_t::DYNAMIC; - k sym::Optimizerd optimizer{params, std::move(problem.factors)}; + sym::Optimizerd optimizer{params, std::move(problem.factors)}; const auto stats = optimizer.Optimize(optimized_values); spdlog::info("Finished in {} iterations", stats.iterations.size()); From 18de060e9ef5656fffb1835f795e5db161ac1a14 Mon Sep 17 00:00:00 2001 From: mariusud Date: Tue, 14 May 2024 10:35:12 +0200 Subject: [PATCH 3/4] lower threshold --- .../bundle_adjustment_in_the_large.cc | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/symforce/examples/bundle_adjustment_in_the_large/bundle_adjustment_in_the_large.cc b/symforce/examples/bundle_adjustment_in_the_large/bundle_adjustment_in_the_large.cc index 1f7fafbca..c79b77360 100644 --- a/symforce/examples/bundle_adjustment_in_the_large/bundle_adjustment_in_the_large.cc +++ b/symforce/examples/bundle_adjustment_in_the_large/bundle_adjustment_in_the_large.cc @@ -43,7 +43,7 @@ Eigen::Vector3d CalculateMedianPosition(const sym::Valuesd& values, int num_came * See https://github.com/symforce-org/symforce/issues/387 */ void FilterCameraOutliers(std::vector& factors, sym::Valuesd& values, - int& num_cameras, double threshold = 100.0) { + int& num_cameras, double threshold = 10.0) { std::vector camera_poses; std::vector filtered_factors; @@ -62,6 +62,8 @@ void FilterCameraOutliers(std::vector& factors, sym::Valuesd& valu filtered_factors.push_back(factors[i]); num_filtered_cameras++; + } else { + spdlog::info("Filtering camera {} with distance {}", i, distance); } } From c2cb6013626a5696a8d0784c1f8853e9191d496d Mon Sep 17 00:00:00 2001 From: mariusud Date: Tue, 14 May 2024 10:42:39 +0200 Subject: [PATCH 4/4] remove log --- .../bundle_adjustment_in_the_large.cc | 2 -- 1 file changed, 2 deletions(-) diff --git a/symforce/examples/bundle_adjustment_in_the_large/bundle_adjustment_in_the_large.cc b/symforce/examples/bundle_adjustment_in_the_large/bundle_adjustment_in_the_large.cc index c79b77360..87db3248b 100644 --- a/symforce/examples/bundle_adjustment_in_the_large/bundle_adjustment_in_the_large.cc +++ b/symforce/examples/bundle_adjustment_in_the_large/bundle_adjustment_in_the_large.cc @@ -62,8 +62,6 @@ void FilterCameraOutliers(std::vector& factors, sym::Valuesd& valu filtered_factors.push_back(factors[i]); num_filtered_cameras++; - } else { - spdlog::info("Filtering camera {} with distance {}", i, distance); } }