gtsam  4.1.0
gtsam
ReferenceFrameFactor.h
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  virtual ~ReferenceFrameFactor(){}
91 
92  NonlinearFactor::shared_ptr clone() const override {
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
gtsam::ReferenceFrameFactor::ReferenceFrameFactor
ReferenceFrameFactor()
default constructor for serialization only
Definition: ReferenceFrameFactor.h:60
gtsam::NoiseModelFactor3
A convenient base class for creating your own NoiseModelFactor with 3 variables.
Definition: NonlinearFactor.h:430
gtsam::ReferenceFrameFactor::print
void print(const std::string &s="", const gtsam::KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
Print.
Definition: ReferenceFrameFactor.h:107
gtsam::traits
A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
Definition: concepts.h:30
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:61
gtsam::NoiseModelFactor::noiseModel
const SharedNoiseModel & noiseModel() const
access to the noise model
Definition: NonlinearFactor.h:203
gtsam::ReferenceFrameFactor::ReferenceFrameFactor
ReferenceFrameFactor(Key globalKey, Key transKey, Key localKey, double sigma=1e-2)
Simple soft constraint constructor for frame of reference, with equal weighting for each degree of fr...
Definition: ReferenceFrameFactor.h:86
gtsam::NoiseModelFactor3< POINT, TRANSFORM, POINT >::key1
Key key1() const
methods to retrieve keys
Definition: NonlinearFactor.h:464
gtsam::ReferenceFrameFactor::evaluateError
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
Combined cost and derivative function using boost::optional.
Definition: ReferenceFrameFactor.h:97
gtsam
Global functions in a separate testing namespace.
Definition: chartTesting.h:28
gtsam::NoiseModelFactor::dim
size_t dim() const override
get the dimension of the factor (number of rows on linearization)
Definition: NonlinearFactor.h:198
gtsam::ReferenceFrameFactor::clone
NonlinearFactor::shared_ptr clone() const override
Creates a shared_ptr clone of the factor - needs to be specialized to allow for subclasses.
Definition: ReferenceFrameFactor.h:92
gtsam::Testable
A helper that implements the traits interface for GTSAM types.
Definition: Testable.h:150
gtsam::transform_point
P transform_point(const T &trans, const P &global, boost::optional< Matrix & > Dtrans, boost::optional< Matrix & > Dglobal)
Transform function that must be specialized specific domains.
Definition: ReferenceFrameFactor.h:30
NonlinearFactor.h
Non-linear factor base classes.
gtsam::ReferenceFrameFactor::ReferenceFrameFactor
ReferenceFrameFactor(Key globalKey, Key transKey, Key localKey, const noiseModel::Base::shared_ptr &model)
General constructor with arbitrary noise model (constrained or otherwise)
Definition: ReferenceFrameFactor.h:72
gtsam::ReferenceFrameFactor
A constraint between two landmarks in separate maps Templated on: Point : Type of landmark Transform ...
Definition: ReferenceFrameFactor.h:57
gtsam::ReferenceFrameFactor::access
friend class boost::serialization::access
Serialization function.
Definition: ReferenceFrameFactor.h:123
gtsam::trans
Matrix trans(const Matrix &A)
static transpose function, just calls Eigen transpose member function
Definition: Matrix.h:245
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::Factor
This is the base class for all factor types.
Definition: Factor.h:55
gtsam::ReferenceFrameFactor::ReferenceFrameFactor
ReferenceFrameFactor(double mu, Key globalKey, Key transKey, Key localKey)
Construct a hard frame of reference reference constraint with equal mu values for each degree of free...
Definition: ReferenceFrameFactor.h:79