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 RangeFactor.h
14 * @brief Serializable factor induced by a range measurement
15 * @date July 2015
16 * @author Frank Dellaert
17 **/
18
19#pragma once
20
21#include <gtsam/nonlinear/ExpressionFactor.h>
22
23namespace gtsam {
24
25// forward declaration of Range functor, assumed partially specified
26template <typename A1, typename A2>
27struct Range;
28
29/**
30 * Binary factor for a range measurement
31 * Works for any two types A1,A2 for which the functor Range<A1,A2>() is defined
32 * @ingroup sam
33 */
34template <typename A1, typename A2 = A1, typename T = double>
35class RangeFactor : public ExpressionFactorN<T, A1, A2> {
36 private:
37 typedef RangeFactor<A1, A2> This;
38 typedef ExpressionFactorN<T, A1, A2> Base;
39
40 public:
41 /// default constructor
42 RangeFactor() {}
43
44 RangeFactor(Key key1, Key key2, T measured, const SharedNoiseModel& model)
45 : Base({key1, key2}, model, measured) {
46 this->initialize(expression(keys: {key1, key2}));
47 }
48
49 /// @return a deep copy of this factor
50 gtsam::NonlinearFactor::shared_ptr clone() const override {
51 return std::static_pointer_cast<gtsam::NonlinearFactor>(
52 r: gtsam::NonlinearFactor::shared_ptr(new This(*this)));
53 }
54
55 // Return measurement expression
56 Expression<T> expression(const typename Base::ArrayNKeys& keys) const override {
57 Expression<A1> a1_(keys[0]);
58 Expression<A2> a2_(keys[1]);
59 return Expression<T>(Range<A1, A2>(), a1_, a2_);
60 }
61
62 Vector evaluateError(const A1& a1, const A2& a2,
63 OptionalMatrixType H1 = OptionalNone,
64 OptionalMatrixType H2 = OptionalNone) const {
65 std::vector<Matrix> Hs(2);
66 const auto& keys = Factor::keys();
67 const Vector error = Base::unwhitenedError(
68 {{keys[0], genericValue(a1)}, {keys[1], genericValue(a2)}},
69 Hs);
70 if (H1) *H1 = Hs[0];
71 if (H2) *H2 = Hs[1];
72 return error;
73 }
74
75 /// print
76 void print(const std::string& s = "",
77 const KeyFormatter& kf = DefaultKeyFormatter) const override {
78 std::cout << s << "RangeFactor" << std::endl;
79 Base::print(s, kf);
80 }
81
82 private:
83#if GTSAM_ENABLE_BOOST_SERIALIZATION
84 friend class boost::serialization::access;
85 template <class ARCHIVE>
86 void serialize(ARCHIVE& ar, const unsigned int /*version*/) {
87 ar& boost::serialization::make_nvp(
88 "Base", boost::serialization::base_object<Base>(*this));
89 }
90#endif
91}; // \ RangeFactor
92
93/// traits
94template <typename A1, typename A2, typename T>
95struct traits<RangeFactor<A1, A2, T> >
96 : public Testable<RangeFactor<A1, A2, T> > {};
97
98/**
99 * Binary factor for a range measurement, with a transform applied
100 * @ingroup sam
101 */
102template <typename A1, typename A2 = A1,
103 typename T = typename Range<A1, A2>::result_type>
104class RangeFactorWithTransform : public ExpressionFactorN<T, A1, A2> {
105 private:
106 typedef RangeFactorWithTransform<A1, A2> This;
107 typedef ExpressionFactorN<T, A1, A2> Base;
108
109 A1 body_T_sensor_; ///< The pose of the sensor in the body frame
110
111 public:
112 //// Default constructor
113 RangeFactorWithTransform() {}
114
115 RangeFactorWithTransform(Key key1, Key key2, T measured,
116 const SharedNoiseModel& model,
117 const A1& body_T_sensor)
118 : Base({key1, key2}, model, measured), body_T_sensor_(body_T_sensor) {
119 this->initialize(expression(keys: {key1, key2}));
120 }
121
122 ~RangeFactorWithTransform() override {}
123
124 /// @return a deep copy of this factor
125 gtsam::NonlinearFactor::shared_ptr clone() const override {
126 return std::static_pointer_cast<gtsam::NonlinearFactor>(
127 r: gtsam::NonlinearFactor::shared_ptr(new This(*this)));
128 }
129
130 // Return measurement expression
131 Expression<T> expression(const typename Base::ArrayNKeys& keys) const override {
132 Expression<A1> body_T_sensor__(body_T_sensor_);
133 Expression<A1> nav_T_body_(keys[0]);
134 Expression<A1> nav_T_sensor_(traits<A1>::Compose, nav_T_body_,
135 body_T_sensor__);
136 Expression<A2> a2_(keys[1]);
137 return Expression<T>(Range<A1, A2>(), nav_T_sensor_, a2_);
138 }
139
140 Vector evaluateError(const A1& a1, const A2& a2,
141 OptionalMatrixType H1 = OptionalNone, OptionalMatrixType H2 = OptionalNone) const {
142 std::vector<Matrix> Hs(2);
143 const auto &keys = Factor::keys();
144 const Vector error = Base::unwhitenedError(
145 {{keys[0], genericValue(a1)}, {keys[1], genericValue(a2)}},
146 Hs);
147 if (H1) *H1 = Hs[0];
148 if (H2) *H2 = Hs[1];
149 return error;
150 }
151
152 // An evaluateError overload to accept matrices (Matrix&) and pass it to the
153 // OptionalMatrixType evaluateError overload
154 Vector evaluateError(const A1& a1, const A2& a2, Matrix& H1, Matrix& H2) const {
155 return evaluateError(a1, a2, &H1, &H2);
156 }
157
158 /** print contents */
159 void print(const std::string& s = "",
160 const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {
161 std::cout << s << "RangeFactorWithTransform" << std::endl;
162 this->body_T_sensor_.print(" sensor pose in body frame: ");
163 Base::print(s, keyFormatter);
164 }
165
166 private:
167#if GTSAM_ENABLE_BOOST_SERIALIZATION
168 friend class boost::serialization::access;
169 /** Serialization function */
170 template <typename ARCHIVE>
171 void serialize(ARCHIVE& ar, const unsigned int /*version*/) {
172 // **IMPORTANT** We need to (de)serialize parameters before the base class,
173 // since it calls expression() and we need all parameters ready at that
174 // point.
175 ar& BOOST_SERIALIZATION_NVP(body_T_sensor_);
176 ar& boost::serialization::make_nvp(
177 "Base", boost::serialization::base_object<Base>(*this));
178 }
179#endif
180}; // \ RangeFactorWithTransform
181
182/// traits
183template <typename A1, typename A2, typename T>
184struct traits<RangeFactorWithTransform<A1, A2, T> >
185 : public Testable<RangeFactorWithTransform<A1, A2, T> > {};
186
187} // \ namespace gtsam
188