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/base/Vector.h>
22 #include <gtsam/geometry/Point3.h>
23 #include <gtsam/geometry/Unit3.h>
26 
27 namespace gtsam {
28 
41 class TranslationFactor : public NoiseModelFactorN<Point3, Point3> {
42  private:
45 
46  public:
47  // Provide access to the Matrix& version of evaluateError:
48  using NoiseModelFactor2<Point3, Point3>::evaluateError;
49 
52 
53  TranslationFactor(Key a, Key b, const Unit3& w_aZb,
55  : Base(noiseModel, a, b), measured_w_aZb_(w_aZb.point3()) {}
56 
68  Vector evaluateError(const Point3& Ta, const Point3& Tb,
70  OptionalMatrixType H2) const override {
71  const Point3 dir = Tb - Ta;
72  Matrix33 H_predicted_dir;
73  const Point3 predicted =
74  normalize(dir, H1 || H2 ? &H_predicted_dir : nullptr);
75  if (H1) *H1 = -H_predicted_dir;
76  if (H2) *H2 = H_predicted_dir;
77  return predicted - measured_w_aZb_;
78  }
79 
80  private:
81 #ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
82  friend class boost::serialization::access;
83  template <class ARCHIVE>
84  void serialize(ARCHIVE& ar, const unsigned int /*version*/) {
85  ar& boost::serialization::make_nvp(
86  "Base", boost::serialization::base_object<Base>(*this));
87  }
88 #endif
89 }; // \ TranslationFactor
90 
105  : public NoiseModelFactorN<Point3, Point3, Vector1> {
106  private:
109 
110  public:
113 
115  const Unit3& w_aZb,
117  : Base(noiseModel, a, b, scale_key), measured_w_aZb_(w_aZb.point3()) {}
118 
119  // Provide access to the Matrix& version of evaluateError:
120  using NoiseModelFactor2<Point3, Point3, Vector1>::evaluateError;
121 
133  Vector evaluateError(const Point3& Ta, const Point3& Tb, const Vector1& scale,
135  OptionalMatrixType H3) const override {
136  // Ideally we should use a positive real valued scalar datatype for scale.
137  const double abs_scale = abs(scale[0]);
138  const Point3 predicted = (Tb - Ta) * abs_scale;
139  if (H1) {
140  *H1 = -Matrix3::Identity() * abs_scale;
141  }
142  if (H2) {
143  *H2 = Matrix3::Identity() * abs_scale;
144  }
145  if (H3) {
146  *H3 = scale[0] >= 0 ? (Tb - Ta) : (Ta - Tb);
147  }
148  return predicted - measured_w_aZb_;
149  }
150 
151  private:
152 #ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
153  friend class boost::serialization::access;
154  template <class ARCHIVE>
155  void serialize(ARCHIVE& ar, const unsigned int /*version*/) {
156  ar& boost::serialization::make_nvp(
157  "Base", boost::serialization::base_object<Base>(*this));
158  }
159 #endif
160 }; // \ BilinearAngleTranslationFactor
161 } // namespace gtsam
gtsam::TranslationFactor::Base
NoiseModelFactorN< Point3, Point3 > Base
Definition: TranslationFactor.h:43
Vector.h
typedef and functions to augment Eigen's VectorXd
gtsam::TranslationFactor::evaluateError
Vector evaluateError(const Point3 &Ta, const Point3 &Tb, OptionalMatrixType H1, OptionalMatrixType H2) const override
Calculate error: (norm(Tb - Ta) - measurement) where Tb and Ta are Point3 translations and measuremen...
Definition: TranslationFactor.h:68
gtsam::BilinearAngleTranslationFactor::evaluateError
Vector evaluateError(const Point3 &Ta, const Point3 &Tb, const Vector1 &scale, OptionalMatrixType H1, OptionalMatrixType H2, OptionalMatrixType H3) const override
Calculate error: (scale * (Tb - Ta) - measurement) where Tb and Ta are Point3 translations and measur...
Definition: TranslationFactor.h:133
gtsam::point3
Point3_ point3(const Unit3_ &v)
Definition: slam/expressions.h:101
gtsam::Factor
Definition: Factor.h:70
Point3.h
3D Point
gtsam::Vector
Eigen::VectorXd Vector
Definition: Vector.h:38
gtsam::BilinearAngleTranslationFactor::Base
NoiseModelFactorN< Point3, Point3, Vector1 > Base
Definition: TranslationFactor.h:107
Unit3.h
gtsam::BilinearAngleTranslationFactor::BilinearAngleTranslationFactor
BilinearAngleTranslationFactor()
default constructor
Definition: TranslationFactor.h:112
gtsam::normalize
static void normalize(Signature::Row &row)
Definition: Signature.cpp:88
gtsam::NoiseModelFactor::noiseModel
const SharedNoiseModel & noiseModel() const
access to the noise model
Definition: NonlinearFactor.h:245
gtsam::TranslationFactor::measured_w_aZb_
Point3 measured_w_aZb_
Definition: TranslationFactor.h:44
gtsam::NoiseModelFactorN
Definition: NonlinearFactor.h:431
gtsam::TranslationFactor::TranslationFactor
TranslationFactor()
default constructor
Definition: TranslationFactor.h:51
gtsam::BilinearAngleTranslationFactor::measured_w_aZb_
Point3 measured_w_aZb_
Definition: TranslationFactor.h:108
gtsam::BilinearAngleTranslationFactor::BilinearAngleTranslationFactor
BilinearAngleTranslationFactor(Key a, Key b, Key scale_key, const Unit3 &w_aZb, const SharedNoiseModel &noiseModel)
Definition: TranslationFactor.h:114
gtsam::SharedNoiseModel
noiseModel::Base::shared_ptr SharedNoiseModel
Definition: NoiseModel.h:762
NonlinearFactor.h
Non-linear factor base classes.
gtsam::b
const G & b
Definition: Group.h:79
a
ArrayXXi a
Definition: Array_initializer_list_23_cxx11.cpp:1
gtsam
traits
Definition: SFMdata.h:40
gtsam::TranslationFactor::TranslationFactor
TranslationFactor(Key a, Key b, const Unit3 &w_aZb, const SharedNoiseModel &noiseModel)
Definition: TranslationFactor.h:53
NoiseModel.h
gtsam::scale
static double scale(double x, double a, double b, double t1, double t2)
Scale x from [a, b] to [t1, t2].
Definition: Chebyshev.cpp:35
gtsam::Point3
Vector3 Point3
Definition: Point3.h:38
gtsam::OptionalMatrixType
Matrix * OptionalMatrixType
Definition: NonlinearFactor.h:55
Eigen::Matrix
The matrix class, also used for vectors and row-vectors.
Definition: 3rdparty/Eigen/Eigen/src/Core/Matrix.h:178
abs
#define abs(x)
Definition: datatypes.h:17
gtsam::Unit3
Represents a 3D point on a unit sphere.
Definition: Unit3.h:42
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:97
gtsam::BilinearAngleTranslationFactor
Definition: TranslationFactor.h:104
gtsam::TranslationFactor
Definition: TranslationFactor.h:41


gtsam
Author(s):
autogenerated on Fri Nov 1 2024 03:42:32