gtsam 4.2.0
gtsam
OrientedPlane3Factor.h
1/*
2 * @file OrientedPlane3Factor.cpp
3 * @brief OrientedPlane3 Factor class
4 * @author Alex Trevor
5 * @date December 22, 2013
6 */
7
8#pragma once
9
10#include <gtsam/geometry/OrientedPlane3.h>
12
13namespace gtsam {
14
18class GTSAM_EXPORT OrientedPlane3Factor: public NoiseModelFactorN<Pose3, OrientedPlane3> {
19 protected:
20 OrientedPlane3 measured_p_;
22
23 public:
26 }
27 ~OrientedPlane3Factor() override {}
28
36 OrientedPlane3Factor(const Vector4& z, const SharedGaussian& noiseModel,
37 Key poseKey, Key landmarkKey)
38 : Base(noiseModel, poseKey, landmarkKey), measured_p_(z) {}
39
41 void print(const std::string& s = "OrientedPlane3Factor",
42 const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override;
43
45 Vector evaluateError(
46 const Pose3& pose, const OrientedPlane3& plane,
47 boost::optional<Matrix&> H1 = boost::none,
48 boost::optional<Matrix&> H2 = boost::none) const override;
49};
50
51// TODO: Convert this factor to dimension two, three dimensions is redundant for direction prior
52class GTSAM_EXPORT OrientedPlane3DirectionPrior : public NoiseModelFactorN<OrientedPlane3> {
53 protected:
54 OrientedPlane3 measured_p_;
56
57 public:
61 }
62
64 OrientedPlane3DirectionPrior(Key key, const Vector4& z,
65 const SharedGaussian& noiseModel)
66 : Base(noiseModel, key), measured_p_(z) {}
67
69 void print(const std::string& s = "OrientedPlane3DirectionPrior",
70 const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override;
71
73 bool equals(const NonlinearFactor& expected, double tol = 1e-9) const override;
74
75 Vector evaluateError(const OrientedPlane3& plane,
76 boost::optional<Matrix&> H = boost::none) const override;
77};
78
79} // gtsam
80
Non-linear factor base classes.
Global functions in a separate testing namespace.
Definition: chartTesting.h:28
void print(const Matrix &A, const string &s, ostream &stream)
print without optional string, must specify cout yourself
Definition: Matrix.cpp:156
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:100
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
Template to create a binary predicate.
Definition: Testable.h:111
Represents an infinite plane in 3D, which is composed of a planar normal and its perpendicular distan...
Definition: OrientedPlane3.h:36
A 3D pose (R,t) : (Rot3,Point3)
Definition: Pose3.h:37
Definition: Factor.h:68
Nonlinear factor base class.
Definition: NonlinearFactor.h:42
A convenient base class for creating your own NoiseModelFactor with n variables.
Definition: NonlinearFactor.h:400
Factor to measure a planar landmark from a given pose.
Definition: OrientedPlane3Factor.h:18
OrientedPlane3Factor(const Vector4 &z, const SharedGaussian &noiseModel, Key poseKey, Key landmarkKey)
Constructor with measured plane (a,b,c,d) coefficients.
Definition: OrientedPlane3Factor.h:36
OrientedPlane3Factor()
Constructor.
Definition: OrientedPlane3Factor.h:25
Definition: OrientedPlane3Factor.h:52
OrientedPlane3DirectionPrior(Key key, const Vector4 &z, const SharedGaussian &noiseModel)
Constructor with measured plane coefficients (a,b,c,d), noise model, landmark symbol.
Definition: OrientedPlane3Factor.h:64
NoiseModelFactorN< OrientedPlane3 > Base
measured plane parameters
Definition: OrientedPlane3Factor.h:55
OrientedPlane3DirectionPrior()
Constructor.
Definition: OrientedPlane3Factor.h:60