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 PosePriorFactor.h
14 * @author Frank Dellaert
15 **/
16#pragma once
17
18#include <gtsam/nonlinear/NonlinearFactor.h>
19#include <gtsam/geometry/concepts.h>
20#include <gtsam/base/Testable.h>
21
22namespace gtsam {
23
24 /**
25 * A class for a soft prior on any Value type
26 * @ingroup slam
27 */
28 template<class POSE>
29 class PosePriorFactor: public NoiseModelFactorN<POSE> {
30
31 private:
32
33 typedef PosePriorFactor<POSE> This;
34 typedef NoiseModelFactorN<POSE> Base;
35
36 POSE prior_; /** The measurement */
37 std::optional<POSE> body_P_sensor_; ///< The pose of the sensor in the body frame
38
39 /** concept check by type */
40 GTSAM_CONCEPT_TESTABLE_TYPE(POSE)
41 GTSAM_CONCEPT_POSE_TYPE(POSE)
42 public:
43
44 // Provide access to the Matrix& version of evaluateError:
45 using Base::evaluateError;
46
47 /// shorthand for a smart pointer to a factor
48 typedef typename std::shared_ptr<PosePriorFactor<POSE> > shared_ptr;
49
50 /** default constructor - only use for serialization */
51 PosePriorFactor() {}
52
53 ~PosePriorFactor() override {}
54
55 /** Constructor */
56 PosePriorFactor(Key key, const POSE& prior, const SharedNoiseModel& model,
57 std::optional<POSE> body_P_sensor = {}) :
58 Base(model, key), prior_(prior), body_P_sensor_(body_P_sensor) {
59 }
60
61 /// @return a deep copy of this factor
62 gtsam::NonlinearFactor::shared_ptr clone() const override {
63 return std::static_pointer_cast<gtsam::NonlinearFactor>(
64 r: gtsam::NonlinearFactor::shared_ptr(new This(*this))); }
65
66 /** implement functions needed for Testable */
67
68 /** print */
69 void print(const std::string& s, const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {
70 std::cout << s << "PriorFactor on " << keyFormatter(this->key()) << "\n";
71 prior_.print(" prior mean: ");
72 if(this->body_P_sensor_)
73 this->body_P_sensor_->print(" sensor pose in body frame: ");
74 this->noiseModel_->print(" noise model: ");
75 }
76
77 /** equals */
78 bool equals(const NonlinearFactor& expected, double tol=1e-9) const override {
79 const This* e = dynamic_cast<const This*> (&expected);
80 return e != nullptr
81 && Base::equals(*e, tol)
82 && this->prior_.equals(e->prior_, tol)
83 && ((!body_P_sensor_ && !e->body_P_sensor_) || (body_P_sensor_ && e->body_P_sensor_ && body_P_sensor_->equals(*e->body_P_sensor_)));
84 }
85
86 /** implement functions needed to derive from Factor */
87
88 /** vector of errors */
89 Vector evaluateError(const POSE& p, OptionalMatrixType H) const override {
90 if(body_P_sensor_) {
91 // manifold equivalent of h(x)-z -> log(z,h(x))
92 return prior_.localCoordinates(p.compose(*body_P_sensor_, H));
93 } else {
94 if(H) (*H) = I_6x6;
95 // manifold equivalent of h(x)-z -> log(z,h(x))
96 return prior_.localCoordinates(p);
97 }
98 }
99
100 const POSE& prior() const { return prior_; }
101
102 private:
103
104#if GTSAM_ENABLE_BOOST_SERIALIZATION
105 /** Serialization function */
106 friend class boost::serialization::access;
107 template<class ARCHIVE>
108 void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
109 ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
110 ar & BOOST_SERIALIZATION_NVP(prior_);
111 ar & BOOST_SERIALIZATION_NVP(body_P_sensor_);
112 }
113#endif
114 };
115
116} /// namespace gtsam
117