1/**
2 * @file RelativeElevationFactor.h
3 *
4 * @brief Factor representing a known relative altitude in global frame
5 *
6 * @date Aug 17, 2012
7 * @author Alex Cunningham
8 */
9
10#pragma once
11
12#include <gtsam_unstable/dllexport.h>
13#include <gtsam/geometry/Pose3.h>
14#include <gtsam/nonlinear/NonlinearFactor.h>
15
16namespace gtsam {
17
18/**
19 * Binary factor for a relative elevation. Note that this
20 * factor takes into account only elevation, and corrects for orientation.
21 * Unlike a range factor, the relative elevation is signed, and only affects
22 * the Z coordinate. Measurement function h(pose, pt) = h.z() - pt.z()
23 *
24 * Dimension: 1
25 *
26 * TODO: enable use of a Pose3 for the target as well
27 */
28class GTSAM_UNSTABLE_EXPORT RelativeElevationFactor: public NoiseModelFactorN<Pose3, Point3> {
29private:
30
31 double measured_; /** measurement */
32
33 typedef RelativeElevationFactor This;
34 typedef NoiseModelFactorN<Pose3, Point3> Base;
35
36public:
37
38 // Provide access to the Matrix& version of evaluateError:
39 using Base::evaluateError;
40
41 RelativeElevationFactor() : measured_(0.0) {} /* Default constructor */
42
43 RelativeElevationFactor(Key poseKey, Key pointKey, double measured,
44 const SharedNoiseModel& model);
45
46 ~RelativeElevationFactor() override {}
47
48 /// @return a deep copy of this factor
49 gtsam::NonlinearFactor::shared_ptr clone() const override {
50 return std::static_pointer_cast<gtsam::NonlinearFactor>(
51 r: gtsam::NonlinearFactor::shared_ptr(new This(*this))); }
52
53 /** h(x)-z */
54 Vector evaluateError(const Pose3& pose, const Point3& point,
55 OptionalMatrixType H1, OptionalMatrixType H2) const override;
56
57 /** return the measured */
58 inline double measured() const { return measured_; }
59
60 /** equals specialized to this factor */
61 bool equals(const NonlinearFactor& expected, double tol=1e-9) const override;
62
63 /** print contents */
64 void print(const std::string& s="", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override;
65
66private:
67
68#if GTSAM_ENABLE_BOOST_SERIALIZATION
69 /** Serialization function */
70 friend class boost::serialization::access;
71 template<class ARCHIVE>
72 void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
73 // NoiseModelFactor2 instead of NoiseModelFactorN for backward compatibility
74 ar & boost::serialization::make_nvp(n: "NoiseModelFactor2",
75 v&: boost::serialization::base_object<Base>(d&: *this));
76 ar & BOOST_SERIALIZATION_NVP(measured_);
77 }
78#endif
79}; // RelativeElevationFactor
80
81
82} // \namespace gtsam
83
84
85