gtsam  4.1.0
gtsam
PoseToPointFactor.h
1 
7 #pragma once
8 
10 #include <gtsam/geometry/Pose3.h>
12 #include <ostream>
13 
14 namespace gtsam {
15 
20 class PoseToPointFactor : public NoiseModelFactor2<Pose3, Point3> {
21  private:
22  typedef PoseToPointFactor This;
24 
25  Point3 measured_;
27  public:
28  // shorthand for a smart pointer to a factor
29  typedef boost::shared_ptr<PoseToPointFactor> shared_ptr;
30 
33 
36  const SharedNoiseModel& model)
37  : Base(model, key1, key2), measured_(measured) {}
38 
39  virtual ~PoseToPointFactor() {}
40 
44  virtual void print(const std::string& s, const KeyFormatter& keyFormatter =
45  DefaultKeyFormatter) const {
46  std::cout << s << "PoseToPointFactor(" << keyFormatter(this->key1()) << ","
47  << keyFormatter(this->key2()) << ")\n"
48  << " measured: " << measured_.transpose() << std::endl;
49  this->noiseModel_->print(" noise model: ");
50  }
51 
53  virtual bool equals(const NonlinearFactor& expected,
54  double tol = 1e-9) const {
55  const This* e = dynamic_cast<const This*>(&expected);
56  return e != nullptr && Base::equals(*e, tol) &&
57  traits<Point3>::Equals(this->measured_, e->measured_, tol);
58  }
59 
69  Vector evaluateError(const Pose3& wTwi, const Point3& wPwp,
70  boost::optional<Matrix&> H1 = boost::none,
71  boost::optional<Matrix&> H2 = boost::none) const {
72  return wTwi.transformTo(wPwp, H1, H2) - measured_;
73  }
74 
76  const Point3& measured() const { return measured_; }
77 
78  private:
81  template <class ARCHIVE>
82  void serialize(ARCHIVE& ar, const unsigned int /*version*/) {
83  ar& boost::serialization::make_nvp(
84  "NoiseModelFactor2", boost::serialization::base_object<Base>(*this));
85  ar& BOOST_SERIALIZATION_NVP(measured_);
86  }
87 
88 }; // \class PoseToPointFactor
89 
90 } // namespace gtsam
gtsam::PoseToPointFactor::shared_ptr
boost::shared_ptr< PoseToPointFactor > shared_ptr
the point measurement in local coordinates
Definition: PoseToPointFactor.h:29
gtsam::PoseToPointFactor::print
virtual void print(const std::string &s, const KeyFormatter &keyFormatter=DefaultKeyFormatter) const
implement functions needed for Testable
Definition: PoseToPointFactor.h:44
gtsam::PoseToPointFactor::PoseToPointFactor
PoseToPointFactor(Key key1, Key key2, const Point3 &measured, const SharedNoiseModel &model)
Constructor.
Definition: PoseToPointFactor.h:35
gtsam::Pose3::transformTo
Point3 transformTo(const Point3 &point, OptionalJacobian< 3, 6 > Hself=boost::none, OptionalJacobian< 3, 3 > Hpoint=boost::none) const
takes point in world coordinates and transforms it to Pose coordinates
Definition: Pose3.cpp:314
gtsam::PoseToPointFactor::access
friend class boost::serialization::access
Serialization function.
Definition: PoseToPointFactor.h:80
gtsam::SharedNoiseModel
noiseModel::Base::shared_ptr SharedNoiseModel
Note, deliberately not in noiseModel namespace.
Definition: NoiseModel.h:734
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::NoiseModelFactor2< Pose3, Point3 >::key1
Key key1() const
methods to retrieve both keys
Definition: NonlinearFactor.h:387
Pose3.h
3D Pose
gtsam::NoiseModelFactor::equals
bool equals(const NonlinearFactor &f, double tol=1e-9) const override
Check if two factors are equal.
Definition: NonlinearFactor.cpp:71
gtsam
Global functions in a separate testing namespace.
Definition: chartTesting.h:28
gtsam::PoseToPointFactor::measured
const Point3 & measured() const
return the measured
Definition: PoseToPointFactor.h:76
gtsam::NonlinearFactor
Nonlinear factor base class.
Definition: NonlinearFactor.h:43
NonlinearFactor.h
Non-linear factor base classes.
Point3.h
3D Point
gtsam::PoseToPointFactor::PoseToPointFactor
PoseToPointFactor()
default constructor - only use for serialization
Definition: PoseToPointFactor.h:32
gtsam::PoseToPointFactor::equals
virtual bool equals(const NonlinearFactor &expected, double tol=1e-9) const
equals
Definition: PoseToPointFactor.h:53
gtsam::Pose3
Definition: Pose3.h:37
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::NoiseModelFactor2
A convenient base class for creating your own NoiseModelFactor with 2 variables.
Definition: NonlinearFactor.h:355
gtsam::Point3
Vector3 Point3
As of GTSAM 4, in order to make GTSAM more lean, it is now possible to just typedef Point3 to Vector3...
Definition: Point3.h:35
gtsam::PoseToPointFactor::evaluateError
Vector evaluateError(const Pose3 &wTwi, const Point3 &wPwp, boost::optional< Matrix & > H1=boost::none, boost::optional< Matrix & > H2=boost::none) const
implement functions needed to derive from Factor
Definition: PoseToPointFactor.h:69
gtsam::PoseToPointFactor
Definition: PoseToPointFactor.h:20