gtsam  4.1.0
gtsam
RelativeElevationFactor.h
Go to the documentation of this file.
1 
10 #pragma once
11 
12 #include <gtsam_unstable/dllexport.h>
13 #include <gtsam/geometry/Pose3.h>
15 
16 namespace gtsam {
17 
28 class GTSAM_UNSTABLE_EXPORT RelativeElevationFactor: public NoiseModelFactor2<Pose3, Point3> {
29 private:
30 
31  double measured_;
35 
36 public:
37 
38  RelativeElevationFactor() : measured_(0.0) {} /* Default constructor */
39 
40  RelativeElevationFactor(Key poseKey, Key pointKey, double measured,
41  const SharedNoiseModel& model);
42 
43  virtual ~RelativeElevationFactor() {}
44 
46  gtsam::NonlinearFactor::shared_ptr clone() const override {
47  return boost::static_pointer_cast<gtsam::NonlinearFactor>(
48  gtsam::NonlinearFactor::shared_ptr(new This(*this))); }
49 
51  Vector evaluateError(const Pose3& pose, const Point3& point,
52  boost::optional<Matrix&> H1 = boost::none, boost::optional<Matrix&> H2 = boost::none) const override;
53 
55  inline double measured() const { return measured_; }
56 
58  bool equals(const NonlinearFactor& expected, double tol=1e-9) const override;
59 
61  void print(const std::string& s="", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override;
62 
63 private:
64 
66  friend class boost::serialization::access;
67  template<class ARCHIVE>
68  void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
69  ar & boost::serialization::make_nvp("NoiseModelFactor2",
70  boost::serialization::base_object<Base>(*this));
71  ar & BOOST_SERIALIZATION_NVP(measured_);
72  }
73 }; // RelativeElevationFactor
74 
75 
76 } // \namespace gtsam
77 
78 
gtsam::equals
Template to create a binary predicate.
Definition: Testable.h:110
gtsam::RelativeElevationFactor::clone
gtsam::NonlinearFactor::shared_ptr clone() const override
Definition: RelativeElevationFactor.h:46
gtsam::SharedNoiseModel
noiseModel::Base::shared_ptr SharedNoiseModel
Note, deliberately not in noiseModel namespace.
Definition: NoiseModel.h:734
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:61
gtsam::serialize
std::string serialize(const T &input)
serializes to a string
Definition: serialization.h:100
Pose3.h
3D Pose
gtsam
Global functions in a separate testing namespace.
Definition: chartTesting.h:28
gtsam::print
void print(const Matrix &A, const string &s, ostream &stream)
print without optional string, must specify cout yourself
Definition: Matrix.cpp:155
gtsam::NonlinearFactor
Nonlinear factor base class.
Definition: NonlinearFactor.h:43
NonlinearFactor.h
Non-linear factor base classes.
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::RelativeElevationFactor::measured
double measured() const
return the measured
Definition: RelativeElevationFactor.h:55
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::RelativeElevationFactor
Binary factor for a relative elevation.
Definition: RelativeElevationFactor.h:28