Skip to content

Optimize for numerical stability and speed #9

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Merged
MarkYu98 merged 6 commits into from
Feb 5, 2025
Merged

Optimize for numerical stability and speed #9

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
6 commits
Select commit Hold shift + click to select a range
b6567fe
Make utils functions templated
MarkYu98 Jan 31, 2025
69e2f4f
Better implementation of cost functions for numerical stability and s…
MarkYu98 Feb 5, 2025
d5be616
Refactor configs and options, enable multi-threading for ceres
MarkYu98 Feb 5, 2025
e0f5ccc
Update options and config for examples
MarkYu98 Feb 5, 2025
44cd730
Optimize compile options
MarkYu98 Feb 5, 2025
2060f46
Update README.md
MarkYu98 Feb 5, 2025
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
5 changes: 5 additions & 0 deletions CMakeLists.txt
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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)

Expand Down
8 changes: 6 additions & 2 deletions README.md
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand All @@ -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:
Expand Down
4 changes: 3 additions & 1 deletion examples/calibrated.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand All @@ -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"))
Expand Down
4 changes: 3 additions & 1 deletion examples/shared_focal.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand All @@ -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"))
Expand Down
4 changes: 3 additions & 1 deletion examples/two_focal.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand All @@ -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"))
Expand Down
17 changes: 7 additions & 10 deletions src/bindings.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -94,20 +94,17 @@ void bind_ransaclib(py::module &m) {
}

