From 141a5534dc491b58e2d3d4c4ed70f6fbc5389353 Mon Sep 17 00:00:00 2001 From: VladislavZavadskyy Date: Fri, 18 Apr 2025 23:06:44 +0300 Subject: [PATCH 1/2] Changes related to new colmap point projection interface --- glomap/estimators/relpose_estimation.cc | 25 +++++++++++++++++-------- glomap/processors/image_undistorter.cc | 8 ++++++-- glomap/processors/track_filter.cc | 12 +++++++----- 3 files changed, 30 insertions(+), 15 deletions(-) diff --git a/glomap/estimators/relpose_estimation.cc b/glomap/estimators/relpose_estimation.cc index 8cd3b380..b3cee97f 100644 --- a/glomap/estimators/relpose_estimation.cc +++ b/glomap/estimators/relpose_estimation.cc @@ -54,12 +54,14 @@ void EstimateRelativePoses(ViewGraph& view_graph, // Collect the original 2D points points2D_1.clear(); points2D_2.clear(); - for (size_t idx = 0; idx < matches.rows(); idx++) { - points2D_1.push_back(image1.features[matches(idx, 0)]); - points2D_2.push_back(image2.features[matches(idx, 1)]); - } - // If the camera model is not supported by poselib - if (!valid_camera_model) { + + if (valid_camera_model) { + for (size_t idx = 0; idx < matches.rows(); idx++) { + points2D_1.push_back(image1.features[matches(idx, 0)]); + points2D_2.push_back(image2.features[matches(idx, 1)]); + } + } else { + // If the camera model is not supported by poselib // Undistort points // Note that here, we still rescale by the focal length (to avoid // change the RANSAC threshold) @@ -70,8 +72,15 @@ void EstimateRelativePoses(ViewGraph& view_graph, K2_new(0, 0) = camera2.FocalLengthX(); K2_new(1, 1) = camera2.FocalLengthY(); for (size_t idx = 0; idx < matches.rows(); idx++) { - points2D_1[idx] = K1_new * camera1.CamFromImg(points2D_1[idx]); - points2D_2[idx] = K2_new * camera2.CamFromImg(points2D_2[idx]); + std::optional p1 = + camera1.CamFromImg(image1.features[matches(idx, 0)]); + std::optional p2 = + camera1.CamFromImg(image2.features[matches(idx, 1)]); + + if (!p1 || !p2) continue; + + points2D_1.push_back(p1.value()); + points2D_2.push_back(p2.value()); } // Reset the camera to be the pinhole camera with original focal diff --git a/glomap/processors/image_undistorter.cc b/glomap/processors/image_undistorter.cc index 012465d1..05c375aa 100644 --- a/glomap/processors/image_undistorter.cc +++ b/glomap/processors/image_undistorter.cc @@ -31,8 +31,12 @@ void UndistortImages(std::unordered_map& cameras, image.features_undist.clear(); image.features_undist.reserve(num_points); for (int i = 0; i < num_points; i++) { - image.features_undist.emplace_back( - camera.CamFromImg(image.features[i]).homogeneous().normalized()); + std::optional feat_undist = + camera.CamFromImg(image.features[i]); + + if (feat_undist) image.features_undist.emplace_back( + feat_undist.value().homogeneous().normalized() + ); } }); } diff --git a/glomap/processors/track_filter.cc b/glomap/processors/track_filter.cc index c3a78f7d..a2821c2a 100644 --- a/glomap/processors/track_filter.cc +++ b/glomap/processors/track_filter.cc @@ -17,7 +17,7 @@ int TrackFilter::FilterTracksByReprojection( for (auto& [image_id, feature_id] : track.observations) { const Image& image = images.at(image_id); Eigen::Vector3d pt_calc = image.cam_from_world * track.xyz; - if (pt_calc(2) < EPS) continue; + if (pt_calc(2) < std::numeric_limits::epsilon()) continue; double reprojection_error = max_reprojection_error; if (in_normalized_image) { @@ -29,10 +29,12 @@ int TrackFilter::FilterTracksByReprojection( (pt_reproj - feature_undist.head(2) / (feature_undist(2) + EPS)) .norm(); } else { - Eigen::Vector2d pt_reproj = pt_calc.head(2) / pt_calc(2); - Eigen::Vector2d pt_dist; - pt_dist = cameras.at(image.camera_id).ImgFromCam(pt_reproj); - reprojection_error = (pt_dist - image.features.at(feature_id)).norm(); + // The epsilon-depth check should be enough, + // but leaving this in case the implementation changes + std::optional pt_dist = + cameras.at(image.camera_id).ImgFromCam(pt_calc); + if (!pt_dist) continue; + reprojection_error = (pt_dist.value() - image.features.at(feature_id)).norm(); } // If the reprojection error is smaller than the threshold, then keep it From ae41c92d0c1e0583339af6649c99849e28ee9b93 Mon Sep 17 00:00:00 2001 From: VladislavZavadskyy Date: Sat, 19 Apr 2025 10:19:23 +0300 Subject: [PATCH 2/2] Fixed interface inconsistencies with the dev branch of poselib --- glomap/controllers/option_manager.cc | 2 +- glomap/estimators/relpose_estimation.cc | 4 ++-- glomap/estimators/relpose_estimation.h | 7 ++++--- glomap/scene/camera.h | 2 +- 4 files changed, 8 insertions(+), 7 deletions(-) diff --git a/glomap/controllers/option_manager.cc b/glomap/controllers/option_manager.cc index 3a7c8ff4..abaea04e 100644 --- a/glomap/controllers/option_manager.cc +++ b/glomap/controllers/option_manager.cc @@ -151,7 +151,7 @@ void OptionManager::AddRelativePoseEstimationOptions() { added_relative_pose_options_ = true; AddAndRegisterDefaultOption( "RelPoseEstimation.max_epipolar_error", - &mapper->opt_relpose.ransac_options.max_epipolar_error); + &mapper->opt_relpose.relpose_options.max_error); } void OptionManager::AddRotationEstimatorOptions() { diff --git a/glomap/estimators/relpose_estimation.cc b/glomap/estimators/relpose_estimation.cc index b3cee97f..84e652c3 100644 --- a/glomap/estimators/relpose_estimation.cc +++ b/glomap/estimators/relpose_estimation.cc @@ -98,13 +98,13 @@ void EstimateRelativePoses(ViewGraph& view_graph, } inliers.clear(); poselib::CameraPose pose_rel_calc; + try { poselib::estimate_relative_pose(points2D_1, points2D_2, camera_poselib1, camera_poselib2, - options.ransac_options, - options.bundle_options, + options.relpose_options, &pose_rel_calc, &inliers); } catch (const std::exception& e) { diff --git a/glomap/estimators/relpose_estimation.h b/glomap/estimators/relpose_estimation.h index 9f219c9f..98c0bbf8 100644 --- a/glomap/estimators/relpose_estimation.h +++ b/glomap/estimators/relpose_estimation.h @@ -8,10 +8,11 @@ namespace glomap { struct RelativePoseEstimationOptions { // Options for poselib solver - poselib::RansacOptions ransac_options; - poselib::BundleOptions bundle_options; + poselib::RelativePoseOptions relpose_options; - RelativePoseEstimationOptions() { ransac_options.max_iterations = 50000; } + RelativePoseEstimationOptions() { + relpose_options.ransac.max_iterations = 50000; + } }; void EstimateRelativePoses(ViewGraph& view_graph, diff --git a/glomap/scene/camera.h b/glomap/scene/camera.h index eeaf883f..38a14aa4 100644 --- a/glomap/scene/camera.h +++ b/glomap/scene/camera.h @@ -5,7 +5,7 @@ #include #include -#include +#include namespace glomap {