gtsam 4.2.0
gtsam
GncOptimizer.h
Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
2
3 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
4 * Atlanta, Georgia 30332-0415
5 * All Rights Reserved
6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
7
8 * See LICENSE for the license information
9
10 * -------------------------------------------------------------------------- */
11
27#pragma once
28
29#include <gtsam/nonlinear/GncParams.h>
31#include <boost/math/distributions/chi_squared.hpp>
32
33namespace gtsam {
34/*
35 * Quantile of chi-squared distribution with given degrees of freedom at probability alpha.
36 * Equivalent to chi2inv in Matlab.
37 */
38static double Chi2inv(const double alpha, const size_t dofs) {
39 boost::math::chi_squared_distribution<double> chi2(dofs);
40 return boost::math::quantile(chi2, alpha);
41}
42
43/* ************************************************************************* */
44template<class GncParameters>
46 public:
48 typedef typename GncParameters::OptimizerType BaseOptimizer;
49
50 private:
52 Values state_;
53 GncParameters params_;
54 Vector weights_;
55 Vector barcSq_;
56
57 public:
59 GncOptimizer(const NonlinearFactorGraph& graph, const Values& initialValues,
60 const GncParameters& params = GncParameters())
61 : state_(initialValues),
62 params_(params) {
63
64 // make sure all noiseModels are Gaussian or convert to Gaussian
65 nfg_.resize(graph.size());
66 for (size_t i = 0; i < graph.size(); i++) {
67 if (graph[i]) {
68 NoiseModelFactor::shared_ptr factor = boost::dynamic_pointer_cast<
69 NoiseModelFactor>(graph[i]);
70 auto robust = boost::dynamic_pointer_cast<
71 noiseModel::Robust>(factor->noiseModel());
72 // if the factor has a robust loss, we remove the robust loss
73 nfg_[i] = robust ? factor-> cloneWithNewNoiseModel(robust->noise()) : factor;
74 }
75 }
76
77 // check that known inliers and outliers make sense:
78 std::vector<size_t> inconsistentlySpecifiedWeights; // measurements the user has incorrectly specified
79 // to be BOTH known inliers and known outliers
80 std::set_intersection(params.knownInliers.begin(),params.knownInliers.end(),
81 params.knownOutliers.begin(),params.knownOutliers.end(),
82 std::inserter(inconsistentlySpecifiedWeights, inconsistentlySpecifiedWeights.begin()));
83 if(inconsistentlySpecifiedWeights.size() > 0){ // if we have inconsistently specified weights, we throw an exception
84 params.print("params\n");
85 throw std::runtime_error("GncOptimizer::constructor: the user has selected one or more measurements"
86 " to be BOTH a known inlier and a known outlier.");
87 }
88 // check that known inliers are in the graph
89 for (size_t i = 0; i < params.knownInliers.size(); i++){
90 if( params.knownInliers[i] > nfg_.size()-1 ){ // outside graph
91 throw std::runtime_error("GncOptimizer::constructor: the user has selected one or more measurements"
92 "that are not in the factor graph to be known inliers.");
93 }
94 }
95 // check that known outliers are in the graph
96 for (size_t i = 0; i < params.knownOutliers.size(); i++){
97 if( params.knownOutliers[i] > nfg_.size()-1 ){ // outside graph
98 throw std::runtime_error("GncOptimizer::constructor: the user has selected one or more measurements"
99 "that are not in the factor graph to be known outliers.");
100 }
101 }
102 // initialize weights (if we don't have prior knowledge of inliers/outliers
103 // the weights are all initialized to 1.
104 weights_ = initializeWeightsFromKnownInliersAndOutliers();
105
106 // set default barcSq_ (inlier threshold)
107 double alpha = 0.99; // with this (default) probability, inlier residuals are smaller than barcSq_
109 }
110
117 void setInlierCostThresholds(const double inth) {
118 barcSq_ = inth * Vector::Ones(nfg_.size());
119 }
120
125 void setInlierCostThresholds(const Vector& inthVec) {
126 barcSq_ = inthVec;
127 }
128
132 void setInlierCostThresholdsAtProbability(const double alpha) {
133 barcSq_ = Vector::Ones(nfg_.size()); // initialize
134 for (size_t k = 0; k < nfg_.size(); k++) {
135 if (nfg_[k]) {
136 barcSq_[k] = 0.5 * Chi2inv(alpha, nfg_[k]->dim()); // 0.5 derives from the error definition in gtsam
137 }
138 }
139 }
140
144 void setWeights(const Vector w) {
145 if (size_t(w.size()) != nfg_.size()) {
146 throw std::runtime_error(
147 "GncOptimizer::setWeights: the number of specified weights"
148 " does not match the size of the factor graph.");
149 }
150 weights_ = w;
151 }
152
154 const NonlinearFactorGraph& getFactors() const { return nfg_; }
155
157 const Values& getState() const { return state_; }
158
160 const GncParameters& getParams() const { return params_;}
161
163 const Vector& getWeights() const { return weights_;}
164
166 const Vector& getInlierCostThresholds() const {return barcSq_;}
167
169 bool equals(const GncOptimizer& other, double tol = 1e-9) const {
170 return nfg_.equals(other.getFactors())
171 && equal(weights_, other.getWeights())
172 && params_.equals(other.getParams())
173 && equal(barcSq_, other.getInlierCostThresholds());
174 }
175
176 Vector initializeWeightsFromKnownInliersAndOutliers() const{
177 Vector weights = Vector::Ones(nfg_.size());
178 for (size_t i = 0; i < params_.knownOutliers.size(); i++){
179 weights[ params_.knownOutliers[i] ] = 0.0; // known to be outliers
180 }
181 return weights;
182 }
183
186 NonlinearFactorGraph graph_initial = this->makeWeightedGraph(weights_);
187 BaseOptimizer baseOptimizer(
188 graph_initial, state_, params_.baseOptimizerParams);
189 Values result = baseOptimizer.optimize();
190 double mu = initializeMu();
191 double prev_cost = graph_initial.error(result);
192 double cost = 0.0; // this will be updated in the main loop
193
194 // handle the degenerate case that corresponds to small
195 // maximum residual errors at initialization
196 // For GM: if residual error is small, mu -> 0
197 // For TLS: if residual error is small, mu -> -1
198 int nrUnknownInOrOut = nfg_.size() - ( params_.knownInliers.size() + params_.knownOutliers.size() );
199 // ^^ number of measurements that are not known to be inliers or outliers (GNC will need to figure them out)
200 if (mu <= 0 || nrUnknownInOrOut == 0) { // no need to even call GNC in this case
201 if (mu <= 0 && params_.verbosity >= GncParameters::Verbosity::SUMMARY) {
202 std::cout << "GNC Optimizer stopped because maximum residual at "
203 "initialization is small."
204 << std::endl;
205 }
206 if (nrUnknownInOrOut==0 && params_.verbosity >= GncParameters::Verbosity::SUMMARY) {
207 std::cout << "GNC Optimizer stopped because all measurements are already known to be inliers or outliers"
208 << std::endl;
209 }
210 if (params_.verbosity >= GncParameters::Verbosity::MU) {
211 std::cout << "mu: " << mu << std::endl;
212 }
213 if (params_.verbosity >= GncParameters::Verbosity::VALUES) {
214 result.print("result\n");
215 }
216 return result;
217 }
218
219 size_t iter;
220 for (iter = 0; iter < params_.maxIterations; iter++) {
221
222 // display info
223 if (params_.verbosity >= GncParameters::Verbosity::MU) {
224 std::cout << "iter: " << iter << std::endl;
225 std::cout << "mu: " << mu << std::endl;
226 }
227 if (params_.verbosity >= GncParameters::Verbosity::WEIGHTS) {
228 std::cout << "weights: " << weights_ << std::endl;
229 }
230 if (params_.verbosity >= GncParameters::Verbosity::VALUES) {
231 result.print("result\n");
232 }
233 // weights update
234 weights_ = calculateWeights(result, mu);
235
236 // variable/values update
237 NonlinearFactorGraph graph_iter = this->makeWeightedGraph(weights_);
238 BaseOptimizer baseOptimizer_iter(
239 graph_iter, state_, params_.baseOptimizerParams);
240 result = baseOptimizer_iter.optimize();
241
242 // stopping condition
243 cost = graph_iter.error(result);
244 if (checkConvergence(mu, weights_, cost, prev_cost)) {
245 break;
246 }
247
248 // update mu
249 mu = updateMu(mu);
250
251 // get ready for next iteration
252 prev_cost = cost;
253
254 // display info
255 if (params_.verbosity >= GncParameters::Verbosity::VALUES) {
256 std::cout << "previous cost: " << prev_cost << std::endl;
257 std::cout << "current cost: " << cost << std::endl;
258 }
259 }
260 // display info
261 if (params_.verbosity >= GncParameters::Verbosity::SUMMARY) {
262 std::cout << "final iterations: " << iter << std::endl;
263 std::cout << "final mu: " << mu << std::endl;
264 std::cout << "previous cost: " << prev_cost << std::endl;
265 std::cout << "current cost: " << cost << std::endl;
266 }
267 if (params_.verbosity >= GncParameters::Verbosity::WEIGHTS) {
268 std::cout << "final weights: " << weights_ << std::endl;
269 }
270 return result;
271 }
272
274 double initializeMu() const {
275
276 double mu_init = 0.0;
277 // initialize mu to the value specified in Remark 5 in GNC paper.
278 switch (params_.lossType) {
279 case GncLossType::GM:
280 /* surrogate cost is convex for large mu. initialize as in remark 5 in GNC paper.
281 Since barcSq_ can be different for each factor, we compute the max of the quantity in remark 5 in GNC paper
282 */
283 for (size_t k = 0; k < nfg_.size(); k++) {
284 if (nfg_[k]) {
285 mu_init = std::max(mu_init, 2 * nfg_[k]->error(state_) / barcSq_[k]);
286 }
287 }
288 return mu_init; // initial mu
289 case GncLossType::TLS:
290 /* surrogate cost is convex for mu close to zero. initialize as in remark 5 in GNC paper.
291 degenerate case: 2 * rmax_sq - params_.barcSq < 0 (handled in the main loop)
292 according to remark mu = params_.barcSq / (2 * rmax_sq - params_.barcSq) = params_.barcSq/ excessResidual
293 however, if the denominator is 0 or negative, we return mu = -1 which leads to termination of the main GNC loop.
294 Since barcSq_ can be different for each factor, we look for the minimimum (positive) quantity in remark 5 in GNC paper
295 */
296 mu_init = std::numeric_limits<double>::infinity();
297 for (size_t k = 0; k < nfg_.size(); k++) {
298 if (nfg_[k]) {
299 double rk = nfg_[k]->error(state_);
300 mu_init = (2 * rk - barcSq_[k]) > 0 ? // if positive, update mu, otherwise keep same
301 std::min(mu_init, barcSq_[k] / (2 * rk - barcSq_[k]) ) : mu_init;
302 }
303 }
304 if (mu_init >= 0 && mu_init < 1e-6){
305 mu_init = 1e-6; // if mu ~ 0 (but positive), that means we have measurements with large errors,
306 // i.e., rk > barcSq_[k] and rk very large, hence we threshold to 1e-6 to avoid mu = 0
307 }
308
309 return mu_init > 0 && !std::isinf(mu_init) ? mu_init : -1; // if mu <= 0 or mu = inf, return -1,
310 // which leads to termination of the main gnc loop. In this case, all residuals are already below the threshold
311 // and there is no need to robustify (TLS = least squares)
312 default:
313 throw std::runtime_error(
314 "GncOptimizer::initializeMu: called with unknown loss type.");
315 }
316 }
317
319 double updateMu(const double mu) const {
320 switch (params_.lossType) {
321 case GncLossType::GM:
322 // reduce mu, but saturate at 1 (original cost is recovered for mu -> 1)
323 return std::max(1.0, mu / params_.muStep);
324 case GncLossType::TLS:
325 // increases mu at each iteration (original cost is recovered for mu -> inf)
326 return mu * params_.muStep;
327 default:
328 throw std::runtime_error(
329 "GncOptimizer::updateMu: called with unknown loss type.");
330 }
331 }
332
334 bool checkMuConvergence(const double mu) const {
335 bool muConverged = false;
336 switch (params_.lossType) {
337 case GncLossType::GM:
338 muConverged = std::fabs(mu - 1.0) < 1e-9; // mu=1 recovers the original GM function
339 break;
340 case GncLossType::TLS:
341 muConverged = false; // for TLS there is no stopping condition on mu (it must tend to infinity)
342 break;
343 default:
344 throw std::runtime_error(
345 "GncOptimizer::checkMuConvergence: called with unknown loss type.");
346 }
347 if (muConverged && params_.verbosity >= GncParameters::Verbosity::SUMMARY)
348 std::cout << "muConverged = true " << std::endl;
349 return muConverged;
350 }
351
353 bool checkCostConvergence(const double cost, const double prev_cost) const {
354 bool costConverged = std::fabs(cost - prev_cost) / std::max(prev_cost, 1e-7)
355 < params_.relativeCostTol;
356 if (costConverged && params_.verbosity >= GncParameters::Verbosity::SUMMARY){
357 std::cout << "checkCostConvergence = true (prev. cost = " << prev_cost
358 << ", curr. cost = " << cost << ")" << std::endl;
359 }
360 return costConverged;
361 }
362
364 bool checkWeightsConvergence(const Vector& weights) const {
365 bool weightsConverged = false;
366 switch (params_.lossType) {
367 case GncLossType::GM:
368 weightsConverged = false; // for GM, there is no clear binary convergence for the weights
369 break;
370 case GncLossType::TLS:
371 weightsConverged = true;
372 for (int i = 0; i < weights.size(); i++) {
373 if (std::fabs(weights[i] - std::round(weights[i]))
374 > params_.weightsTol) {
375 weightsConverged = false;
376 break;
377 }
378 }
379 break;
380 default:
381 throw std::runtime_error(
382 "GncOptimizer::checkWeightsConvergence: called with unknown loss type.");
383 }
384 if (weightsConverged
385 && params_.verbosity >= GncParameters::Verbosity::SUMMARY)
386 std::cout << "weightsConverged = true " << std::endl;
387 return weightsConverged;
388 }
389
391 bool checkConvergence(const double mu, const Vector& weights,
392 const double cost, const double prev_cost) const {
393 return checkCostConvergence(cost, prev_cost)
395 }
396
398 NonlinearFactorGraph makeWeightedGraph(const Vector& weights) const {
399 // make sure all noiseModels are Gaussian or convert to Gaussian
400 NonlinearFactorGraph newGraph;
401 newGraph.resize(nfg_.size());
402 for (size_t i = 0; i < nfg_.size(); i++) {
403 if (nfg_[i]) {
404 auto factor = boost::dynamic_pointer_cast<
405 NoiseModelFactor>(nfg_[i]);
406 auto noiseModel =
407 boost::dynamic_pointer_cast<noiseModel::Gaussian>(
408 factor->noiseModel());
409 if (noiseModel) {
410 Matrix newInfo = weights[i] * noiseModel->information();
411 auto newNoiseModel = noiseModel::Gaussian::Information(newInfo);
412 newGraph[i] = factor->cloneWithNewNoiseModel(newNoiseModel);
413 } else {
414 throw std::runtime_error(
415 "GncOptimizer::makeWeightedGraph: unexpected non-Gaussian noise model.");
416 }
417 }
418 }
419 return newGraph;
420 }
421
423 Vector calculateWeights(const Values& currentEstimate, const double mu) {
424 Vector weights = initializeWeightsFromKnownInliersAndOutliers();
425
426 // do not update the weights that the user has decided are known inliers
427 std::vector<size_t> allWeights;
428 for (size_t k = 0; k < nfg_.size(); k++) {
429 allWeights.push_back(k);
430 }
431 std::vector<size_t> knownWeights;
432 std::set_union(params_.knownInliers.begin(), params_.knownInliers.end(),
433 params_.knownOutliers.begin(), params_.knownOutliers.end(),
434 std::inserter(knownWeights, knownWeights.begin()));
435
436 std::vector<size_t> unknownWeights;
437 std::set_difference(allWeights.begin(), allWeights.end(),
438 knownWeights.begin(), knownWeights.end(),
439 std::inserter(unknownWeights, unknownWeights.begin()));
440
441 // update weights of known inlier/outlier measurements
442 switch (params_.lossType) {
443 case GncLossType::GM: { // use eq (12) in GNC paper
444 for (size_t k : unknownWeights) {
445 if (nfg_[k]) {
446 double u2_k = nfg_[k]->error(currentEstimate); // squared (and whitened) residual
447 weights[k] = std::pow(
448 (mu * barcSq_[k]) / (u2_k + mu * barcSq_[k]), 2);
449 }
450 }
451 return weights;
452 }
453 case GncLossType::TLS: { // use eq (14) in GNC paper
454 for (size_t k : unknownWeights) {
455 if (nfg_[k]) {
456 double u2_k = nfg_[k]->error(currentEstimate); // squared (and whitened) residual
457 double upperbound = (mu + 1) / mu * barcSq_[k];
458 double lowerbound = mu / (mu + 1) * barcSq_[k];
459 weights[k] = std::sqrt(barcSq_[k] * mu * (mu + 1) / u2_k) - mu;
460 if (u2_k >= upperbound || weights[k] < 0) {
461 weights[k] = 0;
462 } else if (u2_k <= lowerbound || weights[k] > 1) {
463 weights[k] = 1;
464 }
465 }
466 }
467 return weights;
468 }
469 default:
470 throw std::runtime_error(
471 "GncOptimizer::calculateWeights: called with unknown loss type.");
472 }
473 }
474};
475
476}
Factor Graph consisting of non-linear factors.
Global functions in a separate testing namespace.
Definition: chartTesting.h:28
bool equal(const T &obj1, const T &obj2, double tol)
Call equal on the object.
Definition: Testable.h:84
virtual void resize(size_t size)
Directly resize the number of factors in the graph.
Definition: FactorGraph.h:381
size_t size() const
return the number of factors (including any null factors set by remove() ).
Definition: FactorGraph.h:326
static shared_ptr Information(const Matrix &M, bool smart=true)
A Gaussian noise model created by specifying an information matrix.
Definition: NoiseModel.cpp:100
Base class for robust error models The robust M-estimators above simply tell us how to re-weight the ...
Definition: NoiseModel.h:642
Definition: GncOptimizer.h:45
bool checkWeightsConvergence(const Vector &weights) const
Check convergence of weights to binary values.
Definition: GncOptimizer.h:364
void setWeights(const Vector w)
Set weights for each factor.
Definition: GncOptimizer.h:144
bool checkMuConvergence(const double mu) const
Check if we have reached the value of mu for which the surrogate loss matches the original loss.
Definition: GncOptimizer.h:334
GncParameters::OptimizerType BaseOptimizer
For each parameter, specify the corresponding optimizer: e.g., GaussNewtonParams -> GaussNewtonOptimi...
Definition: GncOptimizer.h:48
Values optimize()
Compute optimal solution using graduated non-convexity.
Definition: GncOptimizer.h:185
const Vector & getInlierCostThresholds() const
Get the inlier threshold.
Definition: GncOptimizer.h:166
bool checkCostConvergence(const double cost, const double prev_cost) const
Check convergence of relative cost differences.
Definition: GncOptimizer.h:353
const Values & getState() const
Access a copy of the internal values.
Definition: GncOptimizer.h:157
void setInlierCostThresholds(const Vector &inthVec)
Set the maximum weighted residual error for an inlier (one for each factor).
Definition: GncOptimizer.h:125
Vector calculateWeights(const Values &currentEstimate, const double mu)
Calculate gnc weights.
Definition: GncOptimizer.h:423
double initializeMu() const
Initialize the gnc parameter mu such that loss is approximately convex (remark 5 in GNC paper).
Definition: GncOptimizer.h:274
double updateMu(const double mu) const
Update the gnc parameter mu to gradually increase nonconvexity.
Definition: GncOptimizer.h:319
bool equals(const GncOptimizer &other, double tol=1e-9) const
Equals.
Definition: GncOptimizer.h:169
bool checkConvergence(const double mu, const Vector &weights, const double cost, const double prev_cost) const
Check for convergence between consecutive GNC iterations.
Definition: GncOptimizer.h:391
void setInlierCostThresholds(const double inth)
Set the maximum weighted residual error for an inlier (same for all factors).
Definition: GncOptimizer.h:117
void setInlierCostThresholdsAtProbability(const double alpha)
Set the maximum weighted residual error threshold by specifying the probability alpha that the inlier...
Definition: GncOptimizer.h:132
NonlinearFactorGraph makeWeightedGraph(const Vector &weights) const
Create a graph where each factor is weighted by the gnc weights.
Definition: GncOptimizer.h:398
const NonlinearFactorGraph & getFactors() const
Access a copy of the internal factor graph.
Definition: GncOptimizer.h:154
const Vector & getWeights() const
Access a copy of the GNC weights.
Definition: GncOptimizer.h:163
const GncParameters & getParams() const
Access a copy of the parameters.
Definition: GncOptimizer.h:160
GncOptimizer(const NonlinearFactorGraph &graph, const Values &initialValues, const GncParameters &params=GncParameters())
Constructor.
Definition: GncOptimizer.h:59
A nonlinear sum-of-squares factor with a zero-mean noise model implementing the density Templated on...
Definition: NonlinearFactor.h:174
boost::shared_ptr< This > shared_ptr
Noise model.
Definition: NonlinearFactor.h:186
Definition: NonlinearFactorGraph.h:55
bool equals(const NonlinearFactorGraph &other, double tol=1e-9) const
Test equality.
Definition: NonlinearFactorGraph.cpp:97
double error(const Values &values) const
unnormalized error, in the most common case
Definition: NonlinearFactorGraph.cpp:170
A non-templated config holding any types of Manifold-group elements.
Definition: Values.h:65