LocalOrientedPlane3Factor.h
Go to the documentation of this file.
1 /*
2  * @file LocalOrientedPlane3Factor.h
3  * @brief LocalOrientedPlane3 Factor class
4  * @author David Wisth
5  * @date February 12, 2021
6  */
7 
8 #pragma once
9 
12 #include <gtsam_unstable/dllexport.h>
13 
14 #include <string>
15 
16 namespace gtsam {
17 
37 class GTSAM_UNSTABLE_EXPORT LocalOrientedPlane3Factor
38  : public NoiseModelFactorN<Pose3, Pose3, OrientedPlane3> {
39  protected:
42 public:
43 
44  // Provide access to the Matrix& version of evaluateError:
45  using Base::evaluateError;
46 
49 
51 
63  LocalOrientedPlane3Factor(const Vector4& z, const SharedNoiseModel& noiseModel,
64  Key poseKey, Key anchorPoseKey, Key landmarkKey)
65  : Base(noiseModel, poseKey, anchorPoseKey, landmarkKey), measured_p_(z) {}
66 
68  const SharedNoiseModel& noiseModel,
69  Key poseKey, Key anchorPoseKey, Key landmarkKey)
70  : Base(noiseModel, poseKey, anchorPoseKey, landmarkKey), measured_p_(z) {}
71 
73  void print(const std::string& s = "LocalOrientedPlane3Factor",
74  const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override;
75 
76  /***
77  * Vector of errors
78  * @brief Error = measured_plane_.error(a_plane.transform(inv(wTwa) * wTwi))
79  *
80  * This is the error of the measured and predicted plane in the current
81  * sensor frame, i. The plane is represented in the anchor pose, a.
82  *
83  * @param wTwi The pose of the sensor in world coordinates
84  * @param wTwa The pose of the anchor frame in world coordinates
85  * @param a_plane The estimated plane in anchor frame.
86  *
87  * Note: The optimized plane is represented in anchor frame, a, not the
88  * world frame.
89  */
90  Vector evaluateError(const Pose3& wTwi, const Pose3& wTwa,
91  const OrientedPlane3& a_plane, OptionalMatrixType H1,
92  OptionalMatrixType H2, OptionalMatrixType H3) const override;
93 };
94 
95 } // namespace gtsam
96 
gtsam::LocalOrientedPlane3Factor::LocalOrientedPlane3Factor
LocalOrientedPlane3Factor(const OrientedPlane3 &z, const SharedNoiseModel &noiseModel, Key poseKey, Key anchorPoseKey, Key landmarkKey)
Definition: LocalOrientedPlane3Factor.h:67
s
RealScalar s
Definition: level1_cplx_impl.h:126
gtsam::LocalOrientedPlane3Factor::LocalOrientedPlane3Factor
LocalOrientedPlane3Factor()
Constructor.
Definition: LocalOrientedPlane3Factor.h:48
gtsam::LocalOrientedPlane3Factor::Base
NoiseModelFactorN< Pose3, Pose3, OrientedPlane3 > Base
Definition: LocalOrientedPlane3Factor.h:41
gtsam::Factor
Definition: Factor.h:70
gtsam::Vector
Eigen::VectorXd Vector
Definition: Vector.h:38
gtsam::OrientedPlane3
Represents an infinite plane in 3D, which is composed of a planar normal and its perpendicular distan...
Definition: OrientedPlane3.h:36
poseKey
const gtsam::Key poseKey
Definition: testPoseRotationPrior.cpp:29
OrientedPlane3.h
gtsam::DefaultKeyFormatter
KeyFormatter DefaultKeyFormatter
Assign default key formatter.
Definition: Key.cpp:30
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
pybind_wrapper_test_script.z
z
Definition: pybind_wrapper_test_script.py:61
gtsam::SharedNoiseModel
noiseModel::Base::shared_ptr SharedNoiseModel
Definition: NoiseModel.h:762
gtsam::LocalOrientedPlane3Factor::LocalOrientedPlane3Factor
LocalOrientedPlane3Factor(const Vector4 &z, const SharedNoiseModel &noiseModel, Key poseKey, Key anchorPoseKey, Key landmarkKey)
Definition: LocalOrientedPlane3Factor.h:63
NonlinearFactor.h
Non-linear factor base classes.
gtsam::LocalOrientedPlane3Factor
Definition: LocalOrientedPlane3Factor.h:37
gtsam
traits
Definition: SFMdata.h:40
gtsam::LocalOrientedPlane3Factor::~LocalOrientedPlane3Factor
~LocalOrientedPlane3Factor() override
Definition: LocalOrientedPlane3Factor.h:50
gtsam::OptionalMatrixType
Matrix * OptionalMatrixType
Definition: NonlinearFactor.h:55
gtsam::LocalOrientedPlane3Factor::measured_p_
OrientedPlane3 measured_p_
Definition: LocalOrientedPlane3Factor.h:40
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:97


gtsam
Author(s):
autogenerated on Fri Nov 1 2024 03:33:09