void bind_estimator(py::module &m) {
py::class_<OptimizerConfig>(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_<EstimatorConfig>(m, "EstimatorConfig")
.def(py::init<>())
.def(py::init<int, int, int>(), "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_<PoseAndScale>(m, "PoseAndScale")
.def(py::init<>())
Expand Down
95 changes: 59 additions & 36 deletions src/cost_functions.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -3,6 +3,8 @@
#include "pose.h"
#include "utils.h"

#include <ceres/rotation.h>

namespace madpose {

// *******************************************************************
Expand All @@ -25,11 +27,14 @@ struct LiftProjectionFunctor0 {
template <typename T>
bool operator()(const T *const o0, const T *const qvec, const T *const tvec, T *residuals) const {
Eigen::Vector<T, 3> x3d = x0_calib_.cast<T>() * (x0_depth_ + o0[0]);
Eigen::Matrix<T, 3, 3> R = QuaternionToRotationMatrix<T>(Eigen::Map<const Eigen::Vector<T, 4>>(qvec));
// Eigen::Matrix<T, 3, 3> R = QuaternionToRotationMatrix<T>(Eigen::Map<const Eigen::Vector<T, 4>>(qvec));
Eigen::Vector<T, 3> t = Eigen::Map<const Eigen::Vector<T, 3>>(tvec);

Eigen::Vector<T, 3> x3d_1 = R * x3d + t;
Eigen::Vector<T, 3> x1_hat = K1_ * x3d_1;
// Eigen::Vector<T, 3> x3d_1 = R * x3d + t;
Eigen::Vector<T, 3> x3d_1;
ceres::QuaternionRotatePoint(qvec, x3d.data(), x3d_1.data());

Eigen::Vector<T, 3> 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];
Expand Down Expand Up @@ -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<T, 3> x3d = x1_calib_.cast<T>() * (x1_depth_ + o1[0]) * scale[0];
Eigen::Matrix<T, 3, 3> R = QuaternionToRotationMatrix<T>(Eigen::Map<const Eigen::Vector<T, 4>>(qvec));
// Eigen::Matrix<T, 3, 3> R = QuaternionToRotationMatrix<T>(Eigen::Map<const Eigen::Vector<T, 4>>(qvec));
Eigen::Vector<T, 3> t = Eigen::Map<const Eigen::Vector<T, 3>>(tvec);

Eigen::Vector<T, 3> x3d_0 = R.transpose() * x3d - R.transpose() * t;
Eigen::Vector<T, 4> quat = NormalizeQuaternion<T>(Eigen::Map<const Eigen::Vector<T, 4>>(qvec));
Eigen::Vector<T, 4> qvec_inv = {quat[0], -quat[1], -quat[2], -quat[3]};
Eigen::Vector<T, 3> x3d_1 = x3d - t;
Eigen::Vector<T, 3> x3d_0;
ceres::UnitQuaternionRotatePoint(qvec_inv.data(), x3d_1.data(), x3d_0.data());

// Eigen::Vector<T, 3> x3d_0 = R.transpose() * (x3d - t);
Eigen::Vector<T, 3> x0_hat = K0_ * x3d_0;
x0_hat /= x0_hat[2];
residuals[0] = x0_hat[0] - x0_[0];
Expand All @@ -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<SampsonErrorFunctor, 1, 4, 3>(
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<SampsonErrorFunctor, 1, 4, 3>(new SampsonErrorFunctor(x0, x1, weight)));
}

template <typename T> bool operator()(const T *const qvec, const T *const tvec, T *residuals) const {
Expand All @@ -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<T, 4>(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_;
};

Expand All @@ -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<T, 3> x3d = (K_inv * x0_.cast<T>()) * (x0_depth_ + o0[0]);
Eigen::Matrix<T, 3, 3> R = QuaternionToRotationMatrix<T>(Eigen::Map<const Eigen::Vector<T, 4>>(qvec));
// Eigen::Matrix<T, 3, 3> R = QuaternionToRotationMatrix<T>(Eigen::Map<const Eigen::Vector<T, 4>>(qvec));
Eigen::Vector<T, 3> t = Eigen::Map<const Eigen::Vector<T, 3>>(tvec);

Eigen::Vector<T, 3> x3d_1 = R * x3d + t;
Eigen::Vector<T, 3> x1_hat = K * x3d_1;
// Eigen::Vector<T, 3> x3d_1 = R * x3d + t;
Eigen::Vector<T, 3> x3d_1;
ceres::QuaternionRotatePoint(qvec, x3d.data(), x3d_1.data());

Eigen::Vector<T, 3> 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];
Expand Down Expand Up @@ -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<T, 3> x3d = (K_inv * x1_.cast<T>()) * (x1_depth_ + o1[0]) * scale[0];
Eigen::Matrix<T, 3, 3> R = QuaternionToRotationMatrix<T>(Eigen::Map<const Eigen::Vector<T, 4>>(qvec));
// Eigen::Matrix<T, 3, 3> R = QuaternionToRotationMatrix<T>(Eigen::Map<const Eigen::Vector<T, 4>>(qvec));
Eigen::Vector<T, 3> t = Eigen::Map<const Eigen::Vector<T, 3>>(tvec);

Eigen::Vector<T, 3> x3d_0 = R.transpose() * x3d - R.transpose() * t;
Eigen::Vector<T, 4> quat = NormalizeQuaternion<T>(Eigen::Map<const Eigen::Vector<T, 4>>(qvec));
Eigen::Vector<T, 4> qvec_inv = {quat[0], -quat[1], -quat[2], -quat[3]};
Eigen::Vector<T, 3> x3d_1 = x3d - t;
Eigen::Vector<T, 3> x3d_0;
ceres::UnitQuaternionRotatePoint(qvec_inv.data(), x3d_1.data(), x3d_0.data());

// Eigen::Vector<T, 3> x3d_0 = R.transpose() * (x3d - t);
Eigen::Vector<T, 3> x0_hat = K * x3d_0;
x0_hat /= x0_hat[2];
residuals[0] = x0_hat[0] - x0_[0];
Expand All @@ -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<SampsonErrorSharedFocalFunctor, 1, 4, 3, 1>(
new SampsonErrorSharedFocalFunctor(x0, x1, sq_weight)));
new SampsonErrorSharedFocalFunctor(x0, x1, weight)));
}

template <typename T>
Expand Down Expand Up @@ -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<T, 3> x3d = (K0_inv * x0_.cast<T>()) * (x0_depth_ + o0[0]);
Eigen::Matrix<T, 3, 3> R = QuaternionToRotationMatrix<T>(Eigen::Map<const Eigen::Vector<T, 4>>(qvec));
// Eigen::Matrix<T, 3, 3> R = QuaternionToRotationMatrix<T>(Eigen::Map<const Eigen::Vector<T, 4>>(qvec));
Eigen::Vector<T, 3> t = Eigen::Map<const Eigen::Vector<T, 3>>(tvec);

