gtsam 4.1.1
gtsam
TransformBtwRobotsUnaryFactor.h
Go to the documentation of this file.
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
17#pragma once
18
22#include <gtsam/base/Testable.h>
23#include <gtsam/base/Lie.h>
24
25#include <ostream>
26
27namespace gtsam {
28
34 template<class VALUE>
35 class TransformBtwRobotsUnaryFactor: public NonlinearFactor { // TODO why not NoiseModelFactor1 ?
36
37 public:
38
39 typedef VALUE T;
40
41 private:
42
45
46 gtsam::Key key_;
47
48 VALUE measured_;
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
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 boost::shared_ptr<TransformBtwRobotsUnaryFactor> shared_ptr;
65
68
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(cref_list_of<1>(key)), key_(key), measured_(measured), keyA_(keyA), keyB_(keyB),
74 model_(model){
75
76 setValAValB(valA, valB);
77
78 }
79
81
82
84 gtsam::NonlinearFactor::shared_ptr clone() const override { return boost::make_shared<This>(*this); }
85
86
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(" noise model: ");
98 // Base::print(s, keyFormatter);
99 }
100
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
113 /* ************************************************************************* */
114 void setValAValB(const gtsam::Values& valA, const gtsam::Values& valB){
115 if ( (!valA.exists(keyA_)) && (!valB.exists(keyA_)) && (!valA.exists(keyB_)) && (!valB.exists(keyB_)) )
116 throw("something is wrong!");
117
118 // TODO: make sure the two keys belong to different robots
119
120 if (valA.exists(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 /* ************************************************************************* */
141 /* This version of linearize recalculates the noise model each time */
142 boost::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 boost::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
154 new gtsam::JacobianFactor(key_, A1, b, gtsam::noiseModel::Unit::Create(b.size())));
155 }
156
157
158 /* ************************************************************************* */
159 gtsam::Vector whitenedError(const gtsam::Values& x,
160 boost::optional<std::vector<gtsam::Matrix>&> H = boost::none) const {
161
162 T orgA_T_currA = valA_.at<T>(keyA_);
163 T orgB_T_currB = valB_.at<T>(keyB_);
164 T orgA_T_orgB = x.at<T>(key_);
165
166 T currA_T_currB_pred;
167 if (H) {
168 Matrix H_compose, H_between1;
169 T orgA_T_currB = orgA_T_orgB.compose(orgB_T_currB, H_compose, boost::none);
170 currA_T_currB_pred = orgA_T_currA.between(orgA_T_currB, boost::none, H_between1);
171 (*H)[0] = H_compose * H_between1;
172 }
173 else {
174 T orgA_T_currB = orgA_T_orgB.compose(orgB_T_currB);
175 currA_T_currB_pred = orgA_T_currA.between(orgA_T_currB);
176 }
177
178 const T& currA_T_currB_msr = measured_;
179 Vector error = currA_T_currB_msr.localCoordinates(currA_T_currB_pred);
180
181 if (H)
182 model_->WhitenSystem(*H, error);
183 else
184 model_->whitenInPlace(error);
185
186 return error;
187 }
188
189
190 /* ************************************************************************* */
191 gtsam::Vector unwhitenedError(const gtsam::Values& x) const {
192
193 T orgA_T_currA = valA_.at<T>(keyA_);
194 T orgB_T_currB = valB_.at<T>(keyB_);
195 T orgA_T_orgB = x.at<T>(key_);
196
197 T orgA_T_currB = orgA_T_orgB.compose(orgB_T_currB);
198 T currA_T_currB_pred = orgA_T_currA.between(orgA_T_currB);
199
200 T currA_T_currB_msr = measured_;
201 return currA_T_currB_msr.localCoordinates(currA_T_currB_pred);
202 }
203
204 /* ************************************************************************* */
205
206 size_t dim() const override {
207 return model_->R().rows() + model_->R().cols();
208 }
209
210 private:
211
214 template<class ARCHIVE>
215 void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
216 ar & boost::serialization::make_nvp("NonlinearFactor",
217 boost::serialization::base_object<Base>(*this));
218 //ar & BOOST_SERIALIZATION_NVP(measured_);
219 }
220 }; // \class TransformBtwRobotsUnaryFactor
221
223 template<class VALUE>
225 public Testable<TransformBtwRobotsUnaryFactor<VALUE> > {
226 };
227
228}
Concept check for values that can be used in unit tests.
Base class and basic functions for Lie types.
A factor with a quadratic error function - a Gaussian.
Non-linear factor base classes.
Global functions in a separate testing namespace.
Definition: chartTesting.h:28
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:69
std::function< std::string(Key)> KeyFormatter
Typedef for a function to format a key, i.e. to convert it to a string.
Definition: Key.h:35
A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
Definition: concepts.h:30
A helper that implements the traits interface for GTSAM types.
Definition: Testable.h:151
This is the base class for all factor types.
Definition: Factor.h:56
bool equals(const This &other, double tol=1e-9) const
check equality
Definition: Factor.cpp:42
size_t size() const
Definition: Factor.h:136
boost::shared_ptr< This > shared_ptr
shared_ptr to this class
Definition: GaussianFactor.h:42
A Gaussian factor in the squared-error form.
Definition: JacobianFactor.h:91
static shared_ptr Create(size_t dim)
Create a unit covariance noise model.
Definition: NoiseModel.h:611
Nonlinear factor base class.
Definition: NonlinearFactor.h:43
virtual bool equals(const NonlinearFactor &f, double tol=1e-9) const
Check if two factors are equal.
Definition: NonlinearFactor.cpp:36
virtual bool active(const Values &) const
Checks whether a factor should be used based on a set of values.
Definition: NonlinearFactor.h:106
A non-templated config holding any types of Manifold-group elements.
Definition: Values.h:63
const ValueType at(Key j) const
Retrieve a variable by key j.
Definition: Values-inl.h:346
bool exists(Key j) const
Check if a value exists with key j.
Definition: Values.cpp:96
Definition: TransformBtwRobotsUnaryFactor.h:35
void setValAValB(const gtsam::Values &valA, const gtsam::Values &valB)
implement functions needed to derive from Factor
Definition: TransformBtwRobotsUnaryFactor.h:114
bool equals(const NonlinearFactor &f, double tol=1e-9) const override
equals
Definition: TransformBtwRobotsUnaryFactor.h:102
void print(const std::string &s, const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
implement functions needed for Testable
Definition: TransformBtwRobotsUnaryFactor.h:90
size_t dim() const override
get the dimension of the factor (number of rows on linearization)
Definition: TransformBtwRobotsUnaryFactor.h:206
double error(const gtsam::Values &x) const override
Calculate the error of the factor This is typically equal to log-likelihood, e.g.
Definition: TransformBtwRobotsUnaryFactor.h:131
TransformBtwRobotsUnaryFactor(Key key, const VALUE &measured, Key keyA, Key keyB, const gtsam::Values &valA, const gtsam::Values &valB, const SharedGaussian &model)
Constructor.
Definition: TransformBtwRobotsUnaryFactor.h:70
boost::shared_ptr< gtsam::GaussianFactor > linearize(const gtsam::Values &x) const override
Linearize a non-linearFactorN to get a gtsam::GaussianFactor, Hence .
Definition: TransformBtwRobotsUnaryFactor.h:142
friend class boost::serialization::access
Serialization function.
Definition: TransformBtwRobotsUnaryFactor.h:213
boost::shared_ptr< TransformBtwRobotsUnaryFactor > shared_ptr
concept check by type
Definition: TransformBtwRobotsUnaryFactor.h:64
TransformBtwRobotsUnaryFactor()
default constructor - only use for serialization
Definition: TransformBtwRobotsUnaryFactor.h:67
gtsam::NonlinearFactor::shared_ptr clone() const override
Clone.
Definition: TransformBtwRobotsUnaryFactor.h:84