NonlinearFactor.cpp
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 
21 
22 namespace gtsam {
23 
24 /* ************************************************************************* */
25 double NonlinearFactor::error(const Values& c) const {
26  throw std::runtime_error("NonlinearFactor::error is not implemented");
27 }
28 
29 /* ************************************************************************* */
30 double NonlinearFactor::error(const HybridValues& c) const {
31  return this->error(c.nonlinear());
32 }
33 
34 /* ************************************************************************* */
35 void NonlinearFactor::print(const std::string& s,
36  const KeyFormatter& keyFormatter) const {
37  std::cout << s << " keys = { ";
38  for(Key key: keys()) {
39  std::cout << keyFormatter(key) << " ";
40  }
41  std::cout << "}" << std::endl;
42 }
43 
44 /* ************************************************************************* */
45 bool NonlinearFactor::equals(const NonlinearFactor& f, double tol) const {
46  return Base::equals(f);
47 }
48 
49 /* ************************************************************************* */
51  const std::map<Key, Key>& rekey_mapping) const {
52  shared_ptr new_factor = clone();
53  for (size_t i = 0; i < new_factor->size(); ++i) {
54  Key& cur_key = new_factor->keys()[i];
55  std::map<Key, Key>::const_iterator mapping = rekey_mapping.find(cur_key);
56  if (mapping != rekey_mapping.end())
57  cur_key = mapping->second;
58  }
59  return new_factor;
60 }
61 
62 /* ************************************************************************* */
64  const KeyVector& new_keys) const {
65  assert(new_keys.size() == keys().size());
66  shared_ptr new_factor = clone();
67  new_factor->keys() = new_keys;
68  return new_factor;
69 }
70 
71 /* ************************************************************************* */
72 void NoiseModelFactor::print(const std::string& s,
73  const KeyFormatter& keyFormatter) const {
74  Base::print(s, keyFormatter);
75  if (noiseModel_)
76  noiseModel_->print(" noise model: ");
77 }
78 
79 /* ************************************************************************* */
80 bool NoiseModelFactor::equals(const NonlinearFactor& f, double tol) const {
81  const NoiseModelFactor* e = dynamic_cast<const NoiseModelFactor*>(&f);
82  return e && Base::equals(f, tol)
83  && ((!noiseModel_ && !e->noiseModel_)
84  || (noiseModel_ && e->noiseModel_
85  && noiseModel_->equals(*e->noiseModel_, tol)));
86 }
87 
88 /* ************************************************************************* */
90  const SharedNoiseModel newNoise) const {
91  NoiseModelFactor::shared_ptr new_factor = std::dynamic_pointer_cast<NoiseModelFactor>(clone());
92  new_factor->noiseModel_ = newNoise;
93  return new_factor;
94 }
95 
96 /* ************************************************************************* */
97 static void check(const SharedNoiseModel& noiseModel, size_t m) {
98  if (noiseModel && m != noiseModel->dim()) {
99  throw std::invalid_argument(
100  "NoiseModelFactor: NoiseModel has dimension " +
101  std::to_string(noiseModel->dim()) +
102  " instead of " + std::to_string(m) + ".");
103  }
104 }
105 
106 /* ************************************************************************* */
108  const Vector b = unwhitenedError(c);
109  check(noiseModel_, b.size());
110  return noiseModel_ ? noiseModel_->whiten(b) : b;
111 }
112 
113 /* ************************************************************************* */
115  const Vector b = unwhitenedError(c);
116  check(noiseModel_, b.size());
117  return noiseModel_ ? noiseModel_->unweightedWhiten(b) : b;
118 }
119 
120 /* ************************************************************************* */
121 double NoiseModelFactor::weight(const Values& c) const {
122  if (active(c)) {
123  if (noiseModel_) {
124  const Vector b = unwhitenedError(c);
125  check(noiseModel_, b.size());
126  return noiseModel_->weight(b);
127  }
128  else
129  return 1.0;
130  } else {
131  return 0.0;
132  }
133 }
134 
135 /* ************************************************************************* */
136 double NoiseModelFactor::error(const Values& c) const {
137  if (active(c)) {
138  const Vector b = unwhitenedError(c);
139  check(noiseModel_, b.size());
140  if (noiseModel_)
141  return noiseModel_->loss(noiseModel_->squaredMahalanobisDistance(b));
142  else
143  return 0.5 * b.squaredNorm();
144  } else {
145  return 0.0;
146  }
147 }
148 
149 /* ************************************************************************* */
150 std::shared_ptr<GaussianFactor> NoiseModelFactor::linearize(
151  const Values& x) const {
152 
153  // Only linearize if the factor is active
154  if (!active(x))
155  return std::shared_ptr<JacobianFactor>();
156 
157  // Call evaluate error to get Jacobians and RHS vector b
158  std::vector<Matrix> A(size());
159  Vector b = -unwhitenedError(x, A);
160  check(noiseModel_, b.size());
161 
162  // Whiten the corresponding system now
163  if (noiseModel_)
164  noiseModel_->WhitenSystem(A, b);
165 
166  // Fill in terms, needed to create JacobianFactor below
167  std::vector<std::pair<Key, Matrix> > terms(size());
168  for (size_t j = 0; j < size(); ++j) {
169  terms[j].first = keys()[j];
170  terms[j].second.swap(A[j]);
171  }
172 
173  // TODO pass unwhitened + noise model to Gaussian factor
175  if (noiseModel_ && noiseModel_->isConstrained())
177  new JacobianFactor(terms, b,
178  std::static_pointer_cast<Constrained>(noiseModel_)->unit()));
179  else {
180  return GaussianFactor::shared_ptr(new JacobianFactor(terms, b));
181  }
182 }
183 
184 /* ************************************************************************* */
185 
186 } // \namespace gtsam
const gtsam::Symbol key('X', 0)
Matrix3f m
virtual double error(const Values &c) const
virtual void print(const std::string &s="Factor", const KeyFormatter &formatter=DefaultKeyFormatter) const
print
Definition: Factor.cpp:29
bool equals(const NonlinearFactor &f, double tol=1e-9) const override
size_t size() const
Definition: Factor.h:159
Scalar Scalar * c
Definition: benchVecAdd.cpp:17
void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
Vector unweightedWhitenedError(const Values &c) const
void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
virtual bool equals(const NonlinearFactor &f, double tol=1e-9) const
Eigen::VectorXd Vector
Definition: Vector.h:38
Point2(* f)(const Point3 &, OptionalJacobian< 2, 3 >)
Array< double, 1, 3 > e(1./3., 0.5, 2.)
RealScalar s
std::shared_ptr< Factor > shared_ptr
A shared_ptr to this class.
Definition: Factor.h:75
double weight(const Values &c) const
std::function< std::string(Key)> KeyFormatter
Typedef for a function to format a key, i.e. to convert it to a string.
Definition: Key.h:35
const G & b
Definition: Group.h:86
std::shared_ptr< GaussianFactor > linearize(const Values &x) const override
std::shared_ptr< This > shared_ptr
shared_ptr to this class
shared_ptr cloneWithNewNoiseModel(const SharedNoiseModel newNoise) const
traits
Definition: chartTesting.h:28
std::shared_ptr< This > shared_ptr
SharedNoiseModel noiseModel_
double error(const Values &c) const override
Non-linear factor base classes.
virtual bool active(const Values &) const
std::shared_ptr< This > shared_ptr
virtual shared_ptr rekey(const std::map< Key, Key > &rekey_mapping) const
const KeyVector & keys() const
Access the factor&#39;s involved variable keys.
Definition: Factor.h:142
const G double tol
Definition: Group.h:86
const Values & nonlinear() const
Return the nonlinear values.
Definition: HybridValues.h:95
FastVector< Key > KeyVector
Define collection type once and for all - also used in wrappers.
Definition: Key.h:86
set noclip points set clip one set noclip two set bar set border lt lw set xdata set ydata set zdata set x2data set y2data set boxwidth set dummy x
virtual shared_ptr clone() const
static void check(const SharedNoiseModel &noiseModel, size_t m)
Vector whitenedError(const Values &c) const
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:102
std::ptrdiff_t j
noiseModel::Base::shared_ptr SharedNoiseModel
Definition: NoiseModel.h:741
bool equals(const This &other, double tol=1e-9) const
check equality
Definition: Factor.cpp:42


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