1/**
2 * @file PoseToPointFactor.h
3 * @brief This factor can be used to model relative position measurements
4 * from a (2D or 3D) pose to a landmark
5 * @author David Wisth
6 * @author Luca Carlone
7 **/
8#pragma once
9
10#include <gtsam/geometry/Point2.h>
11#include <gtsam/geometry/Pose2.h>
12#include <gtsam/geometry/Point3.h>
13#include <gtsam/geometry/Pose3.h>
14#include <gtsam/nonlinear/NonlinearFactor.h>
15#include <ostream>
16
17namespace gtsam {
18
19/**
20 * A class for a measurement between a pose and a point.
21 * @ingroup slam
22 */
23template<typename POSE = Pose3, typename POINT = Point3>
24class PoseToPointFactor : public NoiseModelFactorN<POSE, POINT> {
25 private:
26 typedef PoseToPointFactor This;
27 typedef NoiseModelFactorN<POSE, POINT> Base;
28
29 POINT measured_; /** the point measurement in local coordinates */
30
31 public:
32
33 // Provide access to the Matrix& version of evaluateError:
34 using Base::evaluateError;
35
36 // shorthand for a smart pointer to a factor
37 typedef std::shared_ptr<PoseToPointFactor> shared_ptr;
38
39 /** default constructor - only use for serialization */
40 PoseToPointFactor() {}
41
42 /** Constructor */
43 PoseToPointFactor(Key key1, Key key2, const POINT& measured,
44 const SharedNoiseModel& model)
45 : Base(model, key1, key2), measured_(measured) {}
46
47 virtual ~PoseToPointFactor() {}
48
49 /** implement functions needed for Testable */
50
51 /** print */
52 void print(const std::string& s, const KeyFormatter& keyFormatter =
53 DefaultKeyFormatter) const override {
54 std::cout << s << "PoseToPointFactor("
55 << keyFormatter(this->key1()) << ","
56 << keyFormatter(this->key2()) << ")\n"
57 << " measured: " << measured_.transpose() << std::endl;
58 this->noiseModel_->print(" noise model: ");
59 }
60
61 /** equals */
62 bool equals(const NonlinearFactor& expected,
63 double tol = 1e-9) const override {
64 const This* e = dynamic_cast<const This*>(&expected);
65 return e != nullptr && Base::equals(*e, tol) &&
66 traits<POINT>::Equals(this->measured_, e->measured_, tol);
67 }
68
69 /// @return a deep copy of this factor
70 gtsam::NonlinearFactor::shared_ptr clone() const override {
71 return std::static_pointer_cast<gtsam::NonlinearFactor>(
72 r: gtsam::NonlinearFactor::shared_ptr(new This(*this)));
73 }
74
75 /** implement functions needed to derive from Factor */
76
77 /** vector of errors
78 * @brief Error = w_T_b.inverse()*w_P - measured_
79 * @param w_T_b The pose of the body in world coordinates
80 * @param w_P The estimated point location in world coordinates
81 *
82 * Note: measured_ and the error are in local coordiantes.
83 */
84 Vector evaluateError(
85 const POSE& w_T_b, const POINT& w_P,
86 OptionalMatrixType H1,
87 OptionalMatrixType H2) const override {
88 return w_T_b.transformTo(w_P, H1, H2) - measured_;
89 }
90
91 /** return the measured */
92 const POINT& measured() const { return measured_; }
93
94 private:
95#if GTSAM_ENABLE_BOOST_SERIALIZATION
96 /** Serialization function */
97 friend class boost::serialization::access;
98 template <class ARCHIVE>
99 void serialize(ARCHIVE& ar, const unsigned int /*version*/) {
100 // NoiseModelFactor2 instead of NoiseModelFactorN for backward compatibility
101 ar& boost::serialization::make_nvp(
102 "NoiseModelFactor2",
103 boost::serialization::base_object<Base>(*this));
104 ar& BOOST_SERIALIZATION_NVP(measured_);
105 }
106#endif
107
108}; // \class PoseToPointFactor
109
110} // namespace gtsam
111