Eigen::Vector<T, 3> x3d_1 = R * x3d + t;
Eigen::Vector<T, 3> x1_hat = K1 * x3d_1;
Eigen::Vector<T, 3> x3d_1;
ceres::QuaternionRotatePoint(qvec, x3d.data(), x3d_1.data());

// Eigen::Vector<T, 3> x3d_1 = R * x3d + t;
Eigen::Vector<T, 3> 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];
Expand Down Expand Up @@ -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<T, 3> x3d = (K1_inv * x1_.cast<T>()) * (x1_depth_ + o1[0]) * scale[0];
Eigen::Matrix<T, 3, 3> R = QuaternionToRotationMatrix<T>(Eigen::Map<const Eigen::Vector<T, 4>>(qvec));
// Eigen::Matrix<T, 3, 3> R = QuaternionToRotationMatrix<T>(Eigen::Map<const Eigen::Vector<T, 4>>(qvec));
Eigen::Vector<T, 3> t = Eigen::Map<const Eigen::Vector<T, 3>>(tvec);

Eigen::Vector<T, 3> x3d_0 = R.transpose() * x3d - R.transpose() * t;
Eigen::Vector<T, 4> quat = NormalizeQuaternion<T>(Eigen::Map<const Eigen::Vector<T, 4>>(qvec));
Eigen::Vector<T, 4> qvec_inv = {quat[0], -quat[1], -quat[2], -quat[3]};
Eigen::Vector<T, 3> x3d_1 = x3d - t;
Eigen::Vector<T, 3> x3d_0;
ceres::UnitQuaternionRotatePoint(qvec_inv.data(), x3d_1.data(), x3d_0.data());

// Eigen::Vector<T, 3> x3d_0 = R.transpose() * (x3d - t);
Eigen::Vector<T, 3> x0_hat = K0 * x3d_0;
x0_hat /= x0_hat[2];
residuals[0] = x0_hat[0] - x0_[0];
Expand All @@ -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<SampsonErrorTwoFocalFunctor, 1, 4, 3, 1, 1>(
new SampsonErrorTwoFocalFunctor(x0, x1, sq_weight)));
new SampsonErrorTwoFocalFunctor(x0, x1, weight)));
}

template <typename T>
Expand Down
7 changes: 7 additions & 0 deletions src/estimator_config.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
20 changes: 10 additions & 10 deletions src/hybrid_pose_estimator.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -189,7 +189,7 @@ int HybridPoseEstimator::NonMinimalSolver(const std::vector<std::vector<int>> &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;
Expand Down Expand Up @@ -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<double>(x0_calib.head<2>(), x1_calib.head<2>(), E);
return sampson_error * sampson_loss_scale_;
}
}

Expand All @@ -265,6 +264,8 @@ void HybridPoseEstimator::LeastSquares(const std::vector<std::vector<int>> &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)
Expand Down Expand Up @@ -296,7 +297,7 @@ int HybridPoseEstimatorScaleOnly::NonMinimalSolver(const std::vector<std::vector
}

OptimizerConfig config;
config.solver_options.max_num_iterations = 25;
set_ceres_solver_options(config.solver_options);

config.use_sampson = true;
config.use_reprojection = true;
Expand Down Expand Up @@ -407,9 +408,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<double>(x0_calib.head<2>(), x1_calib.head<2>(), E);
return sampson_error * sampson_loss_scale_;
}
}

Expand All @@ -421,10 +421,10 @@ void HybridPoseEstimatorScaleOnly::LeastSquares(const std::vector<std::vector<in
}

OptimizerConfig config;
set_ceres_solver_options(config.solver_options);

config.use_sampson = true;
config.use_reprojection = true;
config.solver_options.max_num_iterations = 25;

if (est_config_.LO_type == EstimatorOption::MD_ONLY)
config.use_sampson = false;
if (est_config_.LO_type == EstimatorOption::EPI_ONLY)
Expand Down
Loading
Loading