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 
47 
49 
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 
61 
62  double prior_inlier_;
64 
66  mutable bool start_with_M_step_;
67 
71 
72  public:
73 
74  // shorthand for a smart pointer to a factor
75  typedef typename std::shared_ptr<TransformBtwRobotsUnaryFactorEM> shared_ptr;
76 
79 
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) :
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 std::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  std::shared_ptr<GaussianFactor> linearize(const Values& x) const override {
166  // Only linearize if the factor is active
167  if (!this->active(x))
168  return std::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 
178  }
179 
180 
181  /* ************************************************************************* */
185  Vector whitenedError(const Values& x, OptionalMatrixVecType H = nullptr) const {
186 
187  bool debug = true;
188 
189  Matrix H_compose, H_between1, H_dummy;
190 
191  T orgA_T_currA = valA_.at<T>(keyA_);
192  T orgB_T_currB = valB_.at<T>(keyB_);
193 
194  T orgA_T_orgB = x.at<T>(key_);
195 
196  T orgA_T_currB = orgA_T_orgB.compose(orgB_T_currB, H_compose, H_dummy);
197 
198  T currA_T_currB_pred = orgA_T_currA.between(orgA_T_currB, H_dummy, H_between1);
199 
200  T currA_T_currB_msr = measured_;
201 
202  Vector err = currA_T_currB_msr.localCoordinates(currA_T_currB_pred);
203 
204  // Calculate indicator probabilities (inlier and outlier)
205  Vector p_inlier_outlier = calcIndicatorProb(x, err);
206  double p_inlier = p_inlier_outlier[0];
207  double p_outlier = p_inlier_outlier[1];
208 
209  if (start_with_M_step_){
210  start_with_M_step_ = false;
211 
212  p_inlier = 0.5;
213  p_outlier = 0.5;
214  }
215 
216  Vector err_wh_inlier = model_inlier_->whiten(err);
217  Vector err_wh_outlier = model_outlier_->whiten(err);
218 
219  Matrix invCov_inlier = model_inlier_->R().transpose() * model_inlier_->R();
220  Matrix invCov_outlier = model_outlier_->R().transpose() * model_outlier_->R();
221 
222  Vector err_wh_eq;
223  err_wh_eq.resize(err_wh_inlier.rows()*2);
224  err_wh_eq << sqrt(p_inlier) * err_wh_inlier.array() , sqrt(p_outlier) * err_wh_outlier.array();
225 
226  Matrix H_unwh = H_compose * H_between1;
227 
228  if (H){
229 
230  Matrix H_inlier = sqrt(p_inlier)*model_inlier_->Whiten(H_unwh);
231  Matrix H_outlier = sqrt(p_outlier)*model_outlier_->Whiten(H_unwh);
232  Matrix H_aug = stack(2, &H_inlier, &H_outlier);
233 
234  (*H)[0].resize(H_aug.rows(),H_aug.cols());
235  (*H)[0] = H_aug;
236  }
237 
238  if (debug){
239  // std::cout<<"H_compose - rows, cols, : "<<H_compose.rows()<<", "<< H_compose.cols()<<std::endl;
240  // std::cout<<"H_between1 - rows, cols, : "<<H_between1.rows()<<", "<< H_between1.cols()<<std::endl;
241  // std::cout<<"H_unwh - rows, cols, : "<<H_unwh.rows()<<", "<< H_unwh.cols()<<std::endl;
242  // std::cout<<"H_unwh: "<<std:endl<<H_unwh[0]
243 
244  }
245 
246 
247  return err_wh_eq;
248  }
249 
250  /* ************************************************************************* */
251  Vector whitenedError(const Values& x, std::vector<Matrix>& H) const {
252  return whitenedError(x, &H);
253  }
254 
255  /* ************************************************************************* */
257 
258  Vector err = unwhitenedError(x);
259 
260  return this->calcIndicatorProb(x, err);
261  }
262 
263  /* ************************************************************************* */
264  Vector calcIndicatorProb(const Values& x, const Vector& err) const {
265 
266  // Calculate indicator probabilities (inlier and outlier)
267  Vector err_wh_inlier = model_inlier_->whiten(err);
268  Vector err_wh_outlier = model_outlier_->whiten(err);
269 
270  Matrix invCov_inlier = model_inlier_->R().transpose() * model_inlier_->R();
271  Matrix invCov_outlier = model_outlier_->R().transpose() * model_outlier_->R();
272 
273  double p_inlier = prior_inlier_ * sqrt(invCov_inlier.norm()) * exp( -0.5 * err_wh_inlier.dot(err_wh_inlier) );
274  double p_outlier = prior_outlier_ * sqrt(invCov_outlier.norm()) * exp( -0.5 * err_wh_outlier.dot(err_wh_outlier) );
275 
276  double sumP = p_inlier + p_outlier;
277  p_inlier /= sumP;
278  p_outlier /= sumP;
279 
281  // Bump up near-zero probabilities (as in linerFlow.h)
282  double minP = 0.05; // == 0.1 / 2 indicator variables
283  if (p_inlier < minP || p_outlier < minP){
284  if (p_inlier < minP)
285  p_inlier = minP;
286  if (p_outlier < minP)
287  p_outlier = minP;
288  sumP = p_inlier + p_outlier;
289  p_inlier /= sumP;
290  p_outlier /= sumP;
291  }
292  }
293 
294  return (Vector(2) << p_inlier, p_outlier).finished();
295  }
296 
297  /* ************************************************************************* */
298  Vector unwhitenedError(const Values& x) const {
299 
300  T orgA_T_currA = valA_.at<T>(keyA_);
301  T orgB_T_currB = valB_.at<T>(keyB_);
302 
303  T orgA_T_orgB = x.at<T>(key_);
304 
305  T orgA_T_currB = orgA_T_orgB.compose(orgB_T_currB);
306 
307  T currA_T_currB_pred = orgA_T_currA.between(orgA_T_currB);
308 
309  T currA_T_currB_msr = measured_;
310 
311  return currA_T_currB_msr.localCoordinates(currA_T_currB_pred);
312  }
313 
314  /* ************************************************************************* */
316  return model_inlier_;
317  }
318 
319  /* ************************************************************************* */
321  return model_outlier_;
322  }
323 
324  /* ************************************************************************* */
326  return (model_inlier_->R().transpose()*model_inlier_->R()).inverse();
327  }
328 
329  /* ************************************************************************* */
331  return (model_outlier_->R().transpose()*model_outlier_->R()).inverse();
332  }
333 
334  /* ************************************************************************* */
336  /* given marginals version, don't need to marginal multiple times if update a lot */
337 
338  KeyVector Keys;
339  Keys.push_back(keyA_);
340  Keys.push_back(keyB_);
341  JointMarginal joint_marginal12 = marginals.jointMarginalCovariance(Keys);
342  Matrix cov1 = joint_marginal12(keyA_, keyA_);
343  Matrix cov2 = joint_marginal12(keyB_, keyB_);
344  Matrix cov12 = joint_marginal12(keyA_, keyB_);
345 
346  updateNoiseModels_givenCovs(values, cov1, cov2, cov12);
347  }
348 
349  /* ************************************************************************* */
351  /* Update model_inlier_ and model_outlier_ to account for uncertainty in robot trajectories
352  * (note these are given in the E step, where indicator probabilities are calculated).
353  *
354  * Principle: R += [H1 H2] * joint_cov12 * [H1 H2]', where H1, H2 are Jacobians of the
355  * unwhitened error w.r.t. states, and R is the measurement covariance (inlier or outlier modes).
356  *
357  * TODO: improve efficiency (info form)
358  */
359 
360  // get joint covariance of the involved states
361 
363 
364  this->updateNoiseModels(values, marginals);
365  }
366 
367  /* ************************************************************************* */
368  void updateNoiseModels_givenCovs(const Values& values, const Matrix& cov1, const Matrix& cov2, const Matrix& cov12){
369  /* Update model_inlier_ and model_outlier_ to account for uncertainty in robot trajectories
370  * (note these are given in the E step, where indicator probabilities are calculated).
371  *
372  * Principle: R += [H1 H2] * joint_cov12 * [H1 H2]', where H1, H2 are Jacobians of the
373  * unwhitened error w.r.t. states, and R is the measurement covariance (inlier or outlier modes).
374  *
375  * TODO: improve efficiency (info form)
376  */
377 
378  const T& p1 = values.at<T>(keyA_);
379  const T& p2 = values.at<T>(keyB_);
380 
381  Matrix H1, H2;
382  p1.between(p2, H1, H2); // h(x)
383 
384  Matrix H;
385  H.resize(H1.rows(), H1.rows()+H2.rows());
386  H << H1, H2; // H = [H1 H2]
387 
388  Matrix joint_cov;
389  joint_cov.resize(cov1.rows()+cov2.rows(), cov1.cols()+cov2.cols());
390  joint_cov << cov1, cov12,
391  cov12.transpose(), cov2;
392 
393  Matrix cov_state = H*joint_cov*H.transpose();
394 
395  // model_inlier_->print("before:");
396 
397  // update inlier and outlier noise models
398  Matrix covRinlier = (model_inlier_->R().transpose()*model_inlier_->R()).inverse();
399  model_inlier_ = noiseModel::Gaussian::Covariance(covRinlier + cov_state);
400 
401  Matrix covRoutlier = (model_outlier_->R().transpose()*model_outlier_->R()).inverse();
402  model_outlier_ = noiseModel::Gaussian::Covariance(covRoutlier + cov_state);
403 
404  // model_inlier_->print("after:");
405  // std::cout<<"covRinlier + cov_state: "<<covRinlier + cov_state<<std::endl;
406  }
407 
408 
409  /* ************************************************************************* */
410 
411  size_t dim() const override {
412  return model_inlier_->R().rows() + model_inlier_->R().cols();
413  }
414 
415  private:
416 
417 #ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
418 
419  friend class boost::serialization::access;
420  template<class ARCHIVE>
421  void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
422  ar & boost::serialization::make_nvp("NonlinearFactor",
423  boost::serialization::base_object<Base>(*this));
424  //ar & BOOST_SERIALIZATION_NVP(measured_);
425  }
426 #endif
427  }; // \class TransformBtwRobotsUnaryFactorEM
428 
430  template<class VALUE>
432  public Testable<TransformBtwRobotsUnaryFactorEM<VALUE> > {
433  };
434 
435 }
H
set noclip points set clip one set noclip two set bar set border lt lw set xdata set ydata set zdata set x2data set y2data set boxwidth set dummy y set format x g set format y g set format x2 g set format y2 g set format z g set angles radians set nogrid set key title set key left top Right noreverse box linetype linewidth samplen spacing width set nolabel set noarrow set nologscale set logscale x set set pointsize set encoding default set nopolar set noparametric set set set set surface set nocontour set clabel set mapping cartesian set nohidden3d set cntrparam order set cntrparam linear set cntrparam levels auto set cntrparam points set size set set xzeroaxis lt lw set x2zeroaxis lt lw set yzeroaxis lt lw set y2zeroaxis lt lw set tics in set ticslevel set tics set mxtics default set mytics default set mx2tics default set my2tics default set xtics border mirror norotate autofreq set ytics border mirror norotate autofreq set ztics border nomirror norotate autofreq set nox2tics set noy2tics set timestamp bottom norotate set rrange[*:*] noreverse nowriteback set trange[*:*] noreverse nowriteback set urange[*:*] noreverse nowriteback set vrange[*:*] noreverse nowriteback set xlabel matrix size set x2label set timefmt d m y n H
Definition: gnuplot_common_settings.hh:74
gtsam::NonlinearFactor::shared_ptr
std::shared_ptr< This > shared_ptr
Definition: NonlinearFactor.h:78
gtsam::TransformBtwRobotsUnaryFactorEM::updateNoiseModels_givenCovs
void updateNoiseModels_givenCovs(const Values &values, const Matrix &cov1, const Matrix &cov2, const Matrix &cov12)
Definition: TransformBtwRobotsUnaryFactorEM.h:368
gtsam::Values::exists
bool exists(Key j) const
Definition: Values.cpp:93
gtsam::TransformBtwRobotsUnaryFactorEM::get_model_outlier_cov
Matrix get_model_outlier_cov() const
Definition: TransformBtwRobotsUnaryFactorEM.h:330
gtsam::TransformBtwRobotsUnaryFactorEM::unwhitenedError
Vector unwhitenedError(const Values &x) const
Definition: TransformBtwRobotsUnaryFactorEM.h:298
gtsam::TransformBtwRobotsUnaryFactorEM::keyB_
Key keyB_
Definition: TransformBtwRobotsUnaryFactorEM.h:55
inverse
const EIGEN_DEVICE_FUNC InverseReturnType inverse() const
Definition: ArrayCwiseUnaryOps.h:411
gtsam::TransformBtwRobotsUnaryFactorEM::shared_ptr
std::shared_ptr< TransformBtwRobotsUnaryFactorEM > shared_ptr
Definition: TransformBtwRobotsUnaryFactorEM.h:75
gtsam::TransformBtwRobotsUnaryFactorEM::equals
bool equals(const NonlinearFactor &f, double tol=1e-9) const override
Definition: TransformBtwRobotsUnaryFactorEM.h:122
s
RealScalar s
Definition: level1_cplx_impl.h:126
e
Array< double, 1, 3 > e(1./3., 0.5, 2.)
Testable.h
Concept check for values that can be used in unit tests.
gtsam::TransformBtwRobotsUnaryFactorEM
Definition: TransformBtwRobotsUnaryFactorEM.h:37
x
set noclip points set clip one set noclip two set bar set border lt lw set xdata set ydata set zdata set x2data set y2data set boxwidth set dummy x
Definition: gnuplot_common_settings.hh:12
gtsam::TransformBtwRobotsUnaryFactorEM::T
VALUE T
Definition: TransformBtwRobotsUnaryFactorEM.h:41
gtsam::JacobianFactor
Definition: JacobianFactor.h:91
gtsam::NonlinearFactor::equals
virtual bool equals(const NonlinearFactor &f, double tol=1e-9) const
Definition: NonlinearFactor.cpp:45
gtsam::Marginals
Definition: Marginals.h:32
gtsam::TransformBtwRobotsUnaryFactorEM::~TransformBtwRobotsUnaryFactorEM
~TransformBtwRobotsUnaryFactorEM() override
Definition: TransformBtwRobotsUnaryFactorEM.h:96
measured
Point2 measured(-17, 30)
gtsam::Matrix
Eigen::MatrixXd Matrix
Definition: base/Matrix.h:39
different_sigmas::values
HybridValues values
Definition: testHybridBayesNet.cpp:245
gtsam::TransformBtwRobotsUnaryFactorEM::setValAValB
void setValAValB(const Values &valA, const Values &valB)
Definition: TransformBtwRobotsUnaryFactorEM.h:137
gtsam::Factor
Definition: Factor.h:70
exp
const EIGEN_DEVICE_FUNC ExpReturnType exp() const
Definition: ArrayCwiseUnaryOps.h:97
gtsam::TransformBtwRobotsUnaryFactorEM::valB_
Values valB_
Definition: TransformBtwRobotsUnaryFactorEM.h:53
gtsam::NonlinearFactor::active
virtual bool active(const Values &) const
Definition: NonlinearFactor.h:141
gtsam::TransformBtwRobotsUnaryFactorEM::model_inlier_
SharedGaussian model_inlier_
Definition: TransformBtwRobotsUnaryFactorEM.h:59
gtsam::Vector
Eigen::VectorXd Vector
Definition: Vector.h:38
gtsam::KeyVector
FastVector< Key > KeyVector
Define collection type once and for all - also used in wrappers.
Definition: Key.h:92
gtsam::TransformBtwRobotsUnaryFactorEM::prior_outlier_
double prior_outlier_
Definition: TransformBtwRobotsUnaryFactorEM.h:63
gtsam::TransformBtwRobotsUnaryFactorEM::get_model_inlier_cov
Matrix get_model_inlier_cov() const
Definition: TransformBtwRobotsUnaryFactorEM.h:325
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)
Definition: TransformBtwRobotsUnaryFactorEM.h:81
gtsam::TransformBtwRobotsUnaryFactorEM::whitenedError
Vector whitenedError(const Values &x, std::vector< Matrix > &H) const
Definition: TransformBtwRobotsUnaryFactorEM.h:251
gtsam::DefaultKeyFormatter
KeyFormatter DefaultKeyFormatter
Assign default key formatter.
Definition: Key.cpp:30
gtsam::TransformBtwRobotsUnaryFactorEM::start_with_M_step_
bool start_with_M_step_
Definition: TransformBtwRobotsUnaryFactorEM.h:66
GTSAM_CONCEPT_LIE_TYPE
#define GTSAM_CONCEPT_LIE_TYPE(T)
Definition: Lie.h:373
gtsam::TransformBtwRobotsUnaryFactorEM::get_model_outlier
SharedGaussian get_model_outlier() const
Definition: TransformBtwRobotsUnaryFactorEM.h:320
gtsam::TransformBtwRobotsUnaryFactorEM::valA_
Values valA_
Definition: TransformBtwRobotsUnaryFactorEM.h:52
simple::p2
static Point3 p2
Definition: testInitializePose3.cpp:51
Eigen::internal::VALUE
@ VALUE
Definition: SpecialFunctionsImpl.h:729
gtsam::Values::at
const ValueType at(Key j) const
Definition: Values-inl.h:261
A
Definition: test_numpy_dtypes.cpp:298
gtsam::TransformBtwRobotsUnaryFactorEM::updateNoiseModels
void updateNoiseModels(const Values &values, const NonlinearFactorGraph &graph)
Definition: TransformBtwRobotsUnaryFactorEM.h:350
BetweenFactor.h
gtsam::KeyFormatter
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
gtsam::stack
Matrix stack(size_t nrMatrices,...)
Definition: Matrix.cpp:395
gtsam::TransformBtwRobotsUnaryFactorEM::calcIndicatorProb
Vector calcIndicatorProb(const Values &x, const Vector &err) const
Definition: TransformBtwRobotsUnaryFactorEM.h:264
gtsam::TransformBtwRobotsUnaryFactorEM::linearize
std::shared_ptr< GaussianFactor > linearize(const Values &x) const override
Definition: TransformBtwRobotsUnaryFactorEM.h:165
gtsam::NonlinearFactorGraph
Definition: NonlinearFactorGraph.h:55
debug
static constexpr bool debug
Definition: testDiscreteBayesTree.cpp:31
gtsam::GaussianFactor::shared_ptr
std::shared_ptr< This > shared_ptr
shared_ptr to this class
Definition: GaussianFactor.h:42
gtsam::noiseModel::Unit::Create
static shared_ptr Create(size_t dim)
Definition: NoiseModel.h:631
gtsam::TransformBtwRobotsUnaryFactorEM::This
TransformBtwRobotsUnaryFactorEM< VALUE > This
Definition: TransformBtwRobotsUnaryFactorEM.h:45
gtsam::TransformBtwRobotsUnaryFactorEM::measured_
VALUE measured_
Definition: TransformBtwRobotsUnaryFactorEM.h:50
gtsam::SharedGaussian
noiseModel::Gaussian::shared_ptr SharedGaussian
Definition: NoiseModel.h:763
gtsam::TransformBtwRobotsUnaryFactorEM::error
double error(const Values &x) const override
Definition: TransformBtwRobotsUnaryFactorEM.h:154
p1
Vector3f p1
Definition: MatrixBase_all.cpp:2
key
const gtsam::Symbol key('X', 0)
tree::f
Point2(* f)(const Point3 &, OptionalJacobian< 2, 3 >)
Definition: testExpression.cpp:218
NonlinearFactor.h
Non-linear factor base classes.
gtsam::TransformBtwRobotsUnaryFactorEM::dim
size_t dim() const override
Definition: TransformBtwRobotsUnaryFactorEM.h:411
gtsam::OptionalMatrixVecType
std::vector< Matrix > * OptionalMatrixVecType
Definition: NonlinearFactor.h:61
gtsam::b
const G & b
Definition: Group.h:79
gtsam::TransformBtwRobotsUnaryFactorEM::print
void print(const std::string &s, const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
Definition: TransformBtwRobotsUnaryFactorEM.h:106
gtsam::TransformBtwRobotsUnaryFactorEM::TransformBtwRobotsUnaryFactorEM
TransformBtwRobotsUnaryFactorEM()
Definition: TransformBtwRobotsUnaryFactorEM.h:78
Lie.h
Base class and basic functions for Lie types.
gtsam
traits
Definition: SFMdata.h:40
gtsam::Testable
Definition: Testable.h:152
Marginals.h
A class for computing marginals in a NonlinearFactorGraph.
gtsam::traits
Definition: Group.h:36
NonlinearFactorGraph.h
Factor Graph consisting of non-linear factors.
keyA
Key keyA
Definition: testFunctorizedFactor.cpp:36
gtsam::Values
Definition: Values.h:65
gtsam::TransformBtwRobotsUnaryFactorEM::calcIndicatorProb
Vector calcIndicatorProb(const Values &x) const
Definition: TransformBtwRobotsUnaryFactorEM.h:256
gtsam::NonlinearFactor
Definition: NonlinearFactor.h:68
GTSAM_CONCEPT_TESTABLE_TYPE
#define GTSAM_CONCEPT_TESTABLE_TYPE(T)
Definition: Testable.h:177
A1
static const double A1[]
Definition: expn.h:6
gtsam::TransformBtwRobotsUnaryFactorEM::keyA_
Key keyA_
Definition: TransformBtwRobotsUnaryFactorEM.h:54
gtsam::TransformBtwRobotsUnaryFactorEM::get_model_inlier
SharedGaussian get_model_inlier() const
Definition: TransformBtwRobotsUnaryFactorEM.h:315
gtsam::TransformBtwRobotsUnaryFactorEM::prior_inlier_
double prior_inlier_
Definition: TransformBtwRobotsUnaryFactorEM.h:62
gtsam::TransformBtwRobotsUnaryFactorEM::flag_bump_up_near_zero_probs_
bool flag_bump_up_near_zero_probs_
Definition: TransformBtwRobotsUnaryFactorEM.h:65
gtsam::tol
const G double tol
Definition: Group.h:79
gtsam::TransformBtwRobotsUnaryFactorEM::key_
Key key_
Definition: TransformBtwRobotsUnaryFactorEM.h:48
GaussianFactor.h
A factor with a quadratic error function - a Gaussian.
gtsam::Marginals::QR
@ QR
Definition: Marginals.h:39
graph
NonlinearFactorGraph graph
Definition: doc/Code/OdometryExample.cpp:2
align_3::t
Point2 t(10, 10)
gtsam::JointMarginal
Definition: Marginals.h:137
marginals
Marginals marginals(graph, result)
gtsam::Factor::size
size_t size() const
Definition: Factor.h:160
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:97
gtsam::TransformBtwRobotsUnaryFactorEM::model_outlier_
SharedGaussian model_outlier_
Definition: TransformBtwRobotsUnaryFactorEM.h:60
gtsam::TransformBtwRobotsUnaryFactorEM::whitenedError
Vector whitenedError(const Values &x, OptionalMatrixVecType H=nullptr) const
Definition: TransformBtwRobotsUnaryFactorEM.h:185
gtsam::TransformBtwRobotsUnaryFactorEM::Base
NonlinearFactor Base
Definition: TransformBtwRobotsUnaryFactorEM.h:46
ceres::sqrt
Jet< T, N > sqrt(const Jet< T, N > &f)
Definition: jet.h:418
gtsam::TransformBtwRobotsUnaryFactorEM::updateNoiseModels
void updateNoiseModels(const Values &values, const Marginals &marginals)
Definition: TransformBtwRobotsUnaryFactorEM.h:335
gtsam::TransformBtwRobotsUnaryFactorEM::clone
NonlinearFactor::shared_ptr clone() const override
Definition: TransformBtwRobotsUnaryFactorEM.h:100
gtsam::noiseModel::Gaussian::Covariance
static shared_ptr Covariance(const Matrix &covariance, bool smart=true)
Definition: NoiseModel.cpp:114


gtsam
Author(s):
autogenerated on Sat Nov 16 2024 04:09:16