FrobeniusFactor.cpp
Go to the documentation of this file.
1 /* ----------------------------------------------------------------------------
2 
3  * GTSAM Copyright 2010-2019, 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 
20 
21 using namespace std;
22 
23 namespace gtsam {
24 
25 //******************************************************************************
27 ConvertNoiseModel(const SharedNoiseModel &model, size_t d, bool defaultToUnit) {
28  double sigma = 1.0;
29 
30  if (model != nullptr) {
31  const auto &robust = std::dynamic_pointer_cast<noiseModel::Robust>(model);
32  Vector sigmas;
33  if (robust) {
34  sigmas = robust->noise()->sigmas();
35  } else {
36  sigmas = model->sigmas();
37  }
38 
39  size_t n = sigmas.size();
40  if (n == 1) {
41  sigma = sigmas(0); // Rot2
42  goto exit;
43  }
44  else if (n == 3 || n == 6) {
45  sigma = sigmas(2); // Pose2, Rot3, or Pose3
46  if (sigmas(0) != sigma || sigmas(1) != sigma) {
47  if (!defaultToUnit) {
48  throw std::runtime_error("Can only convert isotropic rotation noise");
49  }
50  }
51  goto exit;
52  }
53  if (!defaultToUnit) {
54  throw std::runtime_error("Can only convert Pose2/Pose3 noise models");
55  }
56  }
57  exit:
58  auto isoModel = noiseModel::Isotropic::Sigma(d, sigma);
59  const auto &robust = std::dynamic_pointer_cast<noiseModel::Robust>(model);
60  if (robust) {
61  return noiseModel::Robust::Create(
62  noiseModel::mEstimator::Huber::Create(1.345), isoModel);
63  } else {
64  return std::move(isoModel);
65  }
66 }
67 
68 //******************************************************************************
69 
70 } // namespace gtsam
noiseModel::Diagonal::shared_ptr model
int n
SharedNoiseModel ConvertNoiseModel(const SharedNoiseModel &model, size_t d, bool defaultToUnit)
Definition: BFloat16.h:88
Eigen::VectorXd Vector
Definition: Vector.h:38
Various factors that minimize some Frobenius norm.
traits
Definition: chartTesting.h:28
static const double sigma
noiseModel::Base::shared_ptr SharedNoiseModel
Definition: NoiseModel.h:741


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