From b6567fead078eed79bdd0732b68968f4e54d4e19 Mon Sep 17 00:00:00 2001 From: Yifan Yu Date: Fri, 31 Jan 2025 05:03:17 +0100 Subject: [PATCH 1/6] Make utils functions templated --- src/hybrid_pose_estimator.cpp | 10 ++-- src/hybrid_pose_estimator.h | 7 +++ src/hybrid_pose_shared_focal_estimator.cpp | 2 +- src/hybrid_pose_two_focal_estimator.cpp | 2 +- src/utils.h | 54 ++++++++++++---------- 5 files changed, 42 insertions(+), 33 deletions(-) diff --git a/src/hybrid_pose_estimator.cpp b/src/hybrid_pose_estimator.cpp index a4aeb07..ce33371 100644 --- a/src/hybrid_pose_estimator.cpp +++ b/src/hybrid_pose_estimator.cpp @@ -251,9 +251,8 @@ double HybridPoseEstimator::EvaluateModelOnPoint(const PoseScaleOffset &model, i Eigen::Matrix3d E = to_essential_matrix(model.R(), model.t()); - double sampson_error = compute_sampson_error(x0_calib.head<2>(), x1_calib.head<2>(), E); - double loss_scale = 1.0 / (K0_(0, 0) + K0_(1, 1)) + 1.0 / (K1_(0, 0) + K1_(1, 1)); - return sampson_error / std::pow(loss_scale, 2); + double sampson_error = compute_sampson_error(x0_calib.head<2>(), x1_calib.head<2>(), E); + return sampson_error * sampson_loss_scale_; } } @@ -407,9 +406,8 @@ double HybridPoseEstimatorScaleOnly::EvaluateModelOnPoint(const PoseAndScale &mo Eigen::Matrix3d E = to_essential_matrix(model.R(), model.t()); - double sampson_error = compute_sampson_error(x0_calib.head<2>(), x1_calib.head<2>(), E); - double loss_scale = 1.0 / (K0_(0, 0) + K0_(1, 1)) + 1.0 / (K1_(0, 0) + K1_(1, 1)); - return sampson_error / std::pow(loss_scale, 2); + double sampson_error = compute_sampson_error(x0_calib.head<2>(), x1_calib.head<2>(), E); + return sampson_error * sampson_loss_scale_; } } diff --git a/src/hybrid_pose_estimator.h b/src/hybrid_pose_estimator.h index 9807fa0..ee46069 100644 --- a/src/hybrid_pose_estimator.h +++ b/src/hybrid_pose_estimator.h @@ -31,6 +31,9 @@ class HybridPoseEstimator { x0_.col(i) = x0[i].homogeneous(); x1_.col(i) = x1[i].homogeneous(); } + + sampson_loss_scale_ = 1.0 / (K0_(0, 0) + K0_(1, 1)) + 1.0 / (K1_(0, 0) + K1_(1, 1)); + sampson_loss_scale_ = 1.0 / std::pow(sampson_loss_scale_, 2); } ~HybridPoseEstimator() {} @@ -88,6 +91,7 @@ class HybridPoseEstimator { Eigen::VectorXd d0_, d1_; Eigen::Vector2d min_depth_; double sampson_squared_weight_; + double sampson_loss_scale_; EstimatorConfig est_config_; std::vector squared_inlier_thresholds_; @@ -114,6 +118,9 @@ class HybridPoseEstimatorScaleOnly { x0_.col(i) = x0[i].homogeneous(); x1_.col(i) = x1[i].homogeneous(); } + + sampson_loss_scale_ = 1.0 / (K0_(0, 0) + K0_(1, 1)) + 1.0 / (K1_(0, 0) + K1_(1, 1)); + sampson_loss_scale_ = 1.0 / std::pow(sampson_loss_scale_, 2); } ~HybridPoseEstimatorScaleOnly() {} diff --git a/src/hybrid_pose_shared_focal_estimator.cpp b/src/hybrid_pose_shared_focal_estimator.cpp index 9748eeb..eba6506 100644 --- a/src/hybrid_pose_shared_focal_estimator.cpp +++ b/src/hybrid_pose_shared_focal_estimator.cpp @@ -189,7 +189,7 @@ double HybridSharedFocalPoseEstimator::EvaluateModelOnPoint(const PoseScaleOffse Eigen::Matrix3d E = to_essential_matrix(model.R(), model.t()); Eigen::Matrix3d F = K_inv.transpose() * E * K_inv; - double sampson_error = compute_sampson_error(x0_norm_.col(i).head<2>(), x1_norm_.col(i).head<2>(), F); + double sampson_error = compute_sampson_error(x0_norm_.col(i).head<2>(), x1_norm_.col(i).head<2>(), F); return sampson_error; } } diff --git a/src/hybrid_pose_two_focal_estimator.cpp b/src/hybrid_pose_two_focal_estimator.cpp index be8a91a..522112b 100644 --- a/src/hybrid_pose_two_focal_estimator.cpp +++ b/src/hybrid_pose_two_focal_estimator.cpp @@ -241,7 +241,7 @@ double HybridTwoFocalPoseEstimator::EvaluateModelOnPoint(const PoseScaleOffsetTw Eigen::Matrix3d E = to_essential_matrix(model.R(), model.t()); Eigen::Matrix3d F = K1_inv.transpose() * E * K0_inv; - double sampson_error = compute_sampson_error(x0_norm_.col(i).head<2>(), x1_norm_.col(i).head<2>(), F); + double sampson_error = compute_sampson_error(x0_norm_.col(i).head<2>(), x1_norm_.col(i).head<2>(), F); return sampson_error; } } diff --git a/src/utils.h b/src/utils.h index b7fdc32..6a02ace 100644 --- a/src/utils.h +++ b/src/utils.h @@ -21,28 +21,29 @@ namespace py = pybind11; namespace madpose { // -------- Triangulation functions from COLMAP 3.9 -------- -inline Eigen::Vector3d TriangulatePoint(const Eigen::Matrix3x4d &cam1_from_world, - const Eigen::Matrix3x4d &cam2_from_world, const Eigen::Vector2d &point1, - const Eigen::Vector2d &point2) { - Eigen::Matrix4d A; +template +inline Eigen::Vector TriangulatePoint(const Eigen::Matrix &cam1_from_world, + const Eigen::Matrix &cam2_from_world, + const Eigen::Vector &point1, const Eigen::Vector &point2) { + Eigen::Matrix A; A.row(0) = point1(0) * cam1_from_world.row(2) - cam1_from_world.row(0); A.row(1) = point1(1) * cam1_from_world.row(2) - cam1_from_world.row(1); A.row(2) = point2(0) * cam2_from_world.row(2) - cam2_from_world.row(0); A.row(3) = point2(1) * cam2_from_world.row(2) - cam2_from_world.row(1); - Eigen::JacobiSVD svd(A, Eigen::ComputeFullV); + Eigen::JacobiSVD> svd(A, Eigen::ComputeFullV); return svd.matrixV().col(3).hnormalized(); } -inline std::vector TriangulatePoints(const Eigen::Matrix3x4d &cam1_from_world, - const Eigen::Matrix3x4d &cam2_from_world, - const std::vector &points1, - const std::vector &points2) { +template +inline std::vector> +TriangulatePoints(const Eigen::Matrix &cam1_from_world, const Eigen::Matrix &cam2_from_world, + const std::vector> &points1, const std::vector> &points2) { CHECK_EQ(points1.size(), points2.size()); - std::vector points3D(points1.size()); + std::vector> points3D(points1.size()); for (size_t i = 0; i < points3D.size(); ++i) { points3D[i] = TriangulatePoint(cam1_from_world, cam2_from_world, points1[i], points2[i]); @@ -52,28 +53,31 @@ inline std::vector TriangulatePoints(const Eigen::Matrix3x4d &c } // --------------------------------------------------------- -inline Eigen::Matrix3d to_essential_matrix(Eigen::Matrix3d R, Eigen::Vector3d t) { - Eigen::Matrix3d E; +template +inline Eigen::Matrix to_essential_matrix(Eigen::Matrix R, Eigen::Vector t) { + Eigen::Matrix E; E << 0.0, -t(2), t(1), t(2), 0.0, -t(0), -t(1), t(0), 0.0; E = E * R; return E; } -inline double compute_sampson_error(const Eigen::Vector2d &x1, const Eigen::Vector2d &x2, const Eigen::Matrix3d &E) { +template +inline T compute_sampson_error(const Eigen::Vector &x1, const Eigen::Vector &x2, + const Eigen::Matrix &E) { // For some reason this is a lot faster than just using nice Eigen // expressions... - const double Ex1_0 = E(0, 0) * x1(0) + E(0, 1) * x1(1) + E(0, 2); - const double Ex1_1 = E(1, 0) * x1(0) + E(1, 1) * x1(1) + E(1, 2); - const double Ex1_2 = E(2, 0) * x1(0) + E(2, 1) * x1(1) + E(2, 2); - - const double Ex2_0 = E(0, 0) * x2(0) + E(1, 0) * x2(1) + E(2, 0); - const double Ex2_1 = E(0, 1) * x2(0) + E(1, 1) * x2(1) + E(2, 1); - // const double Ex2_2 = E(0, 2) * x2(0) + E(1, 2) * x2(1) + E(2, 2); - - const double C = x2(0) * Ex1_0 + x2(1) * Ex1_1 + Ex1_2; - const double Cx = Ex1_0 * Ex1_0 + Ex1_1 * Ex1_1; - const double Cy = Ex2_0 * Ex2_0 + Ex2_1 * Ex2_1; - const double r2 = C * C / (Cx + Cy); + const T Ex1_0 = E(0, 0) * x1(0) + E(0, 1) * x1(1) + E(0, 2); + const T Ex1_1 = E(1, 0) * x1(0) + E(1, 1) * x1(1) + E(1, 2); + const T Ex1_2 = E(2, 0) * x1(0) + E(2, 1) * x1(1) + E(2, 2); + + const T Ex2_0 = E(0, 0) * x2(0) + E(1, 0) * x2(1) + E(2, 0); + const T Ex2_1 = E(0, 1) * x2(0) + E(1, 1) * x2(1) + E(2, 1); + // const T Ex2_2 = E(0, 2) * x2(0) + E(1, 2) * x2(1) + E(2, 2); + + const T C = x2(0) * Ex1_0 + x2(1) * Ex1_1 + Ex1_2; + const T Cx = Ex1_0 * Ex1_0 + Ex1_1 * Ex1_1; + const T Cy = Ex2_0 * Ex2_0 + Ex2_1 * Ex2_1; + const T r2 = C * C / (Cx + Cy); return r2; } From 69e2f4f572ff52799b4191e49873550fef2dac5f Mon Sep 17 00:00:00 2001 From: Yifan Yu Date: Wed, 5 Feb 2025 06:26:37 +0100 Subject: [PATCH 2/6] Better implementation of cost functions for numerical stability and speed --- src/cost_functions.h | 95 +++++++++++++++++++++++++++----------------- src/optimizer.h | 19 ++++----- 2 files changed, 69 insertions(+), 45 deletions(-) diff --git a/src/cost_functions.h b/src/cost_functions.h index defdd7a..caeaf44 100644 --- a/src/cost_functions.h +++ b/src/cost_functions.h @@ -3,6 +3,8 @@ #include "pose.h" #include "utils.h" +#include + namespace madpose { // ******************************************************************* @@ -25,11 +27,14 @@ struct LiftProjectionFunctor0 { template bool operator()(const T *const o0, const T *const qvec, const T *const tvec, T *residuals) const { Eigen::Vector x3d = x0_calib_.cast() * (x0_depth_ + o0[0]); - Eigen::Matrix R = QuaternionToRotationMatrix(Eigen::Map>(qvec)); + // Eigen::Matrix R = QuaternionToRotationMatrix(Eigen::Map>(qvec)); Eigen::Vector t = Eigen::Map>(tvec); - Eigen::Vector x3d_1 = R * x3d + t; - Eigen::Vector x1_hat = K1_ * x3d_1; + // Eigen::Vector x3d_1 = R * x3d + t; + Eigen::Vector x3d_1; + ceres::QuaternionRotatePoint(qvec, x3d.data(), x3d_1.data()); + + Eigen::Vector x1_hat = K1_ * (x3d_1 + t); x1_hat /= x1_hat[2]; residuals[0] = x1_hat[0] - x1_[0]; residuals[1] = x1_hat[1] - x1_[1]; @@ -58,10 +63,16 @@ struct LiftProjectionFunctor1 { bool operator()(const T *const scale, const T *const o1, const T *const qvec, const T *const tvec, T *residuals) const { Eigen::Vector x3d = x1_calib_.cast() * (x1_depth_ + o1[0]) * scale[0]; - Eigen::Matrix R = QuaternionToRotationMatrix(Eigen::Map>(qvec)); + // Eigen::Matrix R = QuaternionToRotationMatrix(Eigen::Map>(qvec)); Eigen::Vector t = Eigen::Map>(tvec); - Eigen::Vector x3d_0 = R.transpose() * x3d - R.transpose() * t; + Eigen::Vector quat = NormalizeQuaternion(Eigen::Map>(qvec)); + Eigen::Vector qvec_inv = {quat[0], -quat[1], -quat[2], -quat[3]}; + Eigen::Vector x3d_1 = x3d - t; + Eigen::Vector x3d_0; + ceres::UnitQuaternionRotatePoint(qvec_inv.data(), x3d_1.data(), x3d_0.data()); + + // Eigen::Vector x3d_0 = R.transpose() * (x3d - t); Eigen::Vector x0_hat = K0_ * x3d_0; x0_hat /= x0_hat[2]; residuals[0] = x0_hat[0] - x0_[0]; @@ -78,15 +89,12 @@ struct LiftProjectionFunctor1 { struct SampsonErrorFunctor { public: - SampsonErrorFunctor(const Eigen::Vector3d &x0, const Eigen::Vector3d &x1, const Eigen::Matrix3d &K0, - const Eigen::Matrix3d &K1, const double &sq_weight = 1.0) - : x0_(x0), x1_(x1), K0_(K0), K1_(K1), K0_inv_(K0.inverse()), K1_inv_(K1.inverse()), - weight_(std::sqrt(sq_weight)) {} - - static ceres::CostFunction *Create(const Eigen::Vector3d &x0, const Eigen::Vector3d &x1, const Eigen::Matrix3d &K0, - const Eigen::Matrix3d &K1, const double &sq_weight = 1.0) { - return (new ceres::AutoDiffCostFunction( - new SampsonErrorFunctor(x0, x1, K0, K1, sq_weight))); + SampsonErrorFunctor(const Eigen::Vector3d &x0, const Eigen::Vector3d &x1, const double &weight = 1.0) + : x0_(x0), x1_(x1), weight_(weight) {} + + static ceres::CostFunction *Create(const Eigen::Vector3d &x0, const Eigen::Vector3d &x1, + const double &weight = 1.0) { + return (new ceres::AutoDiffCostFunction(new SampsonErrorFunctor(x0, x1, weight))); } template bool operator()(const T *const qvec, const T *const tvec, T *residuals) const { @@ -111,15 +119,12 @@ struct SampsonErrorFunctor { const T C = x2(0) * Ex1_0 + x2(1) * Ex1_1 + Ex1_2; const T r = C / Eigen::Vector(Ex1_0, Ex1_1, Ex2_0, Ex2_1).norm(); - residuals[0] = r / (1.0 / (K0_(0, 0) + K0_(1, 1)) + 1.0 / (K1_(0, 0) + K1_(1, 1))); - residuals[0] *= weight_; + residuals[0] = r * weight_; return true; } private: const Eigen::Vector3d x0_, x1_; - const Eigen::Matrix3d K0_, K1_; - const Eigen::Matrix3d K0_inv_, K1_inv_; const double weight_; }; @@ -145,11 +150,14 @@ struct LiftProjectionSharedFocalFunctor0 { K << focal[0], T(0.0), T(0.0), T(0.0), focal[0], T(0.0), T(0.0), T(0.0), T(1.0); K_inv << T(1.0) / focal[0], T(0.0), T(0.0), T(0.0), T(1.0) / focal[0], T(0.0), T(0.0), T(0.0), T(1.0); Eigen::Vector x3d = (K_inv * x0_.cast()) * (x0_depth_ + o0[0]); - Eigen::Matrix R = QuaternionToRotationMatrix(Eigen::Map>(qvec)); + // Eigen::Matrix R = QuaternionToRotationMatrix(Eigen::Map>(qvec)); Eigen::Vector t = Eigen::Map>(tvec); - Eigen::Vector x3d_1 = R * x3d + t; - Eigen::Vector x1_hat = K * x3d_1; + // Eigen::Vector x3d_1 = R * x3d + t; + Eigen::Vector x3d_1; + ceres::QuaternionRotatePoint(qvec, x3d.data(), x3d_1.data()); + + Eigen::Vector x1_hat = K * (x3d_1 + t); x1_hat /= x1_hat[2]; residuals[0] = x1_hat[0] - x1_[0]; residuals[1] = x1_hat[1] - x1_[1]; @@ -178,10 +186,16 @@ struct LiftProjectionSharedFocalFunctor1 { K << focal[0], T(0.0), T(0.0), T(0.0), focal[0], T(0.0), T(0.0), T(0.0), T(1.0); K_inv << T(1.0) / focal[0], T(0.0), T(0.0), T(0.0), T(1.0) / focal[0], T(0.0), T(0.0), T(0.0), T(1.0); Eigen::Vector x3d = (K_inv * x1_.cast()) * (x1_depth_ + o1[0]) * scale[0]; - Eigen::Matrix R = QuaternionToRotationMatrix(Eigen::Map>(qvec)); + // Eigen::Matrix R = QuaternionToRotationMatrix(Eigen::Map>(qvec)); Eigen::Vector t = Eigen::Map>(tvec); - Eigen::Vector x3d_0 = R.transpose() * x3d - R.transpose() * t; + Eigen::Vector quat = NormalizeQuaternion(Eigen::Map>(qvec)); + Eigen::Vector qvec_inv = {quat[0], -quat[1], -quat[2], -quat[3]}; + Eigen::Vector x3d_1 = x3d - t; + Eigen::Vector x3d_0; + ceres::UnitQuaternionRotatePoint(qvec_inv.data(), x3d_1.data(), x3d_0.data()); + + // Eigen::Vector x3d_0 = R.transpose() * (x3d - t); Eigen::Vector x0_hat = K * x3d_0; x0_hat /= x0_hat[2]; residuals[0] = x0_hat[0] - x0_[0]; @@ -197,13 +211,13 @@ struct LiftProjectionSharedFocalFunctor1 { struct SampsonErrorSharedFocalFunctor { public: - SampsonErrorSharedFocalFunctor(const Eigen::Vector3d &x0, const Eigen::Vector3d &x1, const double &sq_weight = 1.0) - : x0_(x0), x1_(x1), weight_(std::sqrt(sq_weight)) {} + SampsonErrorSharedFocalFunctor(const Eigen::Vector3d &x0, const Eigen::Vector3d &x1, const double &weight = 1.0) + : x0_(x0), x1_(x1), weight_(weight) {} static ceres::CostFunction *Create(const Eigen::Vector3d &x0, const Eigen::Vector3d &x1, - const double &sq_weight = 1.0) { + const double &weight = 1.0) { return (new ceres::AutoDiffCostFunction( - new SampsonErrorSharedFocalFunctor(x0, x1, sq_weight))); + new SampsonErrorSharedFocalFunctor(x0, x1, weight))); } template @@ -263,11 +277,14 @@ struct LiftProjectionTwoFocalFunctor0 { K0_inv << T(1.0) / focal0[0], T(0.0), T(0.0), T(0.0), T(1.0) / focal0[0], T(0.0), T(0.0), T(0.0), T(1.0); K1 << focal1[0], T(0.0), T(0.0), T(0.0), focal1[0], T(0.0), T(0.0), T(0.0), T(1.0); Eigen::Vector x3d = (K0_inv * x0_.cast()) * (x0_depth_ + o0[0]); - Eigen::Matrix R = QuaternionToRotationMatrix(Eigen::Map>(qvec)); + // Eigen::Matrix R = QuaternionToRotationMatrix(Eigen::Map>(qvec)); Eigen::Vector t = Eigen::Map>(tvec); - Eigen::Vector x3d_1 = R * x3d + t; - Eigen::Vector x1_hat = K1 * x3d_1; + Eigen::Vector x3d_1; + ceres::QuaternionRotatePoint(qvec, x3d.data(), x3d_1.data()); + + // Eigen::Vector x3d_1 = R * x3d + t; + Eigen::Vector x1_hat = K1 * (x3d_1 + t); x1_hat /= x1_hat[2]; residuals[0] = x1_hat[0] - x1_[0]; residuals[1] = x1_hat[1] - x1_[1]; @@ -296,10 +313,16 @@ struct LiftProjectionTwoFocalFunctor1 { K0 << focal0[0], T(0.0), T(0.0), T(0.0), focal0[0], T(0.0), T(0.0), T(0.0), T(1.0); K1_inv << T(1.0) / focal1[0], T(0.0), T(0.0), T(0.0), T(1.0) / focal1[0], T(0.0), T(0.0), T(0.0), T(1.0); Eigen::Vector x3d = (K1_inv * x1_.cast()) * (x1_depth_ + o1[0]) * scale[0]; - Eigen::Matrix R = QuaternionToRotationMatrix(Eigen::Map>(qvec)); + // Eigen::Matrix R = QuaternionToRotationMatrix(Eigen::Map>(qvec)); Eigen::Vector t = Eigen::Map>(tvec); - Eigen::Vector x3d_0 = R.transpose() * x3d - R.transpose() * t; + Eigen::Vector quat = NormalizeQuaternion(Eigen::Map>(qvec)); + Eigen::Vector qvec_inv = {quat[0], -quat[1], -quat[2], -quat[3]}; + Eigen::Vector x3d_1 = x3d - t; + Eigen::Vector x3d_0; + ceres::UnitQuaternionRotatePoint(qvec_inv.data(), x3d_1.data(), x3d_0.data()); + + // Eigen::Vector x3d_0 = R.transpose() * (x3d - t); Eigen::Vector x0_hat = K0 * x3d_0; x0_hat /= x0_hat[2]; residuals[0] = x0_hat[0] - x0_[0]; @@ -315,13 +338,13 @@ struct LiftProjectionTwoFocalFunctor1 { struct SampsonErrorTwoFocalFunctor { public: - SampsonErrorTwoFocalFunctor(const Eigen::Vector3d &x0, const Eigen::Vector3d &x1, const double &sq_weight = 1.0) - : x0_(x0), x1_(x1), weight_(std::sqrt(sq_weight)) {} + SampsonErrorTwoFocalFunctor(const Eigen::Vector3d &x0, const Eigen::Vector3d &x1, const double &weight = 1.0) + : x0_(x0), x1_(x1), weight_(weight) {} static ceres::CostFunction *Create(const Eigen::Vector3d &x0, const Eigen::Vector3d &x1, - const double &sq_weight = 1.0) { + const double &weight = 1.0) { return (new ceres::AutoDiffCostFunction( - new SampsonErrorTwoFocalFunctor(x0, x1, sq_weight))); + new SampsonErrorTwoFocalFunctor(x0, x1, weight))); } template diff --git a/src/optimizer.h b/src/optimizer.h index 4ddb279..e7d424b 100644 --- a/src/optimizer.h +++ b/src/optimizer.h @@ -69,8 +69,7 @@ class HybridPoseOptimizer { for (auto &i : indices_sampson_) { Eigen::Vector3d x0 = K0_inv_ * x0_.col(i); Eigen::Vector3d x1 = K1_inv_ * x1_.col(i); - ceres::CostFunction *sampson_cost = - SampsonErrorFunctor::Create(x0, x1, K0_, K1_, config_.weight_sampson); + ceres::CostFunction *sampson_cost = SampsonErrorFunctor::Create(x0, x1, weight); problem_->AddResidualBlock(sampson_cost, sampson_loss_func, qvec_.data(), tvec_.data()); } } @@ -189,11 +188,12 @@ class HybridPoseOptimizerScaleOnly { } if (config_.use_sampson) { + double weight = + std::sqrt(config_.weight_sampson) / (1.0 / (K0_(0, 0) + K0_(1, 1)) + 1.0 / (K1_(0, 0) + K1_(1, 1))); for (auto &i : indices_sampson_) { Eigen::Vector3d x0 = K0_inv_ * x0_.col(i); Eigen::Vector3d x1 = K1_inv_ * x1_.col(i); - ceres::CostFunction *sampson_cost = - SampsonErrorFunctor::Create(x0, x1, K0_, K1_, config_.weight_sampson); + ceres::CostFunction *sampson_cost = SampsonErrorFunctor::Create(x0, x1, weight); problem_->AddResidualBlock(sampson_cost, sampson_loss_func, qvec_.data(), tvec_.data()); } } @@ -305,10 +305,11 @@ class HybridSharedFocalPoseOptimizer { } } + if (config_.use_sampson) { + double weight = std::sqrt(config_.weight_sampson); for (auto &i : indices_sampson_) { - if (config_.use_sampson) { ceres::CostFunction *sampson_cost = - SampsonErrorSharedFocalFunctor::Create(x0_.col(i), x1_.col(i), config_.weight_sampson); + SampsonErrorSharedFocalFunctor::Create(x0_.col(i), x1_.col(i), weight); problem_->AddResidualBlock(sampson_cost, sampson_loss_func, qvec_.data(), tvec_.data(), &focal_); } } @@ -430,10 +431,10 @@ class HybridTwoFocalPoseOptimizer { } } + if (config_.use_sampson) { + double weight = std::sqrt(config_.weight_sampson); for (auto &i : indices_sampson_) { - if (config_.use_sampson) { - ceres::CostFunction *sampson_cost = - SampsonErrorTwoFocalFunctor::Create(x0_.col(i), x1_.col(i), config_.weight_sampson); + ceres::CostFunction *sampson_cost = SampsonErrorTwoFocalFunctor::Create(x0_.col(i), x1_.col(i), weight); problem_->AddResidualBlock(sampson_cost, sampson_loss_func, qvec_.data(), tvec_.data(), &focal0_, &focal1_); } From d5be616aa18e7a6b20526bd55f71250c4f35f3db Mon Sep 17 00:00:00 2001 From: Yifan Yu Date: Wed, 5 Feb 2025 06:29:04 +0100 Subject: [PATCH 3/6] Refactor configs and options, enable multi-threading for ceres --- src/bindings.cpp | 17 +++++------ src/estimator_config.h | 7 +++++ src/hybrid_pose_estimator.cpp | 10 ++++--- src/hybrid_pose_estimator.h | 19 ++++++++++++ src/hybrid_pose_shared_focal_estimator.cpp | 8 +++-- src/hybrid_pose_shared_focal_estimator.h | 9 ++++++ src/hybrid_pose_two_focal_estimator.cpp | 7 +++-- src/hybrid_pose_two_focal_estimator.h | 35 +++++----------------- src/optimizer.h | 4 --- src/optimizer_config.h | 2 +- 10 files changed, 65 insertions(+), 53 deletions(-) diff --git a/src/bindings.cpp b/src/bindings.cpp index adc6637..e8fae39 100644 --- a/src/bindings.cpp +++ b/src/bindings.cpp @@ -94,20 +94,17 @@ void bind_ransaclib(py::module &m) { } void bind_estimator(py::module &m) { - py::class_(m, "OptimizerConfig") - .def(py::init<>()) - .def_readwrite("constant_pose", &OptimizerConfig::constant_pose) - .def_readwrite("constant_scale", &OptimizerConfig::constant_scale) - .def_readwrite("constant_offset", &OptimizerConfig::constant_offset) - .def_readwrite("solver_options", &OptimizerConfig::solver_options) - .def_readwrite("min_depth_constraint", &OptimizerConfig::min_depth_constraint) - .def_readwrite("use_shift", &OptimizerConfig::use_shift); - py::class_(m, "EstimatorConfig") .def(py::init<>()) .def(py::init(), "solver"_a = 0, "score"_a = 0, "LO"_a = 0) .def_readwrite("min_depth_constraint", &EstimatorConfig::min_depth_constraint) - .def_readwrite("use_shift", &EstimatorConfig::use_shift); + .def_readwrite("use_shift", &EstimatorConfig::use_shift) + .def_readwrite("ceres_function_tolerance", &EstimatorConfig::ceres_function_tolerance) + .def_readwrite("ceres_gradient_tolerance", &EstimatorConfig::ceres_gradient_tolerance) + .def_readwrite("ceres_parameter_tolerance", &EstimatorConfig::ceres_parameter_tolerance) + .def_readwrite("ceres_max_num_iterations", &EstimatorConfig::ceres_max_num_iterations) + .def_readwrite("ceres_use_nonmonotonic_steps", &EstimatorConfig::ceres_use_nonmonotonic_steps) + .def_readwrite("ceres_num_threads", &EstimatorConfig::ceres_num_threads); py::class_(m, "PoseAndScale") .def(py::init<>()) diff --git a/src/estimator_config.h b/src/estimator_config.h index 98ff9ae..01b6f48 100644 --- a/src/estimator_config.h +++ b/src/estimator_config.h @@ -23,6 +23,13 @@ class EstimatorConfig { bool min_depth_constraint = true; bool use_shift = true; + + double ceres_function_tolerance = 1e-6; + double ceres_gradient_tolerance = 1e-8; + double ceres_parameter_tolerance = 1e-6; + double ceres_max_num_iterations = 25; + bool ceres_use_nonmonotonic_steps = true; + int ceres_num_threads = 1; }; } // namespace madpose diff --git a/src/hybrid_pose_estimator.cpp b/src/hybrid_pose_estimator.cpp index ce33371..7c3aa14 100644 --- a/src/hybrid_pose_estimator.cpp +++ b/src/hybrid_pose_estimator.cpp @@ -189,7 +189,7 @@ int HybridPoseEstimator::NonMinimalSolver(const std::vector> &s } OptimizerConfig config; - config.solver_options.max_num_iterations = 25; + set_ceres_solver_options(config.solver_options); config.use_sampson = true; config.use_reprojection = true; @@ -264,6 +264,8 @@ void HybridPoseEstimator::LeastSquares(const std::vector> &samp } OptimizerConfig config; + set_ceres_solver_options(config.solver_options); + config.use_sampson = true; config.use_reprojection = true; if (est_config_.LO_type == EstimatorOption::MD_ONLY) @@ -295,7 +297,7 @@ int HybridPoseEstimatorScaleOnly::NonMinimalSolver(const std::vector squared_inlier_thresholds_; + + void set_ceres_solver_options(ceres::Solver::Options &options) const { + options.function_tolerance = est_config_.ceres_function_tolerance; + options.gradient_tolerance = est_config_.ceres_gradient_tolerance; + options.parameter_tolerance = est_config_.ceres_parameter_tolerance; + options.max_num_iterations = est_config_.ceres_max_num_iterations; + options.use_nonmonotonic_steps = est_config_.ceres_use_nonmonotonic_steps; + options.num_threads = est_config_.ceres_num_threads; + } }; class HybridPoseEstimatorScaleOnly { @@ -176,9 +185,19 @@ class HybridPoseEstimatorScaleOnly { Eigen::MatrixXd x0_, x1_; Eigen::VectorXd d0_, d1_; double sampson_squared_weight_; + double sampson_loss_scale_; EstimatorConfig est_config_; std::vector squared_inlier_thresholds_; + + void set_ceres_solver_options(ceres::Solver::Options &options) const { + options.function_tolerance = est_config_.ceres_function_tolerance; + options.gradient_tolerance = est_config_.ceres_gradient_tolerance; + options.parameter_tolerance = est_config_.ceres_parameter_tolerance; + options.max_num_iterations = est_config_.ceres_max_num_iterations; + options.use_nonmonotonic_steps = est_config_.ceres_use_nonmonotonic_steps; + options.num_threads = est_config_.ceres_num_threads; + } }; std::pair diff --git a/src/hybrid_pose_shared_focal_estimator.cpp b/src/hybrid_pose_shared_focal_estimator.cpp index eba6506..025805d 100644 --- a/src/hybrid_pose_shared_focal_estimator.cpp +++ b/src/hybrid_pose_shared_focal_estimator.cpp @@ -130,8 +130,10 @@ int HybridSharedFocalPoseEstimator::NonMinimalSolver(const std::vector squared_inlier_thresholds_; double norm_scale_; + + void set_ceres_solver_options(ceres::Solver::Options &options) const { + options.function_tolerance = est_config_.ceres_function_tolerance; + options.gradient_tolerance = est_config_.ceres_gradient_tolerance; + options.parameter_tolerance = est_config_.ceres_parameter_tolerance; + options.max_num_iterations = est_config_.ceres_max_num_iterations; + options.use_nonmonotonic_steps = est_config_.ceres_use_nonmonotonic_steps; + options.num_threads = est_config_.ceres_num_threads; + } }; std::pair HybridEstimatePoseScaleOffsetSharedFocal( diff --git a/src/hybrid_pose_two_focal_estimator.cpp b/src/hybrid_pose_two_focal_estimator.cpp index 522112b..60f2d9e 100644 --- a/src/hybrid_pose_two_focal_estimator.cpp +++ b/src/hybrid_pose_two_focal_estimator.cpp @@ -182,7 +182,8 @@ int HybridTwoFocalPoseEstimator::NonMinimalSolver(const std::vector squared_inlier_thresholds_; double norm_scale_; -}; - -class HybridTwoFocalPoseEstimator3 : public HybridTwoFocalPoseEstimator { - public: - HybridTwoFocalPoseEstimator3(const std::vector &x0_norm, - const std::vector &x1_norm, const std::vector &depth0, - const std::vector &depth1, const Eigen::Vector2d &min_depth, - const double &norm_scale = 1.0, const double &sampson_squared_weight = 1.0, - const std::vector &squared_inlier_thresholds = {}, - const EstimatorConfig &est_config = EstimatorConfig()) - : HybridTwoFocalPoseEstimator(x0_norm, x1_norm, depth0, depth1, min_depth, norm_scale, sampson_squared_weight, - squared_inlier_thresholds, est_config) {} - int MinimalSolver(const std::vector> &sample, const int solver_idx, - std::vector *models) const { - std::vector> sample_2 = {sample[0], sample[2]}; - return HybridTwoFocalPoseEstimator::MinimalSolver(sample_2, solver_idx, models); + void set_ceres_solver_options(ceres::Solver::Options &options) const { + options.function_tolerance = est_config_.ceres_function_tolerance; + options.gradient_tolerance = est_config_.ceres_gradient_tolerance; + options.parameter_tolerance = est_config_.ceres_parameter_tolerance; + options.max_num_iterations = est_config_.ceres_max_num_iterations; + options.use_nonmonotonic_steps = est_config_.ceres_use_nonmonotonic_steps; + options.num_threads = est_config_.ceres_num_threads; } - - // Returns 0 if no model could be estimated and 1 otherwise. - // Implemented by a simple linear least squares solver. - int NonMinimalSolver(const std::vector> &sample, const int solver_idx, - PoseScaleOffsetTwoFocal *model) const; - - // Evaluates the line on the i-th data point. - double EvaluateModelOnPoint(const PoseScaleOffsetTwoFocal &model, int t, int i, bool is_for_inlier = false) const; - - // Linear least squares solver. - void LeastSquares(const std::vector> &sample, const int solver_idx, - PoseScaleOffsetTwoFocal *model) const; }; std::pair HybridEstimatePoseScaleOffsetTwoFocal( diff --git a/src/optimizer.h b/src/optimizer.h index e7d424b..9aff74c 100644 --- a/src/optimizer.h +++ b/src/optimizer.h @@ -114,7 +114,6 @@ class HybridPoseOptimizer { ceres::Solver::Options solver_options = config_.solver_options; solver_options.linear_solver_type = ceres::DENSE_QR; - solver_options.num_threads = 1; std::string solver_error; CHECK(solver_options.IsValid(&solver_error)) << solver_error; @@ -229,7 +228,6 @@ class HybridPoseOptimizerScaleOnly { ceres::Solver::Options solver_options = config_.solver_options; solver_options.linear_solver_type = ceres::DENSE_QR; - solver_options.num_threads = 1; std::string solver_error; CHECK(solver_options.IsValid(&solver_error)) << solver_error; @@ -354,7 +352,6 @@ class HybridSharedFocalPoseOptimizer { ceres::Solver::Options solver_options = config_.solver_options; solver_options.linear_solver_type = ceres::DENSE_QR; - solver_options.num_threads = 1; std::string solver_error; CHECK(solver_options.IsValid(&solver_error)) << solver_error; @@ -485,7 +482,6 @@ class HybridTwoFocalPoseOptimizer { ceres::Solver::Options solver_options = config_.solver_options; solver_options.linear_solver_type = ceres::DENSE_QR; - solver_options.num_threads = 1; std::string solver_error; CHECK(solver_options.IsValid(&solver_error)) << solver_error; diff --git a/src/optimizer_config.h b/src/optimizer_config.h index d128917..69e5273 100644 --- a/src/optimizer_config.h +++ b/src/optimizer_config.h @@ -13,7 +13,7 @@ class OptimizerConfig { solver_options.minimizer_progress_to_stdout = true; solver_options.max_num_iterations = 100; solver_options.use_nonmonotonic_steps = true; - solver_options.num_threads = -1; + solver_options.num_threads = 1; solver_options.logging_type = ceres::SILENT; problem_options.loss_function_ownership = ceres::DO_NOT_TAKE_OWNERSHIP; problem_options.cost_function_ownership = ceres::TAKE_OWNERSHIP; From e0f5ccce5c1834dfb85b8160c5ead5ab387f17ee Mon Sep 17 00:00:00 2001 From: Yifan Yu Date: Wed, 5 Feb 2025 06:29:41 +0100 Subject: [PATCH 4/6] Update options and config for examples --- examples/calibrated.py | 4 +++- examples/shared_focal.py | 4 +++- examples/two_focal.py | 4 +++- 3 files changed, 9 insertions(+), 3 deletions(-) diff --git a/examples/calibrated.py b/examples/calibrated.py index 7f0a58e..5e5e011 100644 --- a/examples/calibrated.py +++ b/examples/calibrated.py @@ -18,7 +18,8 @@ # Ransac Options and Estimator Configs options = madpose.HybridLORansacOptions() -options.min_num_iterations = 1000 +options.min_num_iterations = 100 +options.max_num_iterations = 1000 options.final_least_squares = True options.threshold_multiplier = 5.0 options.num_lo_steps = 4 @@ -29,6 +30,7 @@ est_config = madpose.EstimatorConfig() est_config.min_depth_constraint = True est_config.use_shift = True +est_config.ceres_num_threads = 8 # Read the image pair image0 = cv2.imread(os.path.join(sample_path, "image0.png")) diff --git a/examples/shared_focal.py b/examples/shared_focal.py index 10e5e7e..a722771 100644 --- a/examples/shared_focal.py +++ b/examples/shared_focal.py @@ -18,7 +18,8 @@ # Ransac Options and Estimator Configs options = madpose.HybridLORansacOptions() -options.min_num_iterations = 1000 +options.min_num_iterations = 100 +options.max_num_iterations = 1000 options.final_least_squares = True options.threshold_multiplier = 5.0 options.num_lo_steps = 4 @@ -29,6 +30,7 @@ est_config = madpose.EstimatorConfig() est_config.min_depth_constraint = True est_config.use_shift = True +est_config.ceres_num_threads = 8 # Read the image pair image0 = cv2.imread(os.path.join(sample_path, "image0.png")) diff --git a/examples/two_focal.py b/examples/two_focal.py index 090bd9a..e736ed4 100644 --- a/examples/two_focal.py +++ b/examples/two_focal.py @@ -18,7 +18,8 @@ # Ransac Options and Estimator Configs options = madpose.HybridLORansacOptions() -options.min_num_iterations = 1000 +options.min_num_iterations = 100 +options.max_num_iterations = 1000 options.final_least_squares = True options.threshold_multiplier = 5.0 options.num_lo_steps = 4 @@ -29,6 +30,7 @@ est_config = madpose.EstimatorConfig() est_config.min_depth_constraint = True est_config.use_shift = True +est_config.ceres_num_threads = 8 # Read the image pair image0 = cv2.imread(os.path.join(sample_path, "image0.png")) From 44cd730892d3dd1a4134882a6ef9b9b68c822065 Mon Sep 17 00:00:00 2001 From: Yifan Yu Date: Wed, 5 Feb 2025 06:29:58 +0100 Subject: [PATCH 5/6] Optimize compile options --- CMakeLists.txt | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/CMakeLists.txt b/CMakeLists.txt index 9698cba..f75c618 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -8,6 +8,11 @@ if(CMAKE_CXX_COMPILER_VERSION VERSION_LESS 9.1) message(FATAL_ERROR "GCC version needs to be at least 9.1") endif() +if(NOT CMAKE_BUILD_TYPE) + set(CMAKE_BUILD_TYPE Release) +endif() +add_compile_options(-O3 -ffast-math -fno-associative-math -DNDEBUG) + # Options option(FETCH_POSELIB "Whether to use PoseLib with FetchContent or with self-installed software" ON) From 2060f46b3f72dfc7849af4cf5dad104ce55b55d9 Mon Sep 17 00:00:00 2001 From: Yifan Yu Date: Wed, 5 Feb 2025 06:42:30 +0100 Subject: [PATCH 6/6] Update README.md --- README.md | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) diff --git a/README.md b/README.md index fa1b47a..2dba51a 100644 --- a/README.md +++ b/README.md @@ -223,8 +223,8 @@ The estimators take `HybridLORansacOptions` and `EstimatorConfig` for related se import madpose options = madpose.HybridLORansacOptions() -options.min_num_iterations = 1000 -options.max_num_iterations = 10000 +options.min_num_iterations = 100 +options.max_num_iterations = 1000 options.success_probability = 0.9999 options.random_seed = 0 # for reproducibility options.final_least_squares = True @@ -240,8 +240,12 @@ est_config = madpose.EstimatorConfig() est_config.min_depth_constraint = True # if disabled, will model the depth with only scale (only applicable to the calibrated camera case) est_config.use_shift = True +# best set to the number of PHYSICAL CPU cores +est_config.ceres_num_threads = 8 ``` +**Note on performance**: if speed is a primary concern, best set `est_config.ceres_num_threads` to the number of _physical_ CPU cores on your machine, also tune `options.min_num_iterations`, `options.max_num_iterations`, and `options.num_lo_steps` for trade-off between speed and accuracy. In addition, `export OPENBLAS_NUM_THREADS=1` to use single-thread dense solver in Ceres to avoid overheads due to the small-sized nature of the Jacobians. + We provide a example image pairs and code snippets in [examples/](examples/) to test the hybrid estimators. More demos and evaluations will be added in the future. We use `numpy` and `cv2` to handle pre-computed data in Python in the examples, you can easily install them using pip: