gtsam  4.1.0
gtsam
ExpressionFactor.h
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 #pragma once
21 
22 #include <array>
23 #include <gtsam/config.h>
24 #include <gtsam/base/Testable.h>
27 #include <numeric>
28 
29 namespace gtsam {
30 
43 template<typename T>
45  BOOST_CONCEPT_ASSERT((IsTestable<T>));
46 
47 protected:
48 
49  typedef ExpressionFactor<T> This;
50  static const int Dim = traits<T>::dimension;
51 
54  FastVector<int> dims_;
55 
56 
57  public:
58  typedef boost::shared_ptr<ExpressionFactor<T> > shared_ptr;
59 
67  ExpressionFactor(const SharedNoiseModel& noiseModel, //
68  const T& measurement, const Expression<T>& expression)
69  : NoiseModelFactor(noiseModel), measured_(measurement) {
71  }
72 
74  virtual ~ExpressionFactor() {}
75 
77  const T& measured() const { return measured_; }
78 
80  void print(const std::string& s = "",
81  const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {
82  NoiseModelFactor::print(s, keyFormatter);
83  traits<T>::Print(measured_, "ExpressionFactor with measurement: ");
84  }
85 
87  bool equals(const NonlinearFactor& f, double tol) const override {
88  const ExpressionFactor* p = dynamic_cast<const ExpressionFactor*>(&f);
89  return p && NoiseModelFactor::equals(f, tol) &&
91  dims_ == p->dims_;
92  }
93 
99  Vector unwhitenedError(const Values& x,
100  boost::optional<std::vector<Matrix>&> H = boost::none) const override {
101  if (H) {
102  const T value = expression_.valueAndDerivatives(x, keys_, dims_, *H);
103  // NOTE(hayk): Doing the reverse, AKA Local(measured_, value) is not correct here
104  // because it would use the tangent space of the measurement instead of the value.
105  return -traits<T>::Local(value, measured_);
106  } else {
107  const T value = expression_.value(x);
108  return -traits<T>::Local(value, measured_);
109  }
110  }
111 
112  boost::shared_ptr<GaussianFactor> linearize(const Values& x) const override {
113  // Only linearize if the factor is active
114  if (!active(x))
115  return boost::shared_ptr<JacobianFactor>();
116 
117  // In case noise model is constrained, we need to provide a noise model
118  SharedDiagonal noiseModel;
119  if (noiseModel_ && noiseModel_->isConstrained()) {
120  noiseModel = boost::static_pointer_cast<noiseModel::Constrained>(
121  noiseModel_)->unit();
122  }
123 
124  // Create a writeable JacobianFactor in advance
125  boost::shared_ptr<JacobianFactor> factor(
126  new JacobianFactor(keys_, dims_, Dim, noiseModel));
127 
128  // Wrap keys and VerticalBlockMatrix into structure passed to expression_
129  VerticalBlockMatrix& Ab = factor->matrixObject();
130  internal::JacobianMap jacobianMap(keys_, Ab);
131 
132  // Zero out Jacobian so we can simply add to it
133  Ab.matrix().setZero();
134 
135  // Get value and Jacobians, writing directly into JacobianFactor
136  T value = expression_.valueAndJacobianMap(x, jacobianMap); // <<< Reverse AD happens here !
137 
138  // Evaluate error and set RHS vector b
139  Ab(size()).col(0) = traits<T>::Local(value, measured_);
140 
141  // Whiten the corresponding system, Ab already contains RHS
142  if (noiseModel_) {
143  Vector b = Ab(size()).col(0); // need b to be valid for Robust noise models
144  noiseModel_->WhitenSystem(Ab.matrix(), b);
145  }
146 
147  return factor;
148  }
149 
151  gtsam::NonlinearFactor::shared_ptr clone() const override {
152  return boost::static_pointer_cast<gtsam::NonlinearFactor>(
153  gtsam::NonlinearFactor::shared_ptr(new This(*this)));
154  }
155 
156 protected:
157  ExpressionFactor() {}
159 
161  ExpressionFactor(const SharedNoiseModel& noiseModel, const T& measurement)
162  : NoiseModelFactor(noiseModel), measured_(measurement) {
163  // Not properly initialized yet, need to call initialize
164  }
165 
168  if (!noiseModel_)
169  throw std::invalid_argument("ExpressionFactor: no NoiseModel.");
170  if (noiseModel_->dim() != Dim)
171  throw std::invalid_argument(
172  "ExpressionFactor was created with a NoiseModel of incorrect dimension.");
174 
175  // Get keys and dimensions for Jacobian matrices
176  // An Expression is assumed unmutable, so we do this now
177  if (keys_.empty()) {
178  // This is the case when called in ExpressionFactor Constructor.
179  // We then take the keys from the expression in sorted order.
180  boost::tie(keys_, dims_) = expression_.keysAndDims();
181  } else {
182  // This happens with classes derived from BinaryExpressionFactor etc.
183  // In that case, the keys_ are already defined and we just need to grab
184  // the dimensions in the correct order.
185  std::map<Key, int> keyedDims;
186  expression_.dims(keyedDims);
187  for (Key key : keys_) dims_.push_back(keyedDims[key]);
188  }
189  }
190 
193  virtual Expression<T> expression() const {
194  throw std::runtime_error("ExpressionFactor::expression not provided: cannot deserialize.");
195  }
196 
197 private:
199  template <class Archive>
200  void save(Archive& ar, const unsigned int /*version*/) const {
201  ar << BOOST_SERIALIZATION_BASE_OBJECT_NVP(NoiseModelFactor);
202  ar << boost::serialization::make_nvp("measured_", this->measured_);
203  }
204 
207  template <class Archive>
208  void load(Archive& ar, const unsigned int /*version*/) {
209  ar >> BOOST_SERIALIZATION_BASE_OBJECT_NVP(NoiseModelFactor);
210  ar >> boost::serialization::make_nvp("measured_", this->measured_);
211  this->initialize(expression());
212  }
213 
214  // Indicate that we implement save/load separately, and be friendly to boost
215  BOOST_SERIALIZATION_SPLIT_MEMBER()
216 
217  friend class boost::serialization::access;
218 
219  // Alignment, see https://eigen.tuxfamily.org/dox/group__TopicStructHavingEigenMembers.html
220  enum { NeedsToAlign = (sizeof(T) % 16) == 0 };
221  public:
223 };
224 // ExpressionFactor
225 
227 template <typename T>
228 struct traits<ExpressionFactor<T> > : public Testable<ExpressionFactor<T> > {};
229 
241 template <typename T, typename... Args>
243 public:
244  static const std::size_t NARY_EXPRESSION_SIZE = sizeof...(Args);
245  using ArrayNKeys = std::array<Key, NARY_EXPRESSION_SIZE>;
246 
248  virtual ~ExpressionFactorN() = default;
249 
250  // Don't provide backward compatible evaluateVector(), due to its problematic
251  // variable length of optional Jacobian arguments. Vector evaluateError(const
252  // Args... args,...);
253 
256  virtual Expression<T> expression(const ArrayNKeys &keys) const {
257  throw std::runtime_error(
258  "ExpressionFactorN::expression not provided: cannot deserialize.");
259  }
260 
261 protected:
263  ExpressionFactorN() = default;
264 
266  ExpressionFactorN(const ArrayNKeys &keys, const SharedNoiseModel &noiseModel,
267  const T &measurement)
268  : ExpressionFactor<T>(noiseModel, measurement) {
269  for (const auto &key : keys)
270  Factor::keys_.push_back(key);
271  }
272 
273 private:
275  Expression<T> expression() const override {
276  ArrayNKeys keys;
277  int idx = 0;
278  for (const auto &key : Factor::keys_)
279  keys[idx++] = key;
280  return expression(keys);
281  }
282 
283  friend class boost::serialization::access;
284  template <class ARCHIVE>
285  void serialize(ARCHIVE &ar, const unsigned int /*version*/) {
286  ar &boost::serialization::make_nvp(
287  "ExpressionFactorN",
288  boost::serialization::base_object<ExpressionFactor<T>>(*this));
289  }
290 };
292 template <typename T, typename... Args>
293 struct traits<ExpressionFactorN<T, Args...>>
294  : public Testable<ExpressionFactorN<T, Args...>> {};
295 // ExpressionFactorN
296 
297 
298 #if defined(GTSAM_ALLOW_DEPRECATED_SINCE_V41)
299 
307 template <typename T, typename A1, typename A2>
308 class ExpressionFactor2 : public ExpressionFactorN<T, A1, A2> {
309 public:
311  ~ExpressionFactor2() override {}
312 
314  Vector evaluateError(const A1 &a1, const A2 &a2,
315  boost::optional<Matrix &> H1 = boost::none,
316  boost::optional<Matrix &> H2 = boost::none) const {
317  Values values;
318  values.insert(this->keys_[0], a1);
319  values.insert(this->keys_[1], a2);
320  std::vector<Matrix> H(2);
321  Vector error = ExpressionFactor<T>::unwhitenedError(values, H);
322  if (H1) (*H1) = H[0];
323  if (H2) (*H2) = H[1];
324  return error;
325  }
326 
329  virtual Expression<T> expression(Key key1, Key key2) const {
330  throw std::runtime_error(
331  "ExpressionFactor2::expression not provided: cannot deserialize.");
332  }
333  virtual Expression<T>
334  expression(const typename ExpressionFactorN<T, A1, A2>::ArrayNKeys &keys)
335  const override {
336  return expression(keys[0], keys[1]);
337  }
338 
339 protected:
341  ExpressionFactor2() {}
342 
344  ExpressionFactor2(Key key1, Key key2, const SharedNoiseModel &noiseModel,
345  const T &measurement)
346  : ExpressionFactorN<T, A1, A2>({key1, key2}, noiseModel, measurement) {}
347 };
348 // ExpressionFactor2
349 #endif
350 
351 } // namespace gtsam
Testable.h
Concept check for values that can be used in unit tests.
gtsam::ExpressionFactor::unwhitenedError
Vector unwhitenedError(const Values &x, boost::optional< std::vector< Matrix > & > H=boost::none) const override
Error function without the NoiseModel, .
Definition: ExpressionFactor.h:99
gtsam::ExpressionFactor::ExpressionFactor
ExpressionFactor(const SharedNoiseModel &noiseModel, const T &measurement, const Expression< T > &expression)
Constructor: creates a factor from a measurement and measurement function.
Definition: ExpressionFactor.h:67
gtsam::NoiseModelFactor::print
void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
Print.
Definition: NonlinearFactor.cpp:63
gtsam::SharedNoiseModel
noiseModel::Base::shared_ptr SharedNoiseModel
Note, deliberately not in noiseModel namespace.
Definition: NoiseModel.h:734
gtsam::traits
A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
Definition: concepts.h:30
gtsam::IsTestable
Definition: Testable.h:57
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:61
gtsam::ExpressionFactor::measured
const T & measured() const
return the measurement
Definition: ExpressionFactor.h:77
gtsam::ExpressionFactorN::expression
virtual Expression< T > expression(const ArrayNKeys &keys) const
Recreate expression from given keys_ and measured_, used in load Needed to deserialize a derived fact...
Definition: ExpressionFactor.h:256
Expression.h
Expressions for Block Automatic Differentiation.
gtsam::Factor::keys_
KeyVector keys_
The keys involved in this factor.
Definition: Factor.h:72
gtsam::NoiseModelFactor
A nonlinear sum-of-squares factor with a zero-mean noise model implementing the density Templated on...
Definition: NonlinearFactor.h:154
gtsam::NoiseModelFactor::equals
bool equals(const NonlinearFactor &f, double tol=1e-9) const override
Check if two factors are equal.
Definition: NonlinearFactor.cpp:71
gtsam::ExpressionFactor::expression
virtual Expression< T > expression() const
Recreate expression from keys_ and measured_, used in load below.
Definition: ExpressionFactor.h:193
gtsam
Global functions in a separate testing namespace.
Definition: chartTesting.h:28
gtsam::ExpressionFactor::~ExpressionFactor
virtual ~ExpressionFactor()
Destructor.
Definition: ExpressionFactor.h:74
gtsam::ExpressionFactor::dims_
FastVector< int > dims_
dimensions of the Jacobian matrices
Definition: ExpressionFactor.h:54
gtsam::Testable
A helper that implements the traits interface for GTSAM types.
Definition: Testable.h:150
gtsam::NonlinearFactor
Nonlinear factor base class.
Definition: NonlinearFactor.h:43
gtsam::ExpressionFactor::equals
bool equals(const NonlinearFactor &f, double tol) const override
equals relies on Testable traits being defined for T
Definition: ExpressionFactor.h:87
NonlinearFactor.h
Non-linear factor base classes.
gtsam::ExpressionFactor::ExpressionFactor
ExpressionFactor(const SharedNoiseModel &noiseModel, const T &measurement)
Default constructor, for serialization.
Definition: ExpressionFactor.h:161
gtsam::KeyFormatter
boost::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::Expression
Expression class that supports automatic differentiation.
Definition: Expression.h:49
GTSAM_MAKE_ALIGNED_OPERATOR_NEW_IF
#define GTSAM_MAKE_ALIGNED_OPERATOR_NEW_IF(NeedsToAlign)
This marks a GTSAM object to require alignment.
Definition: types.h:278
gtsam::ExpressionFactorN
N-ary variadic template for ExpressionFactor meant as a base class for N-ary factors.
Definition: ExpressionFactor.h:242
gtsam::ExpressionFactorN::~ExpressionFactorN
virtual ~ExpressionFactorN()=default
Destructor.
gtsam::ExpressionFactorN::ExpressionFactorN
ExpressionFactorN()=default
Default constructor, for serialization.
gtsam::ExpressionFactor::initialize
void initialize(const Expression< T > &expression)
Initialize with constructor arguments.
Definition: ExpressionFactor.h:167
gtsam::ExpressionFactor::expression_
Expression< T > expression_
the expression that is AD enabled
Definition: ExpressionFactor.h:53
gtsam::Values
A non-templated config holding any types of Manifold-group elements.
Definition: Values.h:71
gtsam::ExpressionFactorN::ExpressionFactorN
ExpressionFactorN(const ArrayNKeys &keys, const SharedNoiseModel &noiseModel, const T &measurement)
Constructor takes care of keys, but still need to call initialize.
Definition: ExpressionFactor.h:266
gtsam::ExpressionFactor::print
void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
print relies on Testable traits being defined for T
Definition: ExpressionFactor.h:80
gtsam::ExpressionFactor
Factor that supports arbitrary expressions via AD.
Definition: ExpressionFactor.h:44
gtsam::ExpressionFactor::clone
gtsam::NonlinearFactor::shared_ptr clone() const override
Definition: ExpressionFactor.h:151
gtsam::ExpressionFactor::measured_
T measured_
the measurement to be compared with the expression
Definition: ExpressionFactor.h:52