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 NoiseModelFactorN<Pose3, Point3> {
29 private:
30 
31  double measured_;
35 
36 public:
37 
38  // Provide access to the Matrix& version of evaluateError:
39  using Base::evaluateError;
40 
41  RelativeElevationFactor() : measured_(0.0) {} /* Default constructor */
42 
44  const SharedNoiseModel& model);
45 
47 
50  return std::static_pointer_cast<gtsam::NonlinearFactor>(
52 
54  Vector evaluateError(const Pose3& pose, const Point3& point,
55  OptionalMatrixType H1, OptionalMatrixType H2) const override;
56 
58  inline double measured() const { return measured_; }
59 
61  bool equals(const NonlinearFactor& expected, double tol=1e-9) const override;
62 
64  void print(const std::string& s="", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override;
65 
66 private:
67 
68 #ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
69 
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("NoiseModelFactor2",
75  boost::serialization::base_object<Base>(*this));
76  ar & BOOST_SERIALIZATION_NVP(measured_);
77  }
78 #endif
79 }; // RelativeElevationFactor
80 
81 
82 } // \namespace gtsam
83 
84 
gtsam::NonlinearFactor::shared_ptr
std::shared_ptr< This > shared_ptr
Definition: NonlinearFactor.h:78
pose
static const Pose3 pose(Rot3(Vector3(1, -1, -1).asDiagonal()), Point3(0, 0, 0.5))
pointKey
const gtsam::Key pointKey
Definition: testRelativeElevationFactor.cpp:25
s
RealScalar s
Definition: level1_cplx_impl.h:126
e
Array< double, 1, 3 > e(1./3., 0.5, 2.)
gtsam::RelativeElevationFactor::This
RelativeElevationFactor This
Definition: RelativeElevationFactor.h:33
measured
Point2 measured(-17, 30)
gtsam::Vector
Eigen::VectorXd Vector
Definition: Vector.h:38
gtsam::RelativeElevationFactor::measured_
double measured_
Definition: RelativeElevationFactor.h:31
poseKey
const gtsam::Key poseKey
Definition: testPoseRotationPrior.cpp:29
gtsam::DefaultKeyFormatter
KeyFormatter DefaultKeyFormatter
Assign default key formatter.
Definition: Key.cpp:30
gtsam::RelativeElevationFactor
Definition: RelativeElevationFactor.h:28
gtsam::RelativeElevationFactor::clone
gtsam::NonlinearFactor::shared_ptr clone() const override
Definition: RelativeElevationFactor.h:49
gtsam_unstable.tests.test_ProjectionFactorRollingShutter.point
point
Definition: test_ProjectionFactorRollingShutter.py:25
gtsam::print
void print(const Matrix &A, const string &s, ostream &stream)
Definition: Matrix.cpp:155
gtsam::KeyFormatter
std::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::Pose3
Definition: Pose3.h:37
gtsam::NoiseModelFactorN
Definition: NonlinearFactor.h:431
cholesky::expected
Matrix expected
Definition: testMatrix.cpp:971
gtsam::RelativeElevationFactor::measured
double measured() const
Definition: RelativeElevationFactor.h:58
gtsam::SharedNoiseModel
noiseModel::Base::shared_ptr SharedNoiseModel
Definition: NoiseModel.h:762
model
noiseModel::Diagonal::shared_ptr model
Definition: doc/Code/Pose2SLAMExample.cpp:7
gtsam::equals
Definition: Testable.h:112
NonlinearFactor.h
Non-linear factor base classes.
gtsam
traits
Definition: SFMdata.h:40
gtsam::RelativeElevationFactor::~RelativeElevationFactor
~RelativeElevationFactor() override
Definition: RelativeElevationFactor.h:46
gtsam::RelativeElevationFactor::RelativeElevationFactor
RelativeElevationFactor()
Definition: RelativeElevationFactor.h:41
gtsam::NonlinearFactor
Definition: NonlinearFactor.h:68
This
#define This
Definition: ActiveSetSolver-inl.h:27
gtsam::tol
const G double tol
Definition: Group.h:79
gtsam::Point3
Vector3 Point3
Definition: Point3.h:38
gtsam::OptionalMatrixType
Matrix * OptionalMatrixType
Definition: NonlinearFactor.h:55
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:97
Pose3.h
3D Pose
gtsam::RelativeElevationFactor::Base
NoiseModelFactorN< Pose3, Point3 > Base
Definition: RelativeElevationFactor.h:34


gtsam
Author(s):
autogenerated on Fri Nov 1 2024 03:34:56