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


gtsam
Author(s):
autogenerated on Sat May 8 2021 02:43:03