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 TransformBtwRobotsUnaryFactor.h
14 * @brief Unary factor for determining transformation between given trajectories of two robots
15 * @author Vadim Indelman
16 **/
17#pragma once
18
19#include <gtsam/slam/BetweenFactor.h>
20#include <gtsam/nonlinear/NonlinearFactor.h>
21#include <gtsam/linear/GaussianFactor.h>
22#include <gtsam/base/Testable.h>
23#include <gtsam/base/Lie.h>
24
25#include <ostream>
26
27namespace gtsam {
28
29 /**
30 * A class for a measurement predicted by "between(config[key1],config[key2])"
31 * @tparam VALUE the Value type
32 * @ingroup slam
33 */
34 template<class VALUE>
35 class TransformBtwRobotsUnaryFactor: public NonlinearFactor { // TODO why not NoiseModelFactorN ?
36
37 public:
38
39 typedef VALUE T;
40
41 private:
42
43 typedef TransformBtwRobotsUnaryFactor<VALUE> This;
44 typedef gtsam::NonlinearFactor Base;
45
46 gtsam::Key key_;
47
48 VALUE measured_; /** The measurement */
49
50 gtsam::Values valA_; // given values for robot A map\trajectory
51 gtsam::Values valB_; // given values for robot B map\trajectory
52 gtsam::Key keyA_; // key of robot A to which the measurement refers
53 gtsam::Key keyB_; // key of robot B to which the measurement refers
54
55 SharedGaussian model_;
56
57 /** concept check by type */
58 GTSAM_CONCEPT_LIE_TYPE(T)
59 GTSAM_CONCEPT_TESTABLE_TYPE(T)
60
61 public:
62
63 // shorthand for a smart pointer to a factor
64 typedef typename std::shared_ptr<TransformBtwRobotsUnaryFactor> shared_ptr;
65
66 /** default constructor - only use for serialization */
67 TransformBtwRobotsUnaryFactor() {}
68
69 /** Constructor */
70 TransformBtwRobotsUnaryFactor(Key key, const VALUE& measured, Key keyA, Key keyB,
71 const gtsam::Values& valA, const gtsam::Values& valB,
72 const SharedGaussian& model) :
73 Base(KeyVector{key}), key_(key), measured_(measured), keyA_(keyA), keyB_(keyB),
74 model_(model){
75
76 setValAValB(valA, valB);
77
78 }
79
80 ~TransformBtwRobotsUnaryFactor() override {}
81
82
83 /** Clone */
84 gtsam::NonlinearFactor::shared_ptr clone() const override { return std::make_shared<This>(*this); }
85
86
87 /** implement functions needed for Testable */
88
89 /** print */
90 void print(const std::string& s, const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {
91 std::cout << s << "TransformBtwRobotsUnaryFactor("
92 << keyFormatter(key_) << ")\n";
93 std::cout << "MR between factor keys: "
94 << keyFormatter(keyA_) << ","
95 << keyFormatter(keyB_) << "\n";
96 measured_.print(" measured: ");
97 model_->print(name: " noise model: ");
98 // Base::print(s, keyFormatter);
99 }
100
101 /** equals */
102 bool equals(const NonlinearFactor& f, double tol=1e-9) const override {
103 const This *t = dynamic_cast<const This*> (&f);
104
105 if(t && Base::equals(f))
106 return key_ == t->key_ && measured_.equals(t->measured_);
107 else
108 return false;
109 }
110
111 /** implement functions needed to derive from Factor */
112
113 /* ************************************************************************* */
114 void setValAValB(const gtsam::Values& valA, const gtsam::Values& valB){
115 if ( (!valA.exists(j: keyA_)) && (!valB.exists(j: keyA_)) && (!valA.exists(j: keyB_)) && (!valB.exists(j: keyB_)) )
116 throw("something is wrong!");
117
118 // TODO: make sure the two keys belong to different robots
119
120 if (valA.exists(j: keyA_)){
121 valA_ = valA;
122 valB_ = valB;
123 }
124 else {
125 valA_ = valB;
126 valB_ = valA;
127 }
128 }
129
130 /* ************************************************************************* */
131 double error(const gtsam::Values& x) const override {
132 return whitenedError(x).squaredNorm();
133 }
134
135 /* ************************************************************************* */
136 /**
137 * Linearize a non-linearFactorN to get a gtsam::GaussianFactor,
138 * \f$ Ax-b \approx h(x+\delta x)-z = h(x) + A \delta x - z \f$
139 * Hence \f$ b = z - h(x) = - \mathtt{error\_vector}(x) \f$
140 */
141 /* This version of linearize recalculates the noise model each time */
142 std::shared_ptr<gtsam::GaussianFactor> linearize(const gtsam::Values& x) const override {
143 // Only linearize if the factor is active
144 if (!this->active(x))
145 return std::shared_ptr<gtsam::JacobianFactor>();
146
147 //std::cout<<"About to linearize"<<std::endl;
148 gtsam::Matrix A1;
149 std::vector<gtsam::Matrix> A(this->size());
150 gtsam::Vector b = -whitenedError(x, A);
151 A1 = A[0];
152
153 return gtsam::GaussianFactor::shared_ptr(
154 new gtsam::JacobianFactor(key_, A1, b, gtsam::noiseModel::Unit::Create(dim: b.size())));
155 }
156
157
158 /* ************************************************************************* */
159 gtsam::Vector whitenedError(const gtsam::Values& x, OptionalMatrixVecType H = nullptr) const {
160
161 T orgA_T_currA = valA_.at<T>(keyA_);
162 T orgB_T_currB = valB_.at<T>(keyB_);
163 T orgA_T_orgB = x.at<T>(key_);
164
165 T currA_T_currB_pred;
166 if (H) {
167 Matrix H_compose, H_between1;
168 T orgA_T_currB = orgA_T_orgB.compose(orgB_T_currB, H_compose, {});
169 currA_T_currB_pred = orgA_T_currA.between(orgA_T_currB, {}, H_between1);
170 (*H)[0] = H_compose * H_between1;
171 }
172 else {
173 T orgA_T_currB = orgA_T_orgB.compose(orgB_T_currB);
174 currA_T_currB_pred = orgA_T_currA.between(orgA_T_currB);
175 }
176
177 const T& currA_T_currB_msr = measured_;
178 Vector error = currA_T_currB_msr.localCoordinates(currA_T_currB_pred);
179
180 if (H)
181 model_->WhitenSystem(A&: *H, b&: error);
182 else
183 model_->whitenInPlace(v&: error);
184
185 return error;
186 }
187
188 /* ************************************************************************* */
189 /** A function overload to accept a vector<matrix> instead of a pointer to
190 * the said type.
191 */
192 gtsam::Vector whitenedError(const gtsam::Values& x, std::vector<Matrix>& H) const {
193 return whitenedError(x, &H);
194 }
195
196
197 /* ************************************************************************* */
198 gtsam::Vector unwhitenedError(const gtsam::Values& x) const {
199
200 T orgA_T_currA = valA_.at<T>(keyA_);
201 T orgB_T_currB = valB_.at<T>(keyB_);
202 T orgA_T_orgB = x.at<T>(key_);
203
204 T orgA_T_currB = orgA_T_orgB.compose(orgB_T_currB);
205 T currA_T_currB_pred = orgA_T_currA.between(orgA_T_currB);
206
207 T currA_T_currB_msr = measured_;
208 return currA_T_currB_msr.localCoordinates(currA_T_currB_pred);
209 }
210
211 /* ************************************************************************* */
212
213 size_t dim() const override {
214 return model_->R().rows() + model_->R().cols();
215 }
216
217 private:
218
219#if GTSAM_ENABLE_BOOST_SERIALIZATION
220 /** Serialization function */
221 friend class boost::serialization::access;
222 template<class ARCHIVE>
223 void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
224 ar & boost::serialization::make_nvp("NonlinearFactor",
225 boost::serialization::base_object<Base>(*this));
226 //ar & BOOST_SERIALIZATION_NVP(measured_);
227 }
228#endif
229 }; // \class TransformBtwRobotsUnaryFactor
230
231 /// traits
232 template<class VALUE>
233 struct traits<TransformBtwRobotsUnaryFactor<VALUE> > :
234 public Testable<TransformBtwRobotsUnaryFactor<VALUE> > {
235 };
236
237} /// namespace gtsam
238