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 
31 #include <boost/math/distributions/chi_squared.hpp>
32 
33 namespace gtsam {
34 /*
35  * Quantile of chi-squared distribution with given degrees of freedom at probability alpha.
36  * Equivalent to chi2inv in Matlab.
37  */
38 static 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 /* ************************************************************************* */
44 template<class GncParameters>
45 class GncOptimizer {
46  public:
48  typedef typename GncParameters::OptimizerType BaseOptimizer;
49 
50  private:
53  GncParameters params_;
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 = std::dynamic_pointer_cast<
69  NoiseModelFactor>(graph[i]);
70  auto robust = std::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.
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 
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 
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)
394  || checkWeightsConvergence(weights) || checkMuConvergence(mu);
395  }
396 
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 = std::dynamic_pointer_cast<
405  NoiseModelFactor>(nfg_[i]);
406  auto noiseModel =
407  std::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) {
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 }
#define max(a, b)
Definition: datatypes.h:20
void setWeights(const Vector w)
Definition: GncOptimizer.h:144
void setInlierCostThresholdsAtProbability(const double alpha)
Definition: GncOptimizer.h:132
Factor Graph consisting of non-linear factors.
const Values & getState() const
Access a copy of the internal values.
Definition: GncOptimizer.h:157
static double Chi2inv(const double alpha, const size_t dofs)
Definition: GncOptimizer.h:38
#define min(a, b)
Definition: datatypes.h:19
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
double mu
bool checkWeightsConvergence(const Vector &weights) const
Check convergence of weights to binary values.
Definition: GncOptimizer.h:364
Eigen::MatrixXd Matrix
Definition: base/Matrix.h:39
NonlinearFactorGraph makeWeightedGraph(const Vector &weights) const
Create a graph where each factor is weighted by the gnc weights.
Definition: GncOptimizer.h:398
double updateMu(const double mu) const
Update the gnc parameter mu to gradually increase nonconvexity.
Definition: GncOptimizer.h:319
iterator iter(handle obj)
Definition: pytypes.h:2273
Values optimize()
Compute optimal solution using graduated non-convexity.
Definition: GncOptimizer.h:185
GncOptimizer(const NonlinearFactorGraph &graph, const Values &initialValues, const GncParameters &params=GncParameters())
Constructor.
Definition: GncOptimizer.h:59
#define isinf(X)
Definition: main.h:94
Real fabs(const Real &a)
NonlinearFactorGraph graph
static const SmartProjectionParams params
const Vector & getInlierCostThresholds() const
Get the inlier threshold.
Definition: GncOptimizer.h:166
size_t size() const
Definition: FactorGraph.h:334
void print(const std::string &str="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const
Definition: Values.cpp:66
const NonlinearFactorGraph & getFactors() const
Access a copy of the internal factor graph.
Definition: GncOptimizer.h:154
Eigen::VectorXd Vector
Definition: Vector.h:38
static shared_ptr Information(const Matrix &M, bool smart=true)
Definition: NoiseModel.cpp:97
Values result
const GncParameters & getParams() const
Access a copy of the parameters.
Definition: GncOptimizer.h:160
Vector weights_
Weights associated to each factor in GNC (this could be a local variable in optimize, but it is useful to make it accessible from outside).
Definition: GncOptimizer.h:54
RealScalar alpha
Array< double, 1, 3 > e(1./3., 0.5, 2.)
bool equals(const NonlinearFactorGraph &other, double tol=1e-9) const
double error(const Values &values) const
RowVector3d w
Vector calculateWeights(const Values &currentEstimate, const double mu)
Calculate gnc weights.
Definition: GncOptimizer.h:423
traits
Definition: chartTesting.h:28
GncParameters::OptimizerType BaseOptimizer
For each parameter, specify the corresponding optimizer: e.g., GaussNewtonParams -> GaussNewtonOptimi...
Definition: GncOptimizer.h:48
std::shared_ptr< This > shared_ptr
virtual void resize(size_t size)
Definition: FactorGraph.h:389
bool equals(const GncOptimizer &other, double tol=1e-9) const
Equals.
Definition: GncOptimizer.h:169
NonlinearFactorGraph nfg_
Original factor graph to be solved by GNC.
Definition: GncOptimizer.h:51
void setInlierCostThresholds(const Vector &inthVec)
Definition: GncOptimizer.h:125
static double error
Definition: testRot3.cpp:37
Jet< T, N > sqrt(const Jet< T, N > &f)
Definition: jet.h:418
const G double tol
Definition: Group.h:86
Vector initializeWeightsFromKnownInliersAndOutliers() const
Definition: GncOptimizer.h:176
GncParameters params_
GNC parameters.
Definition: GncOptimizer.h:53
Jet< T, N > pow(const Jet< T, N > &f, double g)
Definition: jet.h:570
Vector barcSq_
Inlier thresholds. A factor is considered an inlier if factor.error() < barcSq_[i] (where i is the po...
Definition: GncOptimizer.h:55
virtual Matrix information() const
Compute information matrix.
Definition: NoiseModel.cpp:239
const Vector & getWeights() const
Access a copy of the GNC weights.
Definition: GncOptimizer.h:163
bool equal(const T &obj1, const T &obj2, double tol)
Definition: Testable.h:85
Values state_
Initial values to be used at each iteration by GNC.
Definition: GncOptimizer.h:52
void setInlierCostThresholds(const double inth)
Definition: GncOptimizer.h:117
bool checkCostConvergence(const double cost, const double prev_cost) const
Check convergence of relative cost differences.
Definition: GncOptimizer.h:353
double initializeMu() const
Initialize the gnc parameter mu such that loss is approximately convex (remark 5 in GNC paper)...
Definition: GncOptimizer.h:274
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
EIGEN_DEVICE_FUNC const RoundReturnType round() const


gtsam
Author(s):
autogenerated on Tue Jul 4 2023 02:34:19