gtsam  4.1.0
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 
29 namespace 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 }
gtsam::TransformBtwRobotsUnaryFactorEM::TransformBtwRobotsUnaryFactorEM
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
gtsam::GaussianFactor::shared_ptr
boost::shared_ptr< This > shared_ptr
shared_ptr to this class
Definition: GaussianFactor.h:42
Testable.h
Concept check for values that can be used in unit tests.
gtsam::noiseModel::Unit::Create
static shared_ptr Create(size_t dim)
Create a unit covariance noise model.
Definition: NoiseModel.h:608
gtsam::TransformBtwRobotsUnaryFactorEM::setValAValB
void setValAValB(const Values &valA, const Values &valB)
implement functions needed to derive from Factor
Definition: TransformBtwRobotsUnaryFactorEM.h:137
gtsam::TransformBtwRobotsUnaryFactorEM::print
void print(const std::string &s, const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
implement functions needed for Testable
Definition: TransformBtwRobotsUnaryFactorEM.h:106
gtsam::traits
A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
Definition: concepts.h:30
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:61
BetweenFactor.h
gtsam::TransformBtwRobotsUnaryFactorEM::access
friend class boost::serialization::access
Serialization function.
Definition: TransformBtwRobotsUnaryFactorEM.h:411
gtsam::TransformBtwRobotsUnaryFactorEM::equals
bool equals(const NonlinearFactor &f, double tol=1e-9) const override
equals
Definition: TransformBtwRobotsUnaryFactorEM.h:122
NonlinearFactorGraph.h
Factor Graph Constsiting of non-linear factors.
gtsam
Global functions in a separate testing namespace.
Definition: chartTesting.h:28
gtsam::TransformBtwRobotsUnaryFactorEM
Definition: TransformBtwRobotsUnaryFactorEM.h:37
gtsam::Testable
A helper that implements the traits interface for GTSAM types.
Definition: Testable.h:150
gtsam::NonlinearFactor
Nonlinear factor base class.
Definition: NonlinearFactor.h:43
gtsam::TransformBtwRobotsUnaryFactorEM::error
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
NonlinearFactor.h
Non-linear factor base classes.
gtsam::noiseModel::Gaussian::Covariance
static shared_ptr Covariance(const Matrix &covariance, bool smart=true)
A Gaussian noise model created by specifying a covariance matrix.
Definition: NoiseModel.cpp:116
gtsam::JacobianFactor
A Gaussian factor in the squared-error form.
Definition: JacobianFactor.h:91
gtsam::NonlinearFactor::active
virtual bool active(const Values &) const
Checks whether a factor should be used based on a set of values.
Definition: NonlinearFactor.h:106
gtsam::Factor::size
size_t size() const
Definition: Factor.h:129
Marginals.h
A class for computing marginals in a NonlinearFactorGraph.
gtsam::Values::at
ValueType at(Key j) const
Retrieve a variable by key j.
Definition: Values-inl.h:342
gtsam::TransformBtwRobotsUnaryFactorEM::clone
NonlinearFactor::shared_ptr clone() const override
Clone.
Definition: TransformBtwRobotsUnaryFactorEM.h:100
gtsam::KeyFormatter
boost::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
gtsam::Factor
This is the base class for all factor types.
Definition: Factor.h:55
gtsam::KeyVector
FastVector< Key > KeyVector
Define collection type once and for all - also used in wrappers.
Definition: Key.h:86
gtsam::TransformBtwRobotsUnaryFactorEM::dim
size_t dim() const override
get the dimension of the factor (number of rows on linearization)
Definition: TransformBtwRobotsUnaryFactorEM.h:404
GaussianFactor.h
A factor with a quadratic error function - a Gaussian.
gtsam::TransformBtwRobotsUnaryFactorEM::linearize
boost::shared_ptr< GaussianFactor > linearize(const Values &x) const override
Linearize a non-linearFactorN to get a GaussianFactor, Hence .
Definition: TransformBtwRobotsUnaryFactorEM.h:165
gtsam::Values::exists
bool exists(Key j) const
Check if a value exists with key j.
Definition: Values.cpp:104
gtsam::TransformBtwRobotsUnaryFactorEM::shared_ptr
boost::shared_ptr< TransformBtwRobotsUnaryFactorEM > shared_ptr
concept check by type
Definition: TransformBtwRobotsUnaryFactorEM.h:75
gtsam::testing::inverse
T inverse(const T &t)
unary functions
Definition: lieProxies.h:43
gtsam::Values
A non-templated config holding any types of Manifold-group elements.
Definition: Values.h:71
gtsam::Factor::equals
bool equals(const This &other, double tol=1e-9) const
check equality
Definition: Factor.cpp:42
gtsam::TransformBtwRobotsUnaryFactorEM::TransformBtwRobotsUnaryFactorEM
TransformBtwRobotsUnaryFactorEM()
default constructor - only use for serialization
Definition: TransformBtwRobotsUnaryFactorEM.h:78
Lie.h
Base class and basic functions for Lie types.
gtsam::stack
Matrix stack(size_t nrMatrices,...)
create a matrix by stacking other matrices Given a set of matrices: A1, A2, A3...
Definition: Matrix.cpp:396
gtsam::NonlinearFactor::equals
virtual bool equals(const NonlinearFactor &f, double tol=1e-9) const
Check if two factors are equal.
Definition: NonlinearFactor.cpp:36