gtsam  4.1.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 
13 namespace gtsam {
14 
18 class OrientedPlane3Factor: public NoiseModelFactor2<Pose3, OrientedPlane3> {
19 
20 protected:
21  Key poseKey_;
22  Key landmarkKey_;
23  Vector measured_coeffs_;
24  OrientedPlane3 measured_p_;
25 
27 
28 public:
29 
32  }
33  virtual ~OrientedPlane3Factor() {}
34 
36  OrientedPlane3Factor(const Vector&z, const SharedGaussian& noiseModel,
37  const Key& pose, const Key& landmark) :
38  Base(noiseModel, pose, landmark), poseKey_(pose), landmarkKey_(landmark), measured_coeffs_(
39  z) {
40  measured_p_ = OrientedPlane3(Unit3(z(0), z(1), z(2)), z(3));
41  }
42 
44  void print(const std::string& s = "OrientedPlane3Factor",
45  const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override;
46 
48  Vector evaluateError(const Pose3& pose, const OrientedPlane3& plane,
49  boost::optional<Matrix&> H1 = boost::none, boost::optional<Matrix&> H2 =
50  boost::none) const override {
51  OrientedPlane3 predicted_plane = OrientedPlane3::Transform(plane, pose, H1,
52  H2);
53  Vector err(3);
54  err << predicted_plane.error(measured_p_);
55  return (err);
56  }
57  ;
58 };
59 
60 // TODO: Convert this factor to dimension two, three dimensions is redundant for direction prior
61 class OrientedPlane3DirectionPrior: public NoiseModelFactor1<OrientedPlane3> {
62 protected:
63  OrientedPlane3 measured_p_;
66 public:
67 
71  }
72 
74  OrientedPlane3DirectionPrior(Key key, const Vector&z,
75  const SharedGaussian& noiseModel) :
76  Base(noiseModel, key), landmarkKey_(key) {
77  measured_p_ = OrientedPlane3(Unit3(z(0), z(1), z(2)), z(3));
78  }
79 
81  void print(const std::string& s = "OrientedPlane3DirectionPrior",
82  const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override;
83 
85  bool equals(const NonlinearFactor& expected, double tol = 1e-9) const override;
86 
87  Vector evaluateError(const OrientedPlane3& plane,
88  boost::optional<Matrix&> H = boost::none) const override;
89 };
90 
91 } // gtsam
92 
gtsam::OrientedPlane3::error
Vector3 error(const OrientedPlane3 &plane) const
Computes the error between two planes.
Definition: OrientedPlane3.cpp:61
gtsam::OrientedPlane3DirectionPrior::landmarkKey_
Key landmarkKey_
measured plane parameters
Definition: OrientedPlane3Factor.h:64
gtsam::OrientedPlane3DirectionPrior::equals
bool equals(const NonlinearFactor &expected, double tol=1e-9) const override
equals
Definition: OrientedPlane3Factor.cpp:31
gtsam::OrientedPlane3Factor::OrientedPlane3Factor
OrientedPlane3Factor(const Vector &z, const SharedGaussian &noiseModel, const Key &pose, const Key &landmark)
Constructor with measured plane coefficients (a,b,c,d), noise model, pose symbol.
Definition: OrientedPlane3Factor.h:36
gtsam::OrientedPlane3DirectionPrior::OrientedPlane3DirectionPrior
OrientedPlane3DirectionPrior(Key key, const Vector &z, const SharedGaussian &noiseModel)
Constructor with measured plane coefficients (a,b,c,d), noise model, landmark symbol.
Definition: OrientedPlane3Factor.h:74
gtsam::NoiseModelFactor1
A convenient base class for creating your own NoiseModelFactor with 1 variable.
Definition: NonlinearFactor.h:271
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:61
gtsam::NoiseModelFactor::noiseModel
const SharedNoiseModel & noiseModel() const
access to the noise model
Definition: NonlinearFactor.h:203
gtsam::OrientedPlane3::Transform
static OrientedPlane3 Transform(const OrientedPlane3 &plane, const Pose3 &xr, OptionalJacobian< 3, 6 > Hr=boost::none, OptionalJacobian< 3, 3 > Hp=boost::none)
Definition: OrientedPlane3.h:105
gtsam::OrientedPlane3Factor::evaluateError
Vector evaluateError(const Pose3 &pose, const OrientedPlane3 &plane, boost::optional< Matrix & > H1=boost::none, boost::optional< Matrix & > H2=boost::none) const override
evaluateError
Definition: OrientedPlane3Factor.h:48
gtsam
Global functions in a separate testing namespace.
Definition: chartTesting.h:28
gtsam::OrientedPlane3Factor::OrientedPlane3Factor
OrientedPlane3Factor()
Constructor.
Definition: OrientedPlane3Factor.h:31
gtsam::NonlinearFactor
Nonlinear factor base class.
Definition: NonlinearFactor.h:43
NonlinearFactor.h
Non-linear factor base classes.
gtsam::OrientedPlane3DirectionPrior::OrientedPlane3DirectionPrior
OrientedPlane3DirectionPrior()
Constructor.
Definition: OrientedPlane3Factor.h:70
gtsam::Unit3
Represents a 3D point on a unit sphere.
Definition: Unit3.h:42
gtsam::Pose3
Definition: Pose3.h:37
gtsam::OrientedPlane3DirectionPrior
Definition: OrientedPlane3Factor.h:61
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::OrientedPlane3DirectionPrior::print
void print(const std::string &s="OrientedPlane3DirectionPrior", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
print
Definition: OrientedPlane3Factor.cpp:23
gtsam::OrientedPlane3DirectionPrior::evaluateError
Vector evaluateError(const OrientedPlane3 &plane, boost::optional< Matrix & > H=boost::none) const override
Override this method to finish implementing a unary factor.
Definition: OrientedPlane3Factor.cpp:40
gtsam::OrientedPlane3Factor::print
void print(const std::string &s="OrientedPlane3Factor", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
print
Definition: OrientedPlane3Factor.cpp:15
gtsam::OrientedPlane3Factor
Factor to measure a planar landmark from a given pose.
Definition: OrientedPlane3Factor.h:18
gtsam::OrientedPlane3
Represents an infinite plane in 3D, which is composed of a planar normal and its perpendicular distan...
Definition: OrientedPlane3.h:35