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
gtsam::NonlinearFactor::shared_ptr
std::shared_ptr< This > shared_ptr
Definition: NonlinearFactor.h:78
gtsam::HybridValues
Definition: HybridValues.h:37
s
RealScalar s
Definition: level1_cplx_impl.h:126
e
Array< double, 1, 3 > e(1./3., 0.5, 2.)
gtsam::NoiseModelFactor::error
double error(const Values &c) const override
Definition: NonlinearFactor.cpp:136
c
Scalar Scalar * c
Definition: benchVecAdd.cpp:17
x
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
Definition: gnuplot_common_settings.hh:12
gtsam::JacobianFactor
Definition: JacobianFactor.h:91
gtsam::NonlinearFactor::equals
virtual bool equals(const NonlinearFactor &f, double tol=1e-9) const
Definition: NonlinearFactor.cpp:45
gtsam::NoiseModelFactor::shared_ptr
std::shared_ptr< This > shared_ptr
Definition: NonlinearFactor.h:209
gtsam::NonlinearFactor::rekey
virtual shared_ptr rekey(const std::map< Key, Key > &rekey_mapping) const
Definition: NonlinearFactor.cpp:50
gtsam::NoiseModelFactor::noiseModel_
SharedNoiseModel noiseModel_
Definition: NonlinearFactor.h:205
gtsam::Factor::shared_ptr
std::shared_ptr< Factor > shared_ptr
A shared_ptr to this class.
Definition: Factor.h:76
gtsam::NonlinearFactor::active
virtual bool active(const Values &) const
Definition: NonlinearFactor.h:141
gtsam::Vector
Eigen::VectorXd Vector
Definition: Vector.h:38
gtsam::KeyVector
FastVector< Key > KeyVector
Define collection type once and for all - also used in wrappers.
Definition: Key.h:92
gtsam::noiseModel::Constrained
Definition: NoiseModel.h:404
gtsam::NoiseModelFactor::linearize
std::shared_ptr< GaussianFactor > linearize(const Values &x) const override
Definition: NonlinearFactor.cpp:150
gtsam::NoiseModelFactor::whitenedError
Vector whitenedError(const Values &c) const
Definition: NonlinearFactor.cpp:107
A
Definition: test_numpy_dtypes.cpp:298
j
std::ptrdiff_t j
Definition: tut_arithmetic_redux_minmax.cpp:2
gtsam::KeyFormatter
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
gtsam::HybridValues::nonlinear
const Values & nonlinear() const
Return the nonlinear values.
Definition: HybridValues.cpp:60
gtsam::GaussianFactor::shared_ptr
std::shared_ptr< This > shared_ptr
shared_ptr to this class
Definition: GaussianFactor.h:42
gtsam::check
static void check(const SharedNoiseModel &noiseModel, size_t m)
Definition: NonlinearFactor.cpp:97
gtsam::NoiseModelFactor::print
void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
Definition: NonlinearFactor.cpp:72
gtsam::NoiseModelFactor::equals
bool equals(const NonlinearFactor &f, double tol=1e-9) const override
Definition: NonlinearFactor.cpp:80
m
Matrix3f m
Definition: AngleAxis_mimic_euler.cpp:1
gtsam::NoiseModelFactor::weight
double weight(const Values &c) const
Definition: NonlinearFactor.cpp:121
gtsam::SharedNoiseModel
noiseModel::Base::shared_ptr SharedNoiseModel
Definition: NoiseModel.h:762
gtsam::NonlinearFactor::print
void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
Definition: NonlinearFactor.cpp:35
key
const gtsam::Symbol key('X', 0)
tree::f
Point2(* f)(const Point3 &, OptionalJacobian< 2, 3 >)
Definition: testExpression.cpp:218
gtsam::NoiseModelFactor::unweightedWhitenedError
Vector unweightedWhitenedError(const Values &c) const
Definition: NonlinearFactor.cpp:114
NonlinearFactor.h
Non-linear factor base classes.
gtsam::b
const G & b
Definition: Group.h:79
gtsam::NoiseModelFactor::cloneWithNewNoiseModel
shared_ptr cloneWithNewNoiseModel(const SharedNoiseModel newNoise) const
Definition: NonlinearFactor.cpp:89
gtsam
traits
Definition: SFMdata.h:40
gtsam::NoiseModelFactor
Definition: NonlinearFactor.h:197
gtsam::Values
Definition: Values.h:65
gtsam::Factor::keys
const KeyVector & keys() const
Access the factor's involved variable keys.
Definition: Factor.h:143
gtsam::Factor::equals
bool equals(const This &other, double tol=1e-9) const
check equality
Definition: Factor.cpp:42
gtsam::NonlinearFactor
Definition: NonlinearFactor.h:68
gtsam::tol
const G double tol
Definition: Group.h:79
gtsam::Factor::size
size_t size() const
Definition: Factor.h:160
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:97
gtsam::NonlinearFactor::clone
virtual shared_ptr clone() const
Definition: NonlinearFactor.h:153
HybridValues.h
gtsam::NoiseModelFactor::unwhitenedError
virtual Vector unwhitenedError(const Values &x, OptionalMatrixVecType H=nullptr) const =0
i
int i
Definition: BiCGSTAB_step_by_step.cpp:9
gtsam::NonlinearFactor::error
virtual double error(const Values &c) const
Definition: NonlinearFactor.cpp:25


gtsam
Author(s):
autogenerated on Fri Nov 1 2024 03:33:50