gtsam  4.1.0
gtsam
TranslationFactor.h
Go to the documentation of this file.
1 /* ----------------------------------------------------------------------------
2 
3  * GTSAM Copyright 2010-2020, 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 
12 #pragma once
13 
21 #include <gtsam/geometry/Point3.h>
22 #include <gtsam/geometry/Unit3.h>
25 
26 namespace gtsam {
27 
42 class TranslationFactor : public NoiseModelFactor2<Point3, Point3> {
43  private:
45  Point3 measured_w_aZb_;
46 
47  public:
50 
51  TranslationFactor(Key a, Key b, const Unit3& w_aZb,
53  : Base(noiseModel, a, b), measured_w_aZb_(w_aZb.point3()) {}
54 
66  Vector evaluateError(
67  const Point3& Ta, const Point3& Tb,
68  boost::optional<Matrix&> H1 = boost::none,
69  boost::optional<Matrix&> H2 = boost::none) const override {
70  const Point3 dir = Tb - Ta;
71  Matrix33 H_predicted_dir;
72  const Point3 predicted = normalize(dir, H1 || H2 ? &H_predicted_dir : nullptr);
73  if (H1) *H1 = -H_predicted_dir;
74  if (H2) *H2 = H_predicted_dir;
75  return predicted - measured_w_aZb_;
76  }
77 
78  private:
79  friend class boost::serialization::access;
80  template <class ARCHIVE>
81  void serialize(ARCHIVE& ar, const unsigned int /*version*/) {
82  ar& boost::serialization::make_nvp(
83  "Base", boost::serialization::base_object<Base>(*this));
84  }
85 }; // \ TranslationFactor
86 } // namespace gtsam
gtsam::normalize
Point3 normalize(const Point3 &p, OptionalJacobian< 3, 3 > H)
normalize, with optional Jacobian
Definition: Point3.cpp:51
gtsam::SharedNoiseModel
noiseModel::Base::shared_ptr SharedNoiseModel
Note, deliberately not in noiseModel namespace.
Definition: NoiseModel.h:734
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:61
gtsam::NoiseModelFactor::noiseModel
const SharedNoiseModel & noiseModel() const
access to the noise model
Definition: NonlinearFactor.h:203
gtsam::TranslationFactor::evaluateError
Vector evaluateError(const Point3 &Ta, const Point3 &Tb, boost::optional< Matrix & > H1=boost::none, boost::optional< Matrix & > H2=boost::none) const override
Caclulate error: (norm(Tb - Ta) - measurement) where Tb and Ta are Point3 translations and measuremen...
Definition: TranslationFactor.h:66
gtsam
Global functions in a separate testing namespace.
Definition: chartTesting.h:28
NonlinearFactor.h
Non-linear factor base classes.
Point3.h
3D Point
gtsam::TranslationFactor::TranslationFactor
TranslationFactor()
default constructor
Definition: TranslationFactor.h:49
gtsam::TranslationFactor
Definition: TranslationFactor.h:42
gtsam::Unit3
Represents a 3D point on a unit sphere.
Definition: Unit3.h:42
gtsam::Factor
This is the base class for all factor types.
Definition: Factor.h:55
gtsam::NoiseModelFactor2
A convenient base class for creating your own NoiseModelFactor with 2 variables.
Definition: NonlinearFactor.h:355
gtsam::Point3
Vector3 Point3
As of GTSAM 4, in order to make GTSAM more lean, it is now possible to just typedef Point3 to Vector3...
Definition: Point3.h:35
NoiseModel.h