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
12/**
13 * @file BetweenFactor.h
14 * @author Frank Dellaert, Viorela Ila
15 **/
16#pragma once
17
18#include <ostream>
19
20#include <gtsam/nonlinear/NonlinearFactor.h>
21#include <gtsam/geometry/concepts.h>
22#include <gtsam/base/Testable.h>
23
24namespace gtsam {
25
26 /**
27 * A class for a measurement predicted by "between(config[key1],config[key2])"
28 * @tparam POSE the Pose type
29 * @ingroup slam
30 */
31 template<class POSE>
32 class PoseBetweenFactor: public NoiseModelFactorN<POSE, POSE> {
33
34 private:
35
36 typedef PoseBetweenFactor<POSE> This;
37 typedef NoiseModelFactorN<POSE, POSE> Base;
38
39 POSE measured_; /** The measurement */
40 std::optional<POSE> body_P_sensor_; ///< The pose of the sensor in the body frame
41
42 /** concept check by type */
43 GTSAM_CONCEPT_TESTABLE_TYPE(POSE)
44 GTSAM_CONCEPT_POSE_TYPE(POSE)
45 public:
46
47 // Provide access to the Matrix& version of evaluateError:
48 using Base::evaluateError;
49
50 // shorthand for a smart pointer to a factor
51 typedef typename std::shared_ptr<PoseBetweenFactor> shared_ptr;
52
53 /** default constructor - only use for serialization */
54 PoseBetweenFactor() {}
55
56 /** Constructor */
57 PoseBetweenFactor(Key key1, Key key2, const POSE& measured,
58 const SharedNoiseModel& model, std::optional<POSE> body_P_sensor = {}) :
59 Base(model, key1, key2), measured_(measured), body_P_sensor_(body_P_sensor) {
60 }
61
62 ~PoseBetweenFactor() override {}
63
64 /// @return a deep copy of this factor
65 gtsam::NonlinearFactor::shared_ptr clone() const override {
66 return std::static_pointer_cast<gtsam::NonlinearFactor>(
67 r: gtsam::NonlinearFactor::shared_ptr(new This(*this))); }
68
69 /** implement functions needed for Testable */
70
71 /** print */
72 void print(const std::string& s, const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {
73 std::cout << s << "BetweenFactor("
74 << keyFormatter(this->key1()) << ","
75 << keyFormatter(this->key2()) << ")\n";
76 measured_.print(" measured: ");
77 if(this->body_P_sensor_)
78 this->body_P_sensor_->print(" sensor pose in body frame: ");
79 this->noiseModel_->print(" noise model: ");
80 }
81
82 /** equals */
83 bool equals(const NonlinearFactor& expected, double tol=1e-9) const override {
84 const This *e = dynamic_cast<const This*> (&expected);
85 return e != nullptr
86 && Base::equals(*e, tol)
87 && this->measured_.equals(e->measured_, tol)
88 && ((!body_P_sensor_ && !e->body_P_sensor_) || (body_P_sensor_ && e->body_P_sensor_ && body_P_sensor_->equals(*e->body_P_sensor_)));
89 }
90
91 /** implement functions needed to derive from Factor */
92
93 /** vector of errors */
94 Vector evaluateError(const POSE& p1, const POSE& p2,
95 OptionalMatrixType H1, OptionalMatrixType H2) const override {
96 if(body_P_sensor_) {
97 POSE hx;
98 if(H1 || H2) {
99 Matrix H3;
100 hx = p1.compose(*body_P_sensor_,H3).between(p2.compose(*body_P_sensor_), H1, H2); // h(x)
101 if(H1) (*H1) *= H3;
102 if(H2) (*H2) *= H3;
103 } else {
104 hx = p1.compose(*body_P_sensor_).between(p2.compose(*body_P_sensor_)); // h(x)
105 }
106 // manifold equivalent of h(x)-z -> log(z,h(x))
107 return measured_.localCoordinates(hx);
108 } else {
109 POSE hx = p1.between(p2, H1, H2); // h(x)
110 // manifold equivalent of h(x)-z -> log(z,h(x))
111 return measured_.localCoordinates(hx);
112 }
113 }
114
115 /** return the measured */
116 const POSE& measured() const {
117 return measured_;
118 }
119
120 private:
121
122#if GTSAM_ENABLE_BOOST_SERIALIZATION
123 /** Serialization function */
124 friend class boost::serialization::access;
125 template<class ARCHIVE>
126 void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
127 ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
128 ar & BOOST_SERIALIZATION_NVP(measured_);
129
130 }
131#endif
132 }; // \class PoseBetweenFactor
133
134} /// namespace gtsam
135