From 39286f667214a4f7fd88c224943bfb850d5e2a32 Mon Sep 17 00:00:00 2001 From: lcarlone Date: Sun, 19 Dec 2021 10:41:07 -0500 Subject: [PATCH 1/6] added clone to play well with gnc --- gtsam_unstable/slam/PoseToPointFactor.h | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/gtsam_unstable/slam/PoseToPointFactor.h b/gtsam_unstable/slam/PoseToPointFactor.h index cab48e506..b9b2ad5ce 100644 --- a/gtsam_unstable/slam/PoseToPointFactor.h +++ b/gtsam_unstable/slam/PoseToPointFactor.h @@ -61,6 +61,12 @@ class PoseToPointFactor : public NoiseModelFactor2 { traits::Equals(this->measured_, e->measured_, tol); } + /// @return a deep copy of this factor + gtsam::NonlinearFactor::shared_ptr clone() const override { + return boost::static_pointer_cast( + gtsam::NonlinearFactor::shared_ptr(new This(*this))); + } + /** implement functions needed to derive from Factor */ /** vector of errors From c59fbc825fe2f77c96d2e8a1bd7c509c3bf7189c Mon Sep 17 00:00:00 2001 From: lcarlone Date: Wed, 5 Jan 2022 23:01:57 -0500 Subject: [PATCH 2/6] ruled out corner case where weights are outside [0,1] in TLS --- gtsam/nonlinear/GncOptimizer.h | 8 +++----- 1 file changed, 3 insertions(+), 5 deletions(-) diff --git a/gtsam/nonlinear/GncOptimizer.h b/gtsam/nonlinear/GncOptimizer.h index 3025d2468..9601e5f8c 100644 --- a/gtsam/nonlinear/GncOptimizer.h +++ b/gtsam/nonlinear/GncOptimizer.h @@ -439,13 +439,11 @@ class GTSAM_EXPORT GncOptimizer { for (size_t k : unknownWeights) { if (nfg_[k]) { double u2_k = nfg_[k]->error(currentEstimate); // squared (and whitened) residual - if (u2_k >= upperbound) { + weights[k] = std::sqrt(barcSq_[k] * mu * (mu + 1) / u2_k) - mu; + if (u2_k >= upperbound || weights[k] < 0) { weights[k] = 0; - } else if (u2_k <= lowerbound) { + } else if (u2_k <= lowerbound || weights[k] > 1) { weights[k] = 1; - } else { - weights[k] = std::sqrt(barcSq_[k] * mu * (mu + 1) / u2_k) - - mu; } } } From 859e4cb37af125b9351db22dc86837077d5c62f9 Mon Sep 17 00:00:00 2001 From: lcarlone Date: Wed, 12 Jan 2022 21:31:22 -0500 Subject: [PATCH 3/6] thresholded mu to avoid case mu = 0 in TLS. improved verbosity handling --- gtsam/nonlinear/GncOptimizer.h | 25 +++++++++++++++++++------ gtsam/nonlinear/GncParams.h | 2 ++ 2 files changed, 21 insertions(+), 6 deletions(-) diff --git a/gtsam/nonlinear/GncOptimizer.h b/gtsam/nonlinear/GncOptimizer.h index 3025d2468..6c8519aac 100644 --- a/gtsam/nonlinear/GncOptimizer.h +++ b/gtsam/nonlinear/GncOptimizer.h @@ -206,9 +206,11 @@ class GTSAM_EXPORT GncOptimizer { std::cout << "GNC Optimizer stopped because all measurements are already known to be inliers or outliers" << std::endl; } + if (params_.verbosity >= GncParameters::Verbosity::MU) { + std::cout << "mu: " << mu << std::endl; + } if (params_.verbosity >= GncParameters::Verbosity::VALUES) { result.print("result\n"); - std::cout << "mu: " << mu << std::endl; } return result; } @@ -217,12 +219,16 @@ class GTSAM_EXPORT GncOptimizer { for (iter = 0; iter < params_.maxIterations; iter++) { // display info - if (params_.verbosity >= GncParameters::Verbosity::VALUES) { + if (params_.verbosity >= GncParameters::Verbosity::MU) { std::cout << "iter: " << iter << std::endl; - result.print("result\n"); std::cout << "mu: " << mu << std::endl; + } + if (params_.verbosity >= GncParameters::Verbosity::WEIGHTS) { std::cout << "weights: " << weights_ << std::endl; } + if (params_.verbosity >= GncParameters::Verbosity::VALUES) { + result.print("result\n"); + } // weights update weights_ = calculateWeights(result, mu); @@ -253,10 +259,12 @@ class GTSAM_EXPORT GncOptimizer { if (params_.verbosity >= GncParameters::Verbosity::SUMMARY) { std::cout << "final iterations: " << iter << std::endl; std::cout << "final mu: " << mu << std::endl; - std::cout << "final weights: " << weights_ << std::endl; std::cout << "previous cost: " << prev_cost << std::endl; std::cout << "current cost: " << cost << std::endl; } + if (params_.verbosity >= GncParameters::Verbosity::WEIGHTS) { + std::cout << "final weights: " << weights_ << std::endl; + } return result; } @@ -291,6 +299,9 @@ class GTSAM_EXPORT GncOptimizer { std::min(mu_init, barcSq_[k] / (2 * rk - barcSq_[k]) ) : mu_init; } } + if (mu_init >= 0 && mu_init < 1e-6) + mu_init = 1e-6; // if mu ~ 0 (but positive), that means we have measurements with large errors, + // i.e., rk > barcSq_[k] and rk very large, hence we threshold to 1e-6 to avoid mu = 0 return mu_init > 0 && !std::isinf(mu_init) ? mu_init : -1; // if mu <= 0 or mu = inf, return -1, // which leads to termination of the main gnc loop. In this case, all residuals are already below the threshold // and there is no need to robustify (TLS = least squares) @@ -338,8 +349,10 @@ class GTSAM_EXPORT GncOptimizer { bool checkCostConvergence(const double cost, const double prev_cost) const { bool costConverged = std::fabs(cost - prev_cost) / std::max(prev_cost, 1e-7) < params_.relativeCostTol; - if (costConverged && params_.verbosity >= GncParameters::Verbosity::SUMMARY) - std::cout << "checkCostConvergence = true " << std::endl; + if (costConverged && params_.verbosity >= GncParameters::Verbosity::SUMMARY){ + std::cout << "checkCostConvergence = true (prev. cost = " << prev_cost + << ", curr. cost = " << cost << ")" << std::endl; + } return costConverged; } diff --git a/gtsam/nonlinear/GncParams.h b/gtsam/nonlinear/GncParams.h index 086f08acc..1f324ae38 100644 --- a/gtsam/nonlinear/GncParams.h +++ b/gtsam/nonlinear/GncParams.h @@ -48,6 +48,8 @@ class GTSAM_EXPORT GncParams { enum Verbosity { SILENT = 0, SUMMARY, + MU, + WEIGHTS, VALUES }; From 7b0356cbea59c3660c8e85e81f07917b007d0e9c Mon Sep 17 00:00:00 2001 From: lcarlone Date: Tue, 22 Mar 2022 20:40:08 -0400 Subject: [PATCH 4/6] edited --- gtsam/nonlinear/GncOptimizer.h | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/gtsam/nonlinear/GncOptimizer.h b/gtsam/nonlinear/GncOptimizer.h index c5c5d1883..96b03e803 100644 --- a/gtsam/nonlinear/GncOptimizer.h +++ b/gtsam/nonlinear/GncOptimizer.h @@ -447,11 +447,11 @@ class GTSAM_EXPORT GncOptimizer { return weights; } case GncLossType::TLS: { // use eq (14) in GNC paper - double upperbound = (mu + 1) / mu * barcSq_.maxCoeff(); - double lowerbound = mu / (mu + 1) * barcSq_.minCoeff(); for (size_t k : unknownWeights) { if (nfg_[k]) { double u2_k = nfg_[k]->error(currentEstimate); // squared (and whitened) residual + double upperbound = (mu + 1) / mu * barcSq_[k]; + double lowerbound = mu / (mu + 1) * barcSq_[k]; weights[k] = std::sqrt(barcSq_[k] * mu * (mu + 1) / u2_k) - mu; if (u2_k >= upperbound || weights[k] < 0) { weights[k] = 0; From f947b973cdc5d59214f95b80531f17c4f538b771 Mon Sep 17 00:00:00 2001 From: lcarlone Date: Sat, 2 Jul 2022 16:02:42 -0400 Subject: [PATCH 5/6] addressed comments in PR --- gtsam/geometry/triangulation.h | 4 ++-- gtsam/nonlinear/GncOptimizer.h | 6 ++++-- 2 files changed, 6 insertions(+), 4 deletions(-) diff --git a/gtsam/geometry/triangulation.h b/gtsam/geometry/triangulation.h index af01d3a36..a4c6b1a13 100644 --- a/gtsam/geometry/triangulation.h +++ b/gtsam/geometry/triangulation.h @@ -107,7 +107,7 @@ std::pair triangulationGraph( const std::vector& poses, boost::shared_ptr sharedCal, const Point2Vector& measurements, Key landmarkKey, const Point3& initialEstimate, - const SharedNoiseModel& model = nullptr) { + const SharedNoiseModel& model = unit2) { Values values; values.insert(landmarkKey, initialEstimate); // Initial landmark value NonlinearFactorGraph graph; @@ -117,7 +117,7 @@ std::pair triangulationGraph( typedef PinholePose Camera; Camera camera_i(pose_i, sharedCal); graph.emplace_shared > // - (camera_i, measurements[i], model? model : unit2, landmarkKey); + (camera_i, measurements[i], model, landmarkKey); } return std::make_pair(graph, values); } diff --git a/gtsam/nonlinear/GncOptimizer.h b/gtsam/nonlinear/GncOptimizer.h index 96b03e803..e5b4718d6 100644 --- a/gtsam/nonlinear/GncOptimizer.h +++ b/gtsam/nonlinear/GncOptimizer.h @@ -299,9 +299,11 @@ class GTSAM_EXPORT GncOptimizer { std::min(mu_init, barcSq_[k] / (2 * rk - barcSq_[k]) ) : mu_init; } } - if (mu_init >= 0 && mu_init < 1e-6) + if (mu_init >= 0 && mu_init < 1e-6){ mu_init = 1e-6; // if mu ~ 0 (but positive), that means we have measurements with large errors, - // i.e., rk > barcSq_[k] and rk very large, hence we threshold to 1e-6 to avoid mu = 0 + // i.e., rk > barcSq_[k] and rk very large, hence we threshold to 1e-6 to avoid mu = 0 + } + return mu_init > 0 && !std::isinf(mu_init) ? mu_init : -1; // if mu <= 0 or mu = inf, return -1, // which leads to termination of the main gnc loop. In this case, all residuals are already below the threshold // and there is no need to robustify (TLS = least squares) From 79305c1a9d51bdb62f43993a20ad32b5b6fc8ad2 Mon Sep 17 00:00:00 2001 From: lcarlone Date: Sat, 2 Jul 2022 17:05:14 -0400 Subject: [PATCH 6/6] fixed compile issue --- gtsam/geometry/triangulation.h | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/gtsam/geometry/triangulation.h b/gtsam/geometry/triangulation.h index a4c6b1a13..486850903 100644 --- a/gtsam/geometry/triangulation.h +++ b/gtsam/geometry/triangulation.h @@ -107,11 +107,10 @@ std::pair triangulationGraph( const std::vector& poses, boost::shared_ptr sharedCal, const Point2Vector& measurements, Key landmarkKey, const Point3& initialEstimate, - const SharedNoiseModel& model = unit2) { + const SharedNoiseModel& model = noiseModel::Unit::Create(2)) { Values values; values.insert(landmarkKey, initialEstimate); // Initial landmark value NonlinearFactorGraph graph; - static SharedNoiseModel unit2(noiseModel::Unit::Create(2)); for (size_t i = 0; i < measurements.size(); i++) { const Pose3& pose_i = poses[i]; typedef PinholePose Camera;