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  OptionalMatrixType Dtrans,
33  OptionalMatrixType Dglobal) {
34  return trans.transformFrom(global, Dtrans, Dglobal);
35 }
36 
56 template<class POINT, class TRANSFORM>
57 class ReferenceFrameFactor : public NoiseModelFactorN<POINT, TRANSFORM, POINT> {
58 protected:
61 
62 public:
65 
66  // Provide access to the Matrix& version of evaluateError:
67  using Base::evaluateError;
68 
69  typedef POINT Point;
70  typedef TRANSFORM Transform;
71 
75  ReferenceFrameFactor(Key globalKey, Key transKey, Key localKey, const noiseModel::Base::shared_ptr& model)
76  : Base(model,globalKey, transKey, localKey) {}
77 
82  ReferenceFrameFactor(double mu, Key globalKey, Key transKey, Key localKey)
83  : Base(globalKey, transKey, localKey, Point().dim(), mu) {}
84 
89  ReferenceFrameFactor(Key globalKey, Key transKey, Key localKey, double sigma = 1e-2)
90  : Base(noiseModel::Isotropic::Sigma(traits<POINT>::dimension, sigma),
91  globalKey, transKey, localKey) {}
92 
93  ~ReferenceFrameFactor() override{}
94 
96  return std::static_pointer_cast<NonlinearFactor>(
97  NonlinearFactor::shared_ptr(new This(*this))); }
98 
100  Vector evaluateError(const Point& global, const Transform& trans, const Point& local,
101  OptionalMatrixType Dforeign, OptionalMatrixType Dtrans,
102  OptionalMatrixType Dlocal) const override {
103  Point newlocal = transform_point<Transform,Point>(trans, global, Dtrans, Dforeign);
104  if (Dlocal) {
105  *Dlocal = -1* Matrix::Identity(traits<Point>::dimension, traits<Point>::dimension);
106  }
107  return traits<Point>::Local(local,newlocal);
108  }
109 
110  void print(const std::string& s="",
111  const gtsam::KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {
112  std::cout << s << ": ReferenceFrameFactor("
113  << "Global: " << keyFormatter(this->key1()) << ","
114  << " Transform: " << keyFormatter(this->key2()) << ","
115  << " Local: " << keyFormatter(this->key3()) << ")\n";
116  this->noiseModel_->print(" noise model");
117  }
118 
119  // access - convenience functions
120  Key global_key() const { return this->key1(); }
121  Key transform_key() const { return this->key2(); }
122  Key local_key() const { return this->key3(); }
123 
124 private:
125 #ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
126 
127  friend class boost::serialization::access;
128  template<class ARCHIVE>
129  void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
130  ar & boost::serialization::make_nvp("NonlinearFactor3",
131  boost::serialization::base_object<Base>(*this));
132  }
133 #endif
134 };
135 
137 template<class T1, class T2>
138 struct traits<ReferenceFrameFactor<T1, T2> > : public Testable<ReferenceFrameFactor<T1, T2> > {};
139 
140 } // \namespace gtsam
P transform_point(const T &trans, const P &global, OptionalMatrixType Dtrans, OptionalMatrixType Dglobal)
size_t dim() const override
std::string serialize(const T &input)
serializes to a string
noiseModel::Diagonal::shared_ptr model
double mu
virtual Vector evaluateError(const ValueTypes &... x, OptionalMatrixTypeT< ValueTypes >... H) const=0
static const Pose3 T2(Rot3::Rodrigues(0.3, 0.2, 0.1), P2)
static const KeyFormatter DefaultKeyFormatter
Definition: Key.h:43
Matrix trans(const Matrix &A)
Definition: base/Matrix.h:241
NonlinearFactor::shared_ptr clone() const override
ReferenceFrameFactor(Key globalKey, Key transKey, Key localKey, double sigma=1e-2)
const SharedNoiseModel & noiseModel() const
access to the noise model
Matrix * OptionalMatrixType
Eigen::VectorXd Vector
Definition: Vector.h:38
void print(const std::string &s="", const gtsam::KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
std::shared_ptr< Base > shared_ptr
Definition: NoiseModel.h:60
ReferenceFrameFactor(double mu, Key globalKey, Key transKey, Key localKey)
Array< double, 1, 3 > e(1./3., 0.5, 2.)
RealScalar s
Vector evaluateError(const Point &global, const Transform &trans, const Point &local, OptionalMatrixType Dforeign, OptionalMatrixType Dtrans, OptionalMatrixType Dlocal) const override
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
NoiseModelFactorN< POINT, TRANSFORM, POINT > Base
traits
Definition: chartTesting.h:28
A small structure to hold a non zero as a triplet (i,j,value).
Definition: SparseUtil.h:162
ReferenceFrameFactor< POINT, TRANSFORM > This
SharedNoiseModel noiseModel_
ReferenceFrameFactor(Key globalKey, Key transKey, Key localKey, const noiseModel::Base::shared_ptr &model)
Non-linear factor base classes.
static const Similarity3 T1(R, Point3(3.5, -8.2, 4.2), 1)
std::shared_ptr< This > shared_ptr
static const double sigma
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:102


gtsam
Author(s):
autogenerated on Tue Jul 4 2023 02:35:32