gtsam 4.1.1
gtsam
TransformBtwRobotsUnaryFactorEM.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
24#include <gtsam/base/Testable.h>
25#include <gtsam/base/Lie.h>
26
27#include <ostream>
28
29namespace gtsam {
30
36 template<class VALUE>
38
39 public:
40
41 typedef VALUE T;
42
43 private:
44
46 typedef NonlinearFactor Base;
47
48 Key key_;
49
50 VALUE measured_;
52 Values valA_; // given values for robot A map\trajectory
53 Values valB_; // given values for robot B map\trajectory
54 Key keyA_; // key of robot A to which the measurement refers
55 Key keyB_; // key of robot B to which the measurement refers
56
57 // TODO: create function to update valA_ and valB_
58
59 SharedGaussian model_inlier_;
60 SharedGaussian model_outlier_;
61
62 double prior_inlier_;
63 double prior_outlier_;
64
65 bool flag_bump_up_near_zero_probs_;
66 mutable bool start_with_M_step_;
67
69 GTSAM_CONCEPT_LIE_TYPE(T)
70 GTSAM_CONCEPT_TESTABLE_TYPE(T)
71
72 public:
73
74 // shorthand for a smart pointer to a factor
75 typedef typename boost::shared_ptr<TransformBtwRobotsUnaryFactorEM> shared_ptr;
76
79
81 TransformBtwRobotsUnaryFactorEM(Key key, const VALUE& measured, Key keyA, Key keyB,
82 const Values& valA, const Values& valB,
83 const SharedGaussian& model_inlier, const SharedGaussian& model_outlier,
84 const double prior_inlier, const double prior_outlier,
85 const bool flag_bump_up_near_zero_probs = false,
86 const bool start_with_M_step = false) :
87 Base(cref_list_of<1>(key)), key_(key), measured_(measured), keyA_(keyA), keyB_(keyB),
88 model_inlier_(model_inlier), model_outlier_(model_outlier),
89 prior_inlier_(prior_inlier), prior_outlier_(prior_outlier), flag_bump_up_near_zero_probs_(flag_bump_up_near_zero_probs),
90 start_with_M_step_(false){
91
92 setValAValB(valA, valB);
93
94 }
95
97
98
100 NonlinearFactor::shared_ptr clone() const override { return boost::make_shared<This>(*this); }
101
102
106 void print(const std::string& s, const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {
107 std::cout << s << "TransformBtwRobotsUnaryFactorEM("
108 << keyFormatter(key_) << ")\n";
109 std::cout << "MR between factor keys: "
110 << keyFormatter(keyA_) << ","
111 << keyFormatter(keyB_) << "\n";
112 measured_.print(" measured: ");
113 model_inlier_->print(" noise model inlier: ");
114 model_outlier_->print(" noise model outlier: ");
115 std::cout << "(prior_inlier, prior_outlier_) = ("
116 << prior_inlier_ << ","
117 << prior_outlier_ << ")\n";
118 // Base::print(s, keyFormatter);
119 }
120
122 bool equals(const NonlinearFactor& f, double tol=1e-9) const override {
123 const This *t = dynamic_cast<const This*> (&f);
124
125 if(t && Base::equals(f))
126 return key_ == t->key_ && measured_.equals(t->measured_) &&
127 // model_inlier_->equals(t->model_inlier_ ) && // TODO: fix here
128 // model_outlier_->equals(t->model_outlier_ ) &&
129 prior_outlier_ == t->prior_outlier_ && prior_inlier_ == t->prior_inlier_;
130 else
131 return false;
132 }
133
136 /* ************************************************************************* */
137 void setValAValB(const Values& valA, const Values& valB){
138 if ( (!valA.exists(keyA_)) && (!valB.exists(keyA_)) && (!valA.exists(keyB_)) && (!valB.exists(keyB_)) )
139 throw("something is wrong!");
140
141 // TODO: make sure the two keys belong to different robots
142
143 if (valA.exists(keyA_)){
144 valA_ = valA;
145 valB_ = valB;
146 }
147 else {
148 valA_ = valB;
149 valB_ = valA;
150 }
151 }
152
153 /* ************************************************************************* */
154 double error(const Values& x) const override {
155 return whitenedError(x).squaredNorm();
156 }
157
158 /* ************************************************************************* */
164 /* This version of linearize recalculates the noise model each time */
165 boost::shared_ptr<GaussianFactor> linearize(const Values& x) const override {
166 // Only linearize if the factor is active
167 if (!this->active(x))
168 return boost::shared_ptr<JacobianFactor>();
169
170 //std::cout<<"About to linearize"<<std::endl;
171 Matrix A1;
172 std::vector<Matrix> A(this->size());
173 Vector b = -whitenedError(x, A);
174 A1 = A[0];
175
177 new JacobianFactor(key_, A1, b, noiseModel::Unit::Create(b.size())));
178 }
179
180
181 /* ************************************************************************* */
182 Vector whitenedError(const Values& x,
183 boost::optional<std::vector<Matrix>&> H = boost::none) const {
184
185 bool debug = true;
186
187 Matrix H_compose, H_between1, H_dummy;
188
189 T orgA_T_currA = valA_.at<T>(keyA_);
190 T orgB_T_currB = valB_.at<T>(keyB_);
191
192 T orgA_T_orgB = x.at<T>(key_);
193
194 T orgA_T_currB = orgA_T_orgB.compose(orgB_T_currB, H_compose, H_dummy);
195
196 T currA_T_currB_pred = orgA_T_currA.between(orgA_T_currB, H_dummy, H_between1);
197
198 T currA_T_currB_msr = measured_;
199
200 Vector err = currA_T_currB_msr.localCoordinates(currA_T_currB_pred);
201
202 // Calculate indicator probabilities (inlier and outlier)
203 Vector p_inlier_outlier = calcIndicatorProb(x, err);
204 double p_inlier = p_inlier_outlier[0];
205 double p_outlier = p_inlier_outlier[1];
206
207 if (start_with_M_step_){
208 start_with_M_step_ = false;
209
210 p_inlier = 0.5;
211 p_outlier = 0.5;
212 }
213
214 Vector err_wh_inlier = model_inlier_->whiten(err);
215 Vector err_wh_outlier = model_outlier_->whiten(err);
216
217 Matrix invCov_inlier = model_inlier_->R().transpose() * model_inlier_->R();
218 Matrix invCov_outlier = model_outlier_->R().transpose() * model_outlier_->R();
219
220 Vector err_wh_eq;
221 err_wh_eq.resize(err_wh_inlier.rows()*2);
222 err_wh_eq << sqrt(p_inlier) * err_wh_inlier.array() , sqrt(p_outlier) * err_wh_outlier.array();
223
224 Matrix H_unwh = H_compose * H_between1;
225
226 if (H){
227
228 Matrix H_inlier = sqrt(p_inlier)*model_inlier_->Whiten(H_unwh);
229 Matrix H_outlier = sqrt(p_outlier)*model_outlier_->Whiten(H_unwh);
230 Matrix H_aug = stack(2, &H_inlier, &H_outlier);
231
232 (*H)[0].resize(H_aug.rows(),H_aug.cols());
233 (*H)[0] = H_aug;
234 }
235
236 if (debug){
237 // std::cout<<"H_compose - rows, cols, : "<<H_compose.rows()<<", "<< H_compose.cols()<<std::endl;
238 // std::cout<<"H_between1 - rows, cols, : "<<H_between1.rows()<<", "<< H_between1.cols()<<std::endl;
239 // std::cout<<"H_unwh - rows, cols, : "<<H_unwh.rows()<<", "<< H_unwh.cols()<<std::endl;
240 // std::cout<<"H_unwh: "<<std:endl<<H_unwh[0]
241
242 }
243
244
245 return err_wh_eq;
246 }
247
248 /* ************************************************************************* */
249 Vector calcIndicatorProb(const Values& x) const {
250
251 Vector err = unwhitenedError(x);
252
253 return this->calcIndicatorProb(x, err);
254 }
255
256 /* ************************************************************************* */
257 Vector calcIndicatorProb(const Values& x, const Vector& err) const {
258
259 // Calculate indicator probabilities (inlier and outlier)
260 Vector err_wh_inlier = model_inlier_->whiten(err);
261 Vector err_wh_outlier = model_outlier_->whiten(err);
262
263 Matrix invCov_inlier = model_inlier_->R().transpose() * model_inlier_->R();
264 Matrix invCov_outlier = model_outlier_->R().transpose() * model_outlier_->R();
265
266 double p_inlier = prior_inlier_ * sqrt(invCov_inlier.norm()) * exp( -0.5 * err_wh_inlier.dot(err_wh_inlier) );
267 double p_outlier = prior_outlier_ * sqrt(invCov_outlier.norm()) * exp( -0.5 * err_wh_outlier.dot(err_wh_outlier) );
268
269 double sumP = p_inlier + p_outlier;
270 p_inlier /= sumP;
271 p_outlier /= sumP;
272
273 if (flag_bump_up_near_zero_probs_){
274 // Bump up near-zero probabilities (as in linerFlow.h)
275 double minP = 0.05; // == 0.1 / 2 indicator variables
276 if (p_inlier < minP || p_outlier < minP){
277 if (p_inlier < minP)
278 p_inlier = minP;
279 if (p_outlier < minP)
280 p_outlier = minP;
281 sumP = p_inlier + p_outlier;
282 p_inlier /= sumP;
283 p_outlier /= sumP;
284 }
285 }
286
287 return (Vector(2) << p_inlier, p_outlier).finished();
288 }
289
290 /* ************************************************************************* */
291 Vector unwhitenedError(const Values& x) const {
292
293 T orgA_T_currA = valA_.at<T>(keyA_);
294 T orgB_T_currB = valB_.at<T>(keyB_);
295
296 T orgA_T_orgB = x.at<T>(key_);
297
298 T orgA_T_currB = orgA_T_orgB.compose(orgB_T_currB);
299
300 T currA_T_currB_pred = orgA_T_currA.between(orgA_T_currB);
301
302 T currA_T_currB_msr = measured_;
303
304 return currA_T_currB_msr.localCoordinates(currA_T_currB_pred);
305 }
306
307 /* ************************************************************************* */
308 SharedGaussian get_model_inlier() const {
309 return model_inlier_;
310 }
311
312 /* ************************************************************************* */
313 SharedGaussian get_model_outlier() const {
314 return model_outlier_;
315 }
316
317 /* ************************************************************************* */
318 Matrix get_model_inlier_cov() const {
319 return (model_inlier_->R().transpose()*model_inlier_->R()).inverse();
320 }
321
322 /* ************************************************************************* */
323 Matrix get_model_outlier_cov() const {
324 return (model_outlier_->R().transpose()*model_outlier_->R()).inverse();
325 }
326
327 /* ************************************************************************* */
328 void updateNoiseModels(const Values& values, const Marginals& marginals) {
329 /* given marginals version, don't need to marginal multiple times if update a lot */
330
331 KeyVector Keys;
332 Keys.push_back(keyA_);
333 Keys.push_back(keyB_);
334 JointMarginal joint_marginal12 = marginals.jointMarginalCovariance(Keys);
335 Matrix cov1 = joint_marginal12(keyA_, keyA_);
336 Matrix cov2 = joint_marginal12(keyB_, keyB_);
337 Matrix cov12 = joint_marginal12(keyA_, keyB_);
338
339 updateNoiseModels_givenCovs(values, cov1, cov2, cov12);
340 }
341
342 /* ************************************************************************* */
343 void updateNoiseModels(const Values& values, const NonlinearFactorGraph& graph){
344 /* Update model_inlier_ and model_outlier_ to account for uncertainty in robot trajectories
345 * (note these are given in the E step, where indicator probabilities are calculated).
346 *
347 * Principle: R += [H1 H2] * joint_cov12 * [H1 H2]', where H1, H2 are Jacobians of the
348 * unwhitened error w.r.t. states, and R is the measurement covariance (inlier or outlier modes).
349 *
350 * TODO: improve efficiency (info form)
351 */
352
353 // get joint covariance of the involved states
354
355 Marginals marginals(graph, values, Marginals::QR);
356
357 this->updateNoiseModels(values, marginals);
358 }
359
360 /* ************************************************************************* */
361 void updateNoiseModels_givenCovs(const Values& values, const Matrix& cov1, const Matrix& cov2, const Matrix& cov12){
362 /* Update model_inlier_ and model_outlier_ to account for uncertainty in robot trajectories
363 * (note these are given in the E step, where indicator probabilities are calculated).
364 *
365 * Principle: R += [H1 H2] * joint_cov12 * [H1 H2]', where H1, H2 are Jacobians of the
366 * unwhitened error w.r.t. states, and R is the measurement covariance (inlier or outlier modes).
367 *
368 * TODO: improve efficiency (info form)
369 */
370
371 const T& p1 = values.at<T>(keyA_);
372 const T& p2 = values.at<T>(keyB_);
373
374 Matrix H1, H2;
375 p1.between(p2, H1, H2); // h(x)
376
377 Matrix H;
378 H.resize(H1.rows(), H1.rows()+H2.rows());
379 H << H1, H2; // H = [H1 H2]
380
381 Matrix joint_cov;
382 joint_cov.resize(cov1.rows()+cov2.rows(), cov1.cols()+cov2.cols());
383 joint_cov << cov1, cov12,
384 cov12.transpose(), cov2;
385
386 Matrix cov_state = H*joint_cov*H.transpose();
387
388 // model_inlier_->print("before:");
389
390 // update inlier and outlier noise models
391 Matrix covRinlier = (model_inlier_->R().transpose()*model_inlier_->R()).inverse();
392 model_inlier_ = noiseModel::Gaussian::Covariance(covRinlier + cov_state);
393
394 Matrix covRoutlier = (model_outlier_->R().transpose()*model_outlier_->R()).inverse();
395 model_outlier_ = noiseModel::Gaussian::Covariance(covRoutlier + cov_state);
396
397 // model_inlier_->print("after:");
398 // std::cout<<"covRinlier + cov_state: "<<covRinlier + cov_state<<std::endl;
399 }
400
401
402 /* ************************************************************************* */
403
404 size_t dim() const override {
405 return model_inlier_->R().rows() + model_inlier_->R().cols();
406 }
407
408 private:
409
412 template<class ARCHIVE>
413 void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
414 ar & boost::serialization::make_nvp("NonlinearFactor",
415 boost::serialization::base_object<Base>(*this));
416 //ar & BOOST_SERIALIZATION_NVP(measured_);
417 }
418 }; // \class TransformBtwRobotsUnaryFactorEM
419
421 template<class VALUE>
423 public Testable<TransformBtwRobotsUnaryFactorEM<VALUE> > {
424 };
425
426}
Concept check for values that can be used in unit tests.
Base class and basic functions for Lie types.
T inverse(const T &t)
unary functions
Definition: lieProxies.h:43
A factor with a quadratic error function - a Gaussian.
A class for computing marginals in a NonlinearFactorGraph.
Factor Graph consisting of non-linear factors.
Non-linear factor base classes.
Global functions in a separate testing namespace.
Definition: chartTesting.h:28
FastVector< Key > KeyVector
Define collection type once and for all - also used in wrappers.
Definition: Key.h:86
Matrix stack(size_t nrMatrices,...)
create a matrix by stacking other matrices Given a set of matrices: A1, A2, A3...
Definition: Matrix.cpp:396
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 Covariance(const Matrix &covariance, bool smart=true)
A Gaussian noise model created by specifying a covariance matrix.
Definition: NoiseModel.cpp:116
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: TransformBtwRobotsUnaryFactorEM.h:37
double error(const Values &x) const override
Calculate the error of the factor This is typically equal to log-likelihood, e.g.
Definition: TransformBtwRobotsUnaryFactorEM.h:154
size_t dim() const override
get the dimension of the factor (number of rows on linearization)
Definition: TransformBtwRobotsUnaryFactorEM.h:404
void print(const std::string &s, const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
implement functions needed for Testable
Definition: TransformBtwRobotsUnaryFactorEM.h:106
TransformBtwRobotsUnaryFactorEM()
default constructor - only use for serialization
Definition: TransformBtwRobotsUnaryFactorEM.h:78
void setValAValB(const Values &valA, const Values &valB)
implement functions needed to derive from Factor
Definition: TransformBtwRobotsUnaryFactorEM.h:137
bool equals(const NonlinearFactor &f, double tol=1e-9) const override
equals
Definition: TransformBtwRobotsUnaryFactorEM.h:122
boost::shared_ptr< TransformBtwRobotsUnaryFactorEM > shared_ptr
concept check by type
Definition: TransformBtwRobotsUnaryFactorEM.h:75
boost::shared_ptr< GaussianFactor > linearize(const Values &x) const override
Linearize a non-linearFactorN to get a GaussianFactor, Hence .
Definition: TransformBtwRobotsUnaryFactorEM.h:165
NonlinearFactor::shared_ptr clone() const override
Clone.
Definition: TransformBtwRobotsUnaryFactorEM.h:100
friend class boost::serialization::access
Serialization function.
Definition: TransformBtwRobotsUnaryFactorEM.h:411
TransformBtwRobotsUnaryFactorEM(Key key, const VALUE &measured, Key keyA, Key keyB, const Values &valA, const Values &valB, const SharedGaussian &model_inlier, const SharedGaussian &model_outlier, const double prior_inlier, const double prior_outlier, const bool flag_bump_up_near_zero_probs=false, const bool start_with_M_step=false)
Constructor.
Definition: TransformBtwRobotsUnaryFactorEM.h:81