ReferenceFrameFactor.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 
12 /*
13  * @file ReferenceFrameFactor.h
14  * @brief A constraint for combining graphs by common landmarks and a transform node
15  * @author Alex Cunningham
16  */
17 
18 #pragma once
19 
21 
22 namespace gtsam {
23 
29 template<class T, class P>
31  const T& trans, const P& global,
32  boost::optional<Matrix&> Dtrans,
33  boost::optional<Matrix&> Dglobal) {
34  return trans.transformFrom(global, Dtrans, Dglobal);
35 }
36 
56 template<class POINT, class TRANSFORM>
57 class ReferenceFrameFactor : public NoiseModelFactor3<POINT, TRANSFORM, POINT> {
58 protected:
61 
62 public:
65 
66  typedef POINT Point;
67  typedef TRANSFORM Transform;
68 
72  ReferenceFrameFactor(Key globalKey, Key transKey, Key localKey, const noiseModel::Base::shared_ptr& model)
73  : Base(model,globalKey, transKey, localKey) {}
74 
79  ReferenceFrameFactor(double mu, Key globalKey, Key transKey, Key localKey)
80  : Base(globalKey, transKey, localKey, Point().dim(), mu) {}
81 
86  ReferenceFrameFactor(Key globalKey, Key transKey, Key localKey, double sigma = 1e-2)
87  : Base(noiseModel::Isotropic::Sigma(traits<POINT>::dimension, sigma),
88  globalKey, transKey, localKey) {}
89 
90  ~ReferenceFrameFactor() override{}
91 
93  return boost::static_pointer_cast<NonlinearFactor>(
94  NonlinearFactor::shared_ptr(new This(*this))); }
95 
97  Vector evaluateError(const Point& global, const Transform& trans, const Point& local,
98  boost::optional<Matrix&> Dforeign = boost::none,
99  boost::optional<Matrix&> Dtrans = boost::none,
100  boost::optional<Matrix&> Dlocal = boost::none) const override {
101  Point newlocal = transform_point<Transform,Point>(trans, global, Dtrans, Dforeign);
102  if (Dlocal)
103  *Dlocal = -1* Matrix::Identity(traits<Point>::dimension, traits<Point>::dimension);
104  return traits<Point>::Local(local,newlocal);
105  }
106 
107  void print(const std::string& s="",
108  const gtsam::KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {
109  std::cout << s << ": ReferenceFrameFactor("
110  << "Global: " << keyFormatter(this->key1()) << ","
111  << " Transform: " << keyFormatter(this->key2()) << ","
112  << " Local: " << keyFormatter(this->key3()) << ")\n";
113  this->noiseModel_->print(" noise model");
114  }
115 
116  // access - convenience functions
117  Key global_key() const { return this->key1(); }
118  Key transform_key() const { return this->key2(); }
119  Key local_key() const { return this->key3(); }
120 
121 private:
124  template<class ARCHIVE>
125  void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
126  ar & boost::serialization::make_nvp("NonlinearFactor3",
127  boost::serialization::base_object<Base>(*this));
128  }
129 };
130 
132 template<class T1, class T2>
133 struct traits<ReferenceFrameFactor<T1, T2> > : public Testable<ReferenceFrameFactor<T1, T2> > {};
134 
135 } // \namespace gtsam
friend class boost::serialization::access
P transform_point(const T &trans, const P &global, boost::optional< Matrix & > Dtrans, boost::optional< Matrix & > Dglobal)
size_t dim() const override
void serialize(ARCHIVE &ar, const unsigned int)
noiseModel::Diagonal::shared_ptr model
double mu
static const double sigma
Pose2 T2(M_PI/2.0, Point2(0.0, 2.0))
static const KeyFormatter DefaultKeyFormatter
Definition: Key.h:43
const SharedNoiseModel & noiseModel() const
access to the noise model
Matrix trans(const Matrix &A)
Definition: base/Matrix.h:245
NonlinearFactor::shared_ptr clone() const override
ReferenceFrameFactor(Key globalKey, Key transKey, Key localKey, double sigma=1e-2)
Eigen::VectorXd Vector
Definition: Vector.h:38
void print(const std::string &s="", const gtsam::KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
ReferenceFrameFactor(double mu, Key globalKey, Key transKey, Key localKey)
boost::shared_ptr< This > shared_ptr
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
Array< double, 1, 3 > e(1./3., 0.5, 2.)
RealScalar s
NoiseModelFactor3< POINT, TRANSFORM, POINT > Base
boost::shared_ptr< Base > shared_ptr
Definition: NoiseModel.h:56
traits
Definition: chartTesting.h:28
A small structure to hold a non zero as a triplet (i,j,value).
Definition: SparseUtil.h:154
ReferenceFrameFactor< POINT, TRANSFORM > This
SharedNoiseModel noiseModel_
ReferenceFrameFactor(Key globalKey, Key transKey, Key localKey, const noiseModel::Base::shared_ptr &model)
Non-linear factor base classes.
Pose2 T1(M_PI/4.0, Point2(sqrt(0.5), sqrt(0.5)))
Vector evaluateError(const Point &global, const Transform &trans, const Point &local, boost::optional< Matrix & > Dforeign=boost::none, boost::optional< Matrix & > Dtrans=boost::none, boost::optional< Matrix & > Dlocal=boost::none) const override
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:61


gtsam
Author(s):
autogenerated on Sat May 8 2021 02:43:51