gtsam  4.0.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  virtual void print(const std::string& s, const KeyFormatter& keyFormatter =
88  DefaultKeyFormatter) const {
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  virtual bool equals(const NonlinearFactor& f, double tol = 1e-9) const {
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  virtual double error(const Values& x) const {
118  return whitenedError(x).squaredNorm();
119  }
120 
121  /* ************************************************************************* */
127  /* This version of linearize recalculates the noise model each time */
128  virtual boost::shared_ptr<GaussianFactor> linearize(
129  const Values& x) const {
130  // Only linearize if the factor is active
131  if (!this->active(x))
132  return boost::shared_ptr<JacobianFactor>();
133 
134  //std::cout<<"About to linearize"<<std::endl;
135  Matrix A1, A2;
136  std::vector<Matrix> A(this->size());
137  Vector b = -whitenedError(x, A);
138  A1 = A[0];
139  A2 = A[1];
140 
142  new JacobianFactor(key1_, A1, key2_, A2, b,
143  noiseModel::Unit::Create(b.size())));
144  }
145 
146  /* ************************************************************************* */
147  Vector whitenedError(const Values& x,
148  boost::optional<std::vector<Matrix>&> H = boost::none) const {
149 
150  bool debug = true;
151 
152  const T& p1 = x.at<T>(key1_);
153  const T& p2 = x.at<T>(key2_);
154 
155  Matrix H1, H2;
156 
157  T hx = p1.between(p2, H1, H2); // h(x)
158  // manifold equivalent of h(x)-z -> log(z,h(x))
159 
160  Vector err = measured_.localCoordinates(hx);
161 
162  // Calculate indicator probabilities (inlier and outlier)
163  Vector p_inlier_outlier = calcIndicatorProb(x);
164  double p_inlier = p_inlier_outlier[0];
165  double p_outlier = p_inlier_outlier[1];
166 
167  Vector err_wh_inlier = model_inlier_->whiten(err);
168  Vector err_wh_outlier = model_outlier_->whiten(err);
169 
170  Matrix invCov_inlier = model_inlier_->R().transpose() * model_inlier_->R();
171  Matrix invCov_outlier = model_outlier_->R().transpose()
172  * model_outlier_->R();
173 
174  Vector err_wh_eq;
175  err_wh_eq.resize(err_wh_inlier.rows() * 2);
176  err_wh_eq << sqrt(p_inlier) * err_wh_inlier.array(), sqrt(p_outlier)
177  * err_wh_outlier.array();
178 
179  if (H) {
180  // stack Jacobians for the two indicators for each of the key
181 
182  Matrix H1_inlier = sqrt(p_inlier) * model_inlier_->Whiten(H1);
183  Matrix H1_outlier = sqrt(p_outlier) * model_outlier_->Whiten(H1);
184  Matrix H1_aug = stack(2, &H1_inlier, &H1_outlier);
185 
186  Matrix H2_inlier = sqrt(p_inlier) * model_inlier_->Whiten(H2);
187  Matrix H2_outlier = sqrt(p_outlier) * model_outlier_->Whiten(H2);
188  Matrix H2_aug = stack(2, &H2_inlier, &H2_outlier);
189 
190  (*H)[0].resize(H1_aug.rows(), H1_aug.cols());
191  (*H)[1].resize(H2_aug.rows(), H2_aug.cols());
192 
193  (*H)[0] = H1_aug;
194  (*H)[1] = H2_aug;
195  }
196 
197  if (debug) {
198  // std::cout<<"unwhitened error: "<<err[0]<<" "<<err[1]<<" "<<err[2]<<std::endl;
199  // std::cout<<"err_wh_inlier: "<<err_wh_inlier[0]<<" "<<err_wh_inlier[1]<<" "<<err_wh_inlier[2]<<std::endl;
200  // std::cout<<"err_wh_outlier: "<<err_wh_outlier[0]<<" "<<err_wh_outlier[1]<<" "<<err_wh_outlier[2]<<std::endl;
201  //
202  // std::cout<<"p_inlier, p_outlier, sumP: "<<p_inlier<<" "<<p_outlier<<" " << sumP << std::endl;
203  //
204  // std::cout<<"prior_inlier_, prior_outlier_: "<<prior_inlier_<<" "<<prior_outlier_<< std::endl;
205  //
206  // double s_inl = -0.5 * err_wh_inlier.dot(err_wh_inlier);
207  // double s_outl = -0.5 * err_wh_outlier.dot(err_wh_outlier);
208  // std::cout<<"s_inl, s_outl: "<<s_inl<<" "<<s_outl<<std::endl;
209  //
210  // std::cout<<"norm of invCov_inlier, invCov_outlier: "<<invCov_inlier.norm()<<" "<<invCov_outlier.norm()<<std::endl;
211  // double q_inl = invCov_inlier.norm() * exp( -0.5 * err_wh_inlier.dot(err_wh_inlier) );
212  // double q_outl = invCov_outlier.norm() * exp( -0.5 * err_wh_outlier.dot(err_wh_outlier) );
213  // std::cout<<"q_inl, q_outl: "<<q_inl<<" "<<q_outl<<std::endl;
214 
215  // Matrix Cov_inlier = invCov_inlier.inverse();
216  // Matrix Cov_outlier = invCov_outlier.inverse();
217  // std::cout<<"Cov_inlier: "<<std::endl<<
218  // Cov_inlier(0,0) << " " << Cov_inlier(0,1) << " " << Cov_inlier(0,2) <<std::endl<<
219  // Cov_inlier(1,0) << " " << Cov_inlier(1,1) << " " << Cov_inlier(1,2) <<std::endl<<
220  // Cov_inlier(2,0) << " " << Cov_inlier(2,1) << " " << Cov_inlier(2,2) <<std::endl;
221  // std::cout<<"Cov_outlier: "<<std::endl<<
222  // Cov_outlier(0,0) << " " << Cov_outlier(0,1) << " " << Cov_outlier(0,2) <<std::endl<<
223  // Cov_outlier(1,0) << " " << Cov_outlier(1,1) << " " << Cov_outlier(1,2) <<std::endl<<
224  // Cov_outlier(2,0) << " " << Cov_outlier(2,1) << " " << Cov_outlier(2,2) <<std::endl;
225  // std::cout<<"===="<<std::endl;
226  }
227 
228  return err_wh_eq;
229  }
230 
231  /* ************************************************************************* */
232  Vector calcIndicatorProb(const Values& x) const {
233 
234  bool debug = false;
235 
236  Vector err = unwhitenedError(x);
237 
238  // Calculate indicator probabilities (inlier and outlier)
239  Vector err_wh_inlier = model_inlier_->whiten(err);
240  Vector err_wh_outlier = model_outlier_->whiten(err);
241 
242  Matrix invCov_inlier = model_inlier_->R().transpose() * model_inlier_->R();
243  Matrix invCov_outlier = model_outlier_->R().transpose()
244  * model_outlier_->R();
245 
246  double p_inlier = prior_inlier_ * std::sqrt(invCov_inlier.determinant())
247  * exp(-0.5 * err_wh_inlier.dot(err_wh_inlier));
248  double p_outlier = prior_outlier_ * std::sqrt(invCov_outlier.determinant())
249  * exp(-0.5 * err_wh_outlier.dot(err_wh_outlier));
250 
251  if (debug) {
252  std::cout << "in calcIndicatorProb. err_unwh: " << err[0] << ", "
253  << err[1] << ", " << err[2] << std::endl;
254  std::cout << "in calcIndicatorProb. err_wh_inlier: " << err_wh_inlier[0]
255  << ", " << err_wh_inlier[1] << ", " << err_wh_inlier[2] << std::endl;
256  std::cout << "in calcIndicatorProb. err_wh_inlier.dot(err_wh_inlier): "
257  << err_wh_inlier.dot(err_wh_inlier) << std::endl;
258  std::cout << "in calcIndicatorProb. err_wh_outlier.dot(err_wh_outlier): "
259  << err_wh_outlier.dot(err_wh_outlier) << std::endl;
260 
261  std::cout
262  << "in calcIndicatorProb. p_inlier, p_outlier before normalization: "
263  << p_inlier << ", " << p_outlier << std::endl;
264  }
265 
266  double sumP = p_inlier + p_outlier;
267  p_inlier /= sumP;
268  p_outlier /= sumP;
269 
270  if (flag_bump_up_near_zero_probs_) {
271  // Bump up near-zero probabilities (as in linerFlow.h)
272  double minP = 0.05; // == 0.1 / 2 indicator variables
273  if (p_inlier < minP || p_outlier < minP) {
274  if (p_inlier < minP)
275  p_inlier = minP;
276  if (p_outlier < minP)
277  p_outlier = minP;
278  sumP = p_inlier + p_outlier;
279  p_inlier /= sumP;
280  p_outlier /= sumP;
281  }
282  }
283 
284  return (Vector(2) << p_inlier, p_outlier).finished();
285  }
286 
287  /* ************************************************************************* */
288  Vector unwhitenedError(const Values& x) const {
289 
290  const T& p1 = x.at<T>(key1_);
291  const T& p2 = x.at<T>(key2_);
292 
293  Matrix H1, H2;
294 
295  T hx = p1.between(p2, H1, H2); // h(x)
296 
297  return measured_.localCoordinates(hx);
298  }
299 
300  /* ************************************************************************* */
301  void set_flag_bump_up_near_zero_probs(bool flag) {
302  flag_bump_up_near_zero_probs_ = flag;
303  }
304 
305  /* ************************************************************************* */
306  bool get_flag_bump_up_near_zero_probs() const {
307  return flag_bump_up_near_zero_probs_;
308  }
309 
310  /* ************************************************************************* */
311  SharedGaussian get_model_inlier() const {
312  return model_inlier_;
313  }
314 
315  /* ************************************************************************* */
316  SharedGaussian get_model_outlier() const {
317  return model_outlier_;
318  }
319 
320  /* ************************************************************************* */
321  Matrix get_model_inlier_cov() const {
322  return (model_inlier_->R().transpose() * model_inlier_->R()).inverse();
323  }
324 
325  /* ************************************************************************* */
326  Matrix get_model_outlier_cov() const {
327  return (model_outlier_->R().transpose() * model_outlier_->R()).inverse();
328  }
329 
330  /* ************************************************************************* */
331  void updateNoiseModels(const Values& values,
332  const NonlinearFactorGraph& graph) {
333  /* Update model_inlier_ and model_outlier_ to account for uncertainty in robot trajectories
334  * (note these are given in the E step, where indicator probabilities are calculated).
335  *
336  * Principle: R += [H1 H2] * joint_cov12 * [H1 H2]', where H1, H2 are Jacobians of the
337  * unwhitened error w.r.t. states, and R is the measurement covariance (inlier or outlier modes).
338  *
339  * TODO: improve efficiency (info form)
340  */
341 
342  // get joint covariance of the involved states
343  KeyVector Keys;
344  Keys.push_back(key1_);
345  Keys.push_back(key2_);
346  Marginals marginals(graph, values, Marginals::QR);
347  JointMarginal joint_marginal12 = marginals.jointMarginalCovariance(Keys);
348  Matrix cov1 = joint_marginal12(key1_, key1_);
349  Matrix cov2 = joint_marginal12(key2_, key2_);
350  Matrix cov12 = joint_marginal12(key1_, key2_);
351 
352  updateNoiseModels_givenCovs(values, cov1, cov2, cov12);
353  }
354 
355  /* ************************************************************************* */
356  void updateNoiseModels_givenCovs(const Values& values,
357  const Matrix& cov1, const Matrix& cov2, const Matrix& cov12) {
358  /* Update model_inlier_ and model_outlier_ to account for uncertainty in robot trajectories
359  * (note these are given in the E step, where indicator probabilities are calculated).
360  *
361  * Principle: R += [H1 H2] * joint_cov12 * [H1 H2]', where H1, H2 are Jacobians of the
362  * unwhitened error w.r.t. states, and R is the measurement covariance (inlier or outlier modes).
363  *
364  * TODO: improve efficiency (info form)
365  */
366 
367  const T& p1 = values.at<T>(key1_);
368  const T& p2 = values.at<T>(key2_);
369 
370  Matrix H1, H2;
371  p1.between(p2, H1, H2); // h(x)
372 
373  Matrix H;
374  H.resize(H1.rows(), H1.rows() + H2.rows());
375  H << H1, H2; // H = [H1 H2]
376 
377  Matrix joint_cov;
378  joint_cov.resize(cov1.rows() + cov2.rows(), cov1.cols() + cov2.cols());
379  joint_cov << cov1, cov12, cov12.transpose(), cov2;
380 
381  Matrix cov_state = H * joint_cov * H.transpose();
382 
383  // model_inlier_->print("before:");
384 
385  // update inlier and outlier noise models
386  Matrix covRinlier =
387  (model_inlier_->R().transpose() * model_inlier_->R()).inverse();
388  model_inlier_ = noiseModel::Gaussian::Covariance(
389  covRinlier + cov_state);
390 
391  Matrix covRoutlier =
392  (model_outlier_->R().transpose() * model_outlier_->R()).inverse();
393  model_outlier_ = noiseModel::Gaussian::Covariance(
394  covRoutlier + cov_state);
395 
396  // model_inlier_->print("after:");
397  // std::cout<<"covRinlier + cov_state: "<<covRinlier + cov_state<<std::endl;
398  }
399 
400  /* ************************************************************************* */
402  const VALUE& measured() const {
403  return measured_;
404  }
405 
407  std::size_t size() const {
408  return 2;
409  }
410 
411  virtual size_t dim() const {
412  return model_inlier_->R().rows() + model_inlier_->R().cols();
413  }
414 
415 private:
416 
419  template<class ARCHIVE>
420  void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
421  ar
422  & boost::serialization::make_nvp("NonlinearFactor",
423  boost::serialization::base_object<Base>(*this));
424  ar & BOOST_SERIALIZATION_NVP(measured_);
425  }
426 };
427 // \class BetweenFactorEM
428 
429 } // namespace gtsam
JointMarginal jointMarginalCovariance(const KeyVector &variables) const
Compute the joint marginal covariance of several variables.
Definition: Marginals.cpp:83
This is the base class for all factor types.
Definition: Factor.h:54
A non-templated config holding any types of Manifold-group elements.
Definition: Values.h:70
ValueType at(Key j) const
Retrieve a variable by key j.
Definition: Values-inl.h:342
A class to store and access a joint marginal, returned from Marginals::jointMarginalCovariance and Ma...
Definition: Marginals.h:89
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:57
friend class boost::serialization::access
Serialization function.
Definition: BetweenFactorEM.h:418
Template to create a binary predicate.
Definition: Testable.h:110
Definition: BetweenFactorEM.h:34
virtual bool equals(const NonlinearFactor &f, double tol=1e-9) const
Check if two factors are equal.
Definition: NonlinearFactor.cpp:36
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:33
Nonlinear factor base class.
Definition: NonlinearFactor.h:50
A class for computing marginals in a NonlinearFactorGraph.
static shared_ptr Create(size_t dim)
Create a unit covariance noise model.
Definition: NoiseModel.h:601
FastVector< Key > KeyVector
Define collection type once and for all - also used in wrappers.
Definition: Key.h:56
Non-linear factor base classes.
A non-linear factor graph is a graph of non-Gaussian, i.e.
Definition: NonlinearFactorGraph.h:77
A Gaussian factor in the squared-error form.
Definition: JacobianFactor.h:87
Matrix stack(size_t nrMatrices,...)
create a matrix by stacking other matrices Given a set of matrices: A1, A2, A3...
Definition: Matrix.cpp:392
virtual bool active(const Values &) const
Checks whether a factor should be used based on a set of values.
Definition: NonlinearFactor.h:113
A factor with a quadratic error function - a Gaussian.
Global functions in a separate testing namespace.
Definition: chartTesting.h:28
Concept check for values that can be used in unit tests.
A class for computing Gaussian marginals of variables in a NonlinearFactorGraph.
Definition: Marginals.h:32
static shared_ptr Covariance(const Matrix &covariance, bool smart=true)
A Gaussian noise model created by specifying a covariance matrix.
Definition: NoiseModel.cpp:112
Base class and basic functions for Lie types.
bool equals(const This &other, double tol=1e-9) const
check equality
Definition: Factor.cpp:42
boost::shared_ptr< This > shared_ptr
shared_ptr to this class
Definition: GaussianFactor.h:42