gtsam  4.1.0
gtsam
BetweenFactorEM.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 
16 #pragma once
17 
18 #include <ostream>
19 
20 #include <gtsam/base/Testable.h>
21 #include <gtsam/base/Lie.h>
25 
26 namespace gtsam {
27 
33 template<class VALUE>
35 
36 public:
37 
38  typedef VALUE T;
39 
40 private:
41 
43  typedef NonlinearFactor Base;
44 
45  Key key1_;
46  Key key2_;
47 
48  VALUE measured_;
50  SharedGaussian model_inlier_;
51  SharedGaussian model_outlier_;
52 
53  double prior_inlier_;
54  double prior_outlier_;
55 
56  bool flag_bump_up_near_zero_probs_;
57 
59  GTSAM_CONCEPT_LIE_TYPE(T)GTSAM_CONCEPT_TESTABLE_TYPE(T)
60 
61 public:
62 
63  // shorthand for a smart pointer to a factor
64  typedef typename boost::shared_ptr<BetweenFactorEM> shared_ptr;
65 
67  BetweenFactorEM() {
68  }
69 
71  BetweenFactorEM(Key key1, Key key2, const VALUE& measured,
72  const SharedGaussian& model_inlier, const SharedGaussian& model_outlier,
73  const double prior_inlier, const double prior_outlier,
74  const bool flag_bump_up_near_zero_probs = false) :
75  Base(cref_list_of<2>(key1)(key2)), key1_(key1), key2_(key2), measured_(
76  measured), model_inlier_(model_inlier), model_outlier_(model_outlier), prior_inlier_(
77  prior_inlier), prior_outlier_(prior_outlier), flag_bump_up_near_zero_probs_(
78  flag_bump_up_near_zero_probs) {
79  }
80 
81  virtual ~BetweenFactorEM() {
82  }
83 
87  void print(const std::string& s, const KeyFormatter& keyFormatter =
88  DefaultKeyFormatter) const override {
89  std::cout << s << "BetweenFactorEM(" << keyFormatter(key1_) << ","
90  << keyFormatter(key2_) << ")\n";
91  measured_.print(" measured: ");
92  model_inlier_->print(" noise model inlier: ");
93  model_outlier_->print(" noise model outlier: ");
94  std::cout << "(prior_inlier, prior_outlier_) = (" << prior_inlier_ << ","
95  << prior_outlier_ << ")\n";
96  // Base::print(s, keyFormatter);
97  }
98 
100  bool equals(const NonlinearFactor& f, double tol = 1e-9) const override {
101  const This *t = dynamic_cast<const This*>(&f);
102 
103  if (t && Base::equals(f))
104  return key1_ == t->key1_ && key2_ == t->key2_
105  &&
106  // model_inlier_->equals(t->model_inlier_ ) && // TODO: fix here
107  // model_outlier_->equals(t->model_outlier_ ) &&
108  prior_outlier_ == t->prior_outlier_
109  && prior_inlier_ == t->prior_inlier_ && measured_.equals(t->measured_);
110  else
111  return false;
112  }
113 
116  /* ************************************************************************* */
117  double error(const Values &x) const override {
118  return whitenedError(x).squaredNorm();
119  }
120 
121  /* ************************************************************************* */
127  /* This version of linearize recalculates the noise model each time */
128  boost::shared_ptr<GaussianFactor> linearize(const Values &x) const override {
129  // Only linearize if the factor is active
130  if (!this->active(x))
131  return boost::shared_ptr<JacobianFactor>();
132 
133  //std::cout<<"About to linearize"<<std::endl;
134  Matrix A1, A2;
135  std::vector<Matrix> A(this->size());
136  Vector b = -whitenedError(x, A);
137  A1 = A[0];
138  A2 = A[1];
139 
141  new JacobianFactor(key1_, A1, key2_, A2, b,
142  noiseModel::Unit::Create(b.size())));
143  }
144 
145  /* ************************************************************************* */
146  Vector whitenedError(const Values& x,
147  boost::optional<std::vector<Matrix>&> H = boost::none) const {
148 
149  bool debug = true;
150 
151  const T& p1 = x.at<T>(key1_);
152  const T& p2 = x.at<T>(key2_);
153 
154  Matrix H1, H2;
155 
156  T hx = p1.between(p2, H1, H2); // h(x)
157  // manifold equivalent of h(x)-z -> log(z,h(x))
158 
159  Vector err = measured_.localCoordinates(hx);
160 
161  // Calculate indicator probabilities (inlier and outlier)
162  Vector p_inlier_outlier = calcIndicatorProb(x);
163  double p_inlier = p_inlier_outlier[0];
164  double p_outlier = p_inlier_outlier[1];
165 
166  Vector err_wh_inlier = model_inlier_->whiten(err);
167  Vector err_wh_outlier = model_outlier_->whiten(err);
168 
169  Matrix invCov_inlier = model_inlier_->R().transpose() * model_inlier_->R();
170  Matrix invCov_outlier = model_outlier_->R().transpose()
171  * model_outlier_->R();
172 
173  Vector err_wh_eq;
174  err_wh_eq.resize(err_wh_inlier.rows() * 2);
175  err_wh_eq << sqrt(p_inlier) * err_wh_inlier.array(), sqrt(p_outlier)
176  * err_wh_outlier.array();
177 
178  if (H) {
179  // stack Jacobians for the two indicators for each of the key
180 
181  Matrix H1_inlier = sqrt(p_inlier) * model_inlier_->Whiten(H1);
182  Matrix H1_outlier = sqrt(p_outlier) * model_outlier_->Whiten(H1);
183  Matrix H1_aug = stack(2, &H1_inlier, &H1_outlier);
184 
185  Matrix H2_inlier = sqrt(p_inlier) * model_inlier_->Whiten(H2);
186  Matrix H2_outlier = sqrt(p_outlier) * model_outlier_->Whiten(H2);
187  Matrix H2_aug = stack(2, &H2_inlier, &H2_outlier);
188 
189  (*H)[0].resize(H1_aug.rows(), H1_aug.cols());
190  (*H)[1].resize(H2_aug.rows(), H2_aug.cols());
191 
192  (*H)[0] = H1_aug;
193  (*H)[1] = H2_aug;
194  }
195 
196  if (debug) {
197  // std::cout<<"unwhitened error: "<<err[0]<<" "<<err[1]<<" "<<err[2]<<std::endl;
198  // std::cout<<"err_wh_inlier: "<<err_wh_inlier[0]<<" "<<err_wh_inlier[1]<<" "<<err_wh_inlier[2]<<std::endl;
199  // std::cout<<"err_wh_outlier: "<<err_wh_outlier[0]<<" "<<err_wh_outlier[1]<<" "<<err_wh_outlier[2]<<std::endl;
200  //
201  // std::cout<<"p_inlier, p_outlier, sumP: "<<p_inlier<<" "<<p_outlier<<" " << sumP << std::endl;
202  //
203  // std::cout<<"prior_inlier_, prior_outlier_: "<<prior_inlier_<<" "<<prior_outlier_<< std::endl;
204  //
205  // double s_inl = -0.5 * err_wh_inlier.dot(err_wh_inlier);
206  // double s_outl = -0.5 * err_wh_outlier.dot(err_wh_outlier);
207  // std::cout<<"s_inl, s_outl: "<<s_inl<<" "<<s_outl<<std::endl;
208  //
209  // std::cout<<"norm of invCov_inlier, invCov_outlier: "<<invCov_inlier.norm()<<" "<<invCov_outlier.norm()<<std::endl;
210  // double q_inl = invCov_inlier.norm() * exp( -0.5 * err_wh_inlier.dot(err_wh_inlier) );
211  // double q_outl = invCov_outlier.norm() * exp( -0.5 * err_wh_outlier.dot(err_wh_outlier) );
212  // std::cout<<"q_inl, q_outl: "<<q_inl<<" "<<q_outl<<std::endl;
213 
214  // Matrix Cov_inlier = invCov_inlier.inverse();
215  // Matrix Cov_outlier = invCov_outlier.inverse();
216  // std::cout<<"Cov_inlier: "<<std::endl<<
217  // Cov_inlier(0,0) << " " << Cov_inlier(0,1) << " " << Cov_inlier(0,2) <<std::endl<<
218  // Cov_inlier(1,0) << " " << Cov_inlier(1,1) << " " << Cov_inlier(1,2) <<std::endl<<
219  // Cov_inlier(2,0) << " " << Cov_inlier(2,1) << " " << Cov_inlier(2,2) <<std::endl;
220  // std::cout<<"Cov_outlier: "<<std::endl<<
221  // Cov_outlier(0,0) << " " << Cov_outlier(0,1) << " " << Cov_outlier(0,2) <<std::endl<<
222  // Cov_outlier(1,0) << " " << Cov_outlier(1,1) << " " << Cov_outlier(1,2) <<std::endl<<
223  // Cov_outlier(2,0) << " " << Cov_outlier(2,1) << " " << Cov_outlier(2,2) <<std::endl;
224  // std::cout<<"===="<<std::endl;
225  }
226 
227  return err_wh_eq;
228  }
229 
230  /* ************************************************************************* */
231  Vector calcIndicatorProb(const Values& x) const {
232 
233  bool debug = false;
234 
235  Vector err = unwhitenedError(x);
236 
237  // Calculate indicator probabilities (inlier and outlier)
238  Vector err_wh_inlier = model_inlier_->whiten(err);
239  Vector err_wh_outlier = model_outlier_->whiten(err);
240 
241  Matrix invCov_inlier = model_inlier_->R().transpose() * model_inlier_->R();
242  Matrix invCov_outlier = model_outlier_->R().transpose()
243  * model_outlier_->R();
244 
245  double p_inlier = prior_inlier_ * std::sqrt(invCov_inlier.determinant())
246  * exp(-0.5 * err_wh_inlier.dot(err_wh_inlier));
247  double p_outlier = prior_outlier_ * std::sqrt(invCov_outlier.determinant())
248  * exp(-0.5 * err_wh_outlier.dot(err_wh_outlier));
249 
250  if (debug) {
251  std::cout << "in calcIndicatorProb. err_unwh: " << err[0] << ", "
252  << err[1] << ", " << err[2] << std::endl;
253  std::cout << "in calcIndicatorProb. err_wh_inlier: " << err_wh_inlier[0]
254  << ", " << err_wh_inlier[1] << ", " << err_wh_inlier[2] << std::endl;
255  std::cout << "in calcIndicatorProb. err_wh_inlier.dot(err_wh_inlier): "
256  << err_wh_inlier.dot(err_wh_inlier) << std::endl;
257  std::cout << "in calcIndicatorProb. err_wh_outlier.dot(err_wh_outlier): "
258  << err_wh_outlier.dot(err_wh_outlier) << std::endl;
259 
260  std::cout
261  << "in calcIndicatorProb. p_inlier, p_outlier before normalization: "
262  << p_inlier << ", " << p_outlier << std::endl;
263  }
264 
265  double sumP = p_inlier + p_outlier;
266  p_inlier /= sumP;
267  p_outlier /= sumP;
268 
269  if (flag_bump_up_near_zero_probs_) {
270  // Bump up near-zero probabilities (as in linerFlow.h)
271  double minP = 0.05; // == 0.1 / 2 indicator variables
272  if (p_inlier < minP || p_outlier < minP) {
273  if (p_inlier < minP)
274  p_inlier = minP;
275  if (p_outlier < minP)
276  p_outlier = minP;
277  sumP = p_inlier + p_outlier;
278  p_inlier /= sumP;
279  p_outlier /= sumP;
280  }
281  }
282 
283  return (Vector(2) << p_inlier, p_outlier).finished();
284  }
285 
286  /* ************************************************************************* */
287  Vector unwhitenedError(const Values& x) const {
288 
289  const T& p1 = x.at<T>(key1_);
290  const T& p2 = x.at<T>(key2_);
291 
292  Matrix H1, H2;
293 
294  T hx = p1.between(p2, H1, H2); // h(x)
295 
296  return measured_.localCoordinates(hx);
297  }
298 
299  /* ************************************************************************* */
300  void set_flag_bump_up_near_zero_probs(bool flag) {
301  flag_bump_up_near_zero_probs_ = flag;
302  }
303 
304  /* ************************************************************************* */
305  bool get_flag_bump_up_near_zero_probs() const {
306  return flag_bump_up_near_zero_probs_;
307  }
308 
309  /* ************************************************************************* */
310  SharedGaussian get_model_inlier() const {
311  return model_inlier_;
312  }
313 
314  /* ************************************************************************* */
315  SharedGaussian get_model_outlier() const {
316  return model_outlier_;
317  }
318 
319  /* ************************************************************************* */
320  Matrix get_model_inlier_cov() const {
321  return (model_inlier_->R().transpose() * model_inlier_->R()).inverse();
322  }
323 
324  /* ************************************************************************* */
325  Matrix get_model_outlier_cov() const {
326  return (model_outlier_->R().transpose() * model_outlier_->R()).inverse();
327  }
328 
329  /* ************************************************************************* */
330  void updateNoiseModels(const Values& values,
331  const NonlinearFactorGraph& graph) {
332  /* Update model_inlier_ and model_outlier_ to account for uncertainty in robot trajectories
333  * (note these are given in the E step, where indicator probabilities are calculated).
334  *
335  * Principle: R += [H1 H2] * joint_cov12 * [H1 H2]', where H1, H2 are Jacobians of the
336  * unwhitened error w.r.t. states, and R is the measurement covariance (inlier or outlier modes).
337  *
338  * TODO: improve efficiency (info form)
339  */
340 
341  // get joint covariance of the involved states
342  KeyVector Keys;
343  Keys.push_back(key1_);
344  Keys.push_back(key2_);
345  Marginals marginals(graph, values, Marginals::QR);
346  JointMarginal joint_marginal12 = marginals.jointMarginalCovariance(Keys);
347  Matrix cov1 = joint_marginal12(key1_, key1_);
348  Matrix cov2 = joint_marginal12(key2_, key2_);
349  Matrix cov12 = joint_marginal12(key1_, key2_);
350 
351  updateNoiseModels_givenCovs(values, cov1, cov2, cov12);
352  }
353 
354  /* ************************************************************************* */
355  void updateNoiseModels_givenCovs(const Values& values,
356  const Matrix& cov1, const Matrix& cov2, const Matrix& cov12) {
357  /* Update model_inlier_ and model_outlier_ to account for uncertainty in robot trajectories
358  * (note these are given in the E step, where indicator probabilities are calculated).
359  *
360  * Principle: R += [H1 H2] * joint_cov12 * [H1 H2]', where H1, H2 are Jacobians of the
361  * unwhitened error w.r.t. states, and R is the measurement covariance (inlier or outlier modes).
362  *
363  * TODO: improve efficiency (info form)
364  */
365 
366  const T& p1 = values.at<T>(key1_);
367  const T& p2 = values.at<T>(key2_);
368 
369  Matrix H1, H2;
370  p1.between(p2, H1, H2); // h(x)
371 
372  Matrix H;
373  H.resize(H1.rows(), H1.rows() + H2.rows());
374  H << H1, H2; // H = [H1 H2]
375 
376  Matrix joint_cov;
377  joint_cov.resize(cov1.rows() + cov2.rows(), cov1.cols() + cov2.cols());
378  joint_cov << cov1, cov12, cov12.transpose(), cov2;
379 
380  Matrix cov_state = H * joint_cov * H.transpose();
381 
382  // model_inlier_->print("before:");
383 
384  // update inlier and outlier noise models
385  Matrix covRinlier =
386  (model_inlier_->R().transpose() * model_inlier_->R()).inverse();
387  model_inlier_ = noiseModel::Gaussian::Covariance(
388  covRinlier + cov_state);
389 
390  Matrix covRoutlier =
391  (model_outlier_->R().transpose() * model_outlier_->R()).inverse();
392  model_outlier_ = noiseModel::Gaussian::Covariance(
393  covRoutlier + cov_state);
394 
395  // model_inlier_->print("after:");
396  // std::cout<<"covRinlier + cov_state: "<<covRinlier + cov_state<<std::endl;
397  }
398 
399  /* ************************************************************************* */
401  const VALUE& measured() const {
402  return measured_;
403  }
404 
405  size_t dim() const override {
406  return model_inlier_->R().rows() + model_inlier_->R().cols();
407  }
408 
409 private:
410 
413  template<class ARCHIVE>
414  void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
415  ar
416  & boost::serialization::make_nvp("NonlinearFactor",
417  boost::serialization::base_object<Base>(*this));
418  ar & BOOST_SERIALIZATION_NVP(measured_);
419  }
420 };
421 // \class BetweenFactorEM
422 
423 } // namespace gtsam
gtsam::GaussianFactor::shared_ptr
boost::shared_ptr< This > shared_ptr
shared_ptr to this class
Definition: GaussianFactor.h:42
gtsam::NonlinearFactorGraph
A non-linear factor graph is a graph of non-Gaussian, i.e.
Definition: NonlinearFactorGraph.h:78
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::equals
Template to create a binary predicate.
Definition: Testable.h:110
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:61
gtsam::Marginals
A class for computing Gaussian marginals of variables in a NonlinearFactorGraph.
Definition: Marginals.h:32
gtsam
Global functions in a separate testing namespace.
Definition: chartTesting.h:28
gtsam::NonlinearFactor
Nonlinear factor base class.
Definition: NonlinearFactor.h:43
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::BetweenFactorEM
Definition: BetweenFactorEM.h:34
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::JointMarginal
A class to store and access a joint marginal, returned from Marginals::jointMarginalCovariance and Ma...
Definition: Marginals.h:151
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
GaussianFactor.h
A factor with a quadratic error function - a Gaussian.
gtsam::Marginals::jointMarginalCovariance
JointMarginal jointMarginalCovariance(const KeyVector &variables) const
Compute the joint marginal covariance of several variables.
Definition: Marginals.cpp:134
gtsam::BetweenFactorEM::access
friend class boost::serialization::access
Serialization function.
Definition: BetweenFactorEM.h:412
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
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