gtsam 4.1.1
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>
45class GTSAM_EXPORT GncOptimizer {
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_
108 setInlierCostThresholdsAtProbability(alpha);
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(graph_initial, state_);
188 Values result = baseOptimizer.optimize();
189 double mu = initializeMu();
190 double prev_cost = graph_initial.error(result);
191 double cost = 0.0; // this will be updated in the main loop
192
193 // handle the degenerate case that corresponds to small
194 // maximum residual errors at initialization
195 // For GM: if residual error is small, mu -> 0
196 // For TLS: if residual error is small, mu -> -1
197 int nrUnknownInOrOut = nfg_.size() - ( params_.knownInliers.size() + params_.knownOutliers.size() );
198 // ^^ number of measurements that are not known to be inliers or outliers (GNC will need to figure them out)
199 if (mu <= 0 || nrUnknownInOrOut == 0) { // no need to even call GNC in this case
200 if (mu <= 0 && params_.verbosity >= GncParameters::Verbosity::SUMMARY) {
201 std::cout << "GNC Optimizer stopped because maximum residual at "
202 "initialization is small."
203 << std::endl;
204 }
205 if (nrUnknownInOrOut==0 && params_.verbosity >= GncParameters::Verbosity::SUMMARY) {
206 std::cout << "GNC Optimizer stopped because all measurements are already known to be inliers or outliers"
207 << std::endl;
208 }
209 if (params_.verbosity >= GncParameters::Verbosity::VALUES) {
210 result.print("result\n");
211 std::cout << "mu: " << mu << std::endl;
212 }
213 return result;
214 }
215
216 size_t iter;
217 for (iter = 0; iter < params_.maxIterations; iter++) {
218
219 // display info
220 if (params_.verbosity >= GncParameters::Verbosity::VALUES) {
221 std::cout << "iter: " << iter << std::endl;
222 result.print("result\n");
223 std::cout << "mu: " << mu << std::endl;
224 std::cout << "weights: " << weights_ << std::endl;
225 }
226 // weights update
227 weights_ = calculateWeights(result, mu);
228
229 // variable/values update
230 NonlinearFactorGraph graph_iter = this->makeWeightedGraph(weights_);
231 BaseOptimizer baseOptimizer_iter(graph_iter, state_);
232 result = baseOptimizer_iter.optimize();
233
234 // stopping condition
235 cost = graph_iter.error(result);
236 if (checkConvergence(mu, weights_, cost, prev_cost)) {
237 break;
238 }
239
240 // update mu
241 mu = updateMu(mu);
242
243 // get ready for next iteration
244 prev_cost = cost;
245
246 // display info
247 if (params_.verbosity >= GncParameters::Verbosity::VALUES) {
248 std::cout << "previous cost: " << prev_cost << std::endl;
249 std::cout << "current cost: " << cost << std::endl;
250 }
251 }
252 // display info
253 if (params_.verbosity >= GncParameters::Verbosity::SUMMARY) {
254 std::cout << "final iterations: " << iter << std::endl;
255 std::cout << "final mu: " << mu << std::endl;
256 std::cout << "final weights: " << weights_ << std::endl;
257 std::cout << "previous cost: " << prev_cost << std::endl;
258 std::cout << "current cost: " << cost << std::endl;
259 }
260 return result;
261 }
262
264 double initializeMu() const {
265
266 double mu_init = 0.0;
267 // initialize mu to the value specified in Remark 5 in GNC paper.
268 switch (params_.lossType) {
269 case GncLossType::GM:
270 /* surrogate cost is convex for large mu. initialize as in remark 5 in GNC paper.
271 Since barcSq_ can be different for each factor, we compute the max of the quantity in remark 5 in GNC paper
272 */
273 for (size_t k = 0; k < nfg_.size(); k++) {
274 if (nfg_[k]) {
275 mu_init = std::max(mu_init, 2 * nfg_[k]->error(state_) / barcSq_[k]);
276 }
277 }
278 return mu_init; // initial mu
279 case GncLossType::TLS:
280 /* surrogate cost is convex for mu close to zero. initialize as in remark 5 in GNC paper.
281 degenerate case: 2 * rmax_sq - params_.barcSq < 0 (handled in the main loop)
282 according to remark mu = params_.barcSq / (2 * rmax_sq - params_.barcSq) = params_.barcSq/ excessResidual
283 however, if the denominator is 0 or negative, we return mu = -1 which leads to termination of the main GNC loop.
284 Since barcSq_ can be different for each factor, we look for the minimimum (positive) quantity in remark 5 in GNC paper
285 */
286 mu_init = std::numeric_limits<double>::infinity();
287 for (size_t k = 0; k < nfg_.size(); k++) {
288 if (nfg_[k]) {
289 double rk = nfg_[k]->error(state_);
290 mu_init = (2 * rk - barcSq_[k]) > 0 ? // if positive, update mu, otherwise keep same
291 std::min(mu_init, barcSq_[k] / (2 * rk - barcSq_[k]) ) : mu_init;
292 }
293 }
294 return mu_init > 0 && !std::isinf(mu_init) ? mu_init : -1; // if mu <= 0 or mu = inf, return -1,
295 // which leads to termination of the main gnc loop. In this case, all residuals are already below the threshold
296 // and there is no need to robustify (TLS = least squares)
297 default:
298 throw std::runtime_error(
299 "GncOptimizer::initializeMu: called with unknown loss type.");
300 }
301 }
302
304 double updateMu(const double mu) const {
305 switch (params_.lossType) {
306 case GncLossType::GM:
307 // reduce mu, but saturate at 1 (original cost is recovered for mu -> 1)
308 return std::max(1.0, mu / params_.muStep);
309 case GncLossType::TLS:
310 // increases mu at each iteration (original cost is recovered for mu -> inf)
311 return mu * params_.muStep;
312 default:
313 throw std::runtime_error(
314 "GncOptimizer::updateMu: called with unknown loss type.");
315 }
316 }
317
319 bool checkMuConvergence(const double mu) const {
320 bool muConverged = false;
321 switch (params_.lossType) {
322 case GncLossType::GM:
323 muConverged = std::fabs(mu - 1.0) < 1e-9; // mu=1 recovers the original GM function
324 break;
325 case GncLossType::TLS:
326 muConverged = false; // for TLS there is no stopping condition on mu (it must tend to infinity)
327 break;
328 default:
329 throw std::runtime_error(
330 "GncOptimizer::checkMuConvergence: called with unknown loss type.");
331 }
332 if (muConverged && params_.verbosity >= GncParameters::Verbosity::SUMMARY)
333 std::cout << "muConverged = true " << std::endl;
334 return muConverged;
335 }
336
338 bool checkCostConvergence(const double cost, const double prev_cost) const {
339 bool costConverged = std::fabs(cost - prev_cost) / std::max(prev_cost, 1e-7)
340 < params_.relativeCostTol;
341 if (costConverged && params_.verbosity >= GncParameters::Verbosity::SUMMARY)
342 std::cout << "checkCostConvergence = true " << std::endl;
343 return costConverged;
344 }
345
347 bool checkWeightsConvergence(const Vector& weights) const {
348 bool weightsConverged = false;
349 switch (params_.lossType) {
350 case GncLossType::GM:
351 weightsConverged = false; // for GM, there is no clear binary convergence for the weights
352 break;
353 case GncLossType::TLS:
354 weightsConverged = true;
355 for (int i = 0; i < weights.size(); i++) {
356 if (std::fabs(weights[i] - std::round(weights[i]))
357 > params_.weightsTol) {
358 weightsConverged = false;
359 break;
360 }
361 }
362 break;
363 default:
364 throw std::runtime_error(
365 "GncOptimizer::checkWeightsConvergence: called with unknown loss type.");
366 }
367 if (weightsConverged
368 && params_.verbosity >= GncParameters::Verbosity::SUMMARY)
369 std::cout << "weightsConverged = true " << std::endl;
370 return weightsConverged;
371 }
372
374 bool checkConvergence(const double mu, const Vector& weights,
375 const double cost, const double prev_cost) const {
376 return checkCostConvergence(cost, prev_cost)
377 || checkWeightsConvergence(weights) || checkMuConvergence(mu);
378 }
379
381 NonlinearFactorGraph makeWeightedGraph(const Vector& weights) const {
382 // make sure all noiseModels are Gaussian or convert to Gaussian
383 NonlinearFactorGraph newGraph;
384 newGraph.resize(nfg_.size());
385 for (size_t i = 0; i < nfg_.size(); i++) {
386 if (nfg_[i]) {
387 auto factor = boost::dynamic_pointer_cast<
388 NoiseModelFactor>(nfg_[i]);
389 auto noiseModel =
390 boost::dynamic_pointer_cast<noiseModel::Gaussian>(
391 factor->noiseModel());
392 if (noiseModel) {
393 Matrix newInfo = weights[i] * noiseModel->information();
394 auto newNoiseModel = noiseModel::Gaussian::Information(newInfo);
395 newGraph[i] = factor->cloneWithNewNoiseModel(newNoiseModel);
396 } else {
397 throw std::runtime_error(
398 "GncOptimizer::makeWeightedGraph: unexpected non-Gaussian noise model.");
399 }
400 }
401 }
402 return newGraph;
403 }
404
406 Vector calculateWeights(const Values& currentEstimate, const double mu) {
407 Vector weights = initializeWeightsFromKnownInliersAndOutliers();
408
409 // do not update the weights that the user has decided are known inliers
410 std::vector<size_t> allWeights;
411 for (size_t k = 0; k < nfg_.size(); k++) {
412 allWeights.push_back(k);
413 }
414 std::vector<size_t> knownWeights;
415 std::set_union(params_.knownInliers.begin(), params_.knownInliers.end(),
416 params_.knownOutliers.begin(), params_.knownOutliers.end(),
417 std::inserter(knownWeights, knownWeights.begin()));
418
419 std::vector<size_t> unknownWeights;
420 std::set_difference(allWeights.begin(), allWeights.end(),
421 knownWeights.begin(), knownWeights.end(),
422 std::inserter(unknownWeights, unknownWeights.begin()));
423
424 // update weights of known inlier/outlier measurements
425 switch (params_.lossType) {
426 case GncLossType::GM: { // use eq (12) in GNC paper
427 for (size_t k : unknownWeights) {
428 if (nfg_[k]) {
429 double u2_k = nfg_[k]->error(currentEstimate); // squared (and whitened) residual
430 weights[k] = std::pow(
431 (mu * barcSq_[k]) / (u2_k + mu * barcSq_[k]), 2);
432 }
433 }
434 return weights;
435 }
436 case GncLossType::TLS: { // use eq (14) in GNC paper
437 double upperbound = (mu + 1) / mu * barcSq_.maxCoeff();
438 double lowerbound = mu / (mu + 1) * barcSq_.minCoeff();
439 for (size_t k : unknownWeights) {
440 if (nfg_[k]) {
441 double u2_k = nfg_[k]->error(currentEstimate); // squared (and whitened) residual
442 if (u2_k >= upperbound) {
443 weights[k] = 0;
444 } else if (u2_k <= lowerbound) {
445 weights[k] = 1;
446 } else {
447 weights[k] = std::sqrt(barcSq_[k] * mu * (mu + 1) / u2_k)
448 - mu;
449 }
450 }
451 }
452 return weights;
453 }
454 default:
455 throw std::runtime_error(
456 "GncOptimizer::calculateWeights: called with unknown loss type.");
457 }
458 }
459};
460
461}
Factor Graph consisting of non-linear factors.
Global functions in a separate testing namespace.
Definition: chartTesting.h:28
bool checkConvergence(double relativeErrorTreshold, double absoluteErrorTreshold, double errorThreshold, double currentError, double newError, NonlinearOptimizerParams::Verbosity verbosity)
Check whether the relative error decrease is less than relativeErrorTreshold, the absolute error decr...
Definition: NonlinearOptimizer.cpp:185
bool equal(const T &obj1, const T &obj2, double tol)
Call equal on the object.
Definition: Testable.h:84
void resize(size_t size)
Directly resize the number of factors in the graph.
Definition: FactorGraph.h:357
size_t size() const
return the number of factors (including any null factors set by remove() ).
Definition: FactorGraph.h:305
static shared_ptr Information(const Matrix &M, bool smart=true)
A Gaussian noise model created by specifying an information matrix.
Definition: NoiseModel.cpp:99
Base class for robust error models The robust M-estimators above simply tell us how to re-weight the ...
Definition: NoiseModel.h:656
Definition: GncOptimizer.h:45
bool checkWeightsConvergence(const Vector &weights) const
Check convergence of weights to binary values.
Definition: GncOptimizer.h:347
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:319
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:338
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:406
double initializeMu() const
Initialize the gnc parameter mu such that loss is approximately convex (remark 5 in GNC paper).
Definition: GncOptimizer.h:264
double updateMu(const double mu) const
Update the gnc parameter mu to gradually increase nonconvexity.
Definition: GncOptimizer.h:304
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:374
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:381
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:162
boost::shared_ptr< This > shared_ptr
Noise model.
Definition: NonlinearFactor.h:174
A non-linear factor graph is a graph of non-Gaussian, i.e.
Definition: NonlinearFactorGraph.h:78
bool equals(const NonlinearFactorGraph &other, double tol=1e-9) const
Test equality.
Definition: NonlinearFactorGraph.cpp:89
double error(const Values &values) const
unnormalized error, in the most common case
Definition: NonlinearFactorGraph.cpp:271
A non-templated config holding any types of Manifold-group elements.
Definition: Values.h:63