Represents an infinite plane in 3D, which is composed of a planar normal and its perpendicular distance to the origin. Currently it provides a transform of the plane, and a norm 1 differencing of two planes. Refer to Trevor12iros for more math details. More...
#include <OrientedPlane3.h>
Public Types | |
enum | { dimension = 3 } |
Public Member Functions | |
size_t | dim () const |
Dimensionality of tangent space = 3 DOF. More... | |
double | distance (OptionalJacobian< 1, 3 > H={}) const |
Return the perpendicular distance to the origin. More... | |
Vector3 | errorVector (const OrientedPlane3 &other, OptionalJacobian< 3, 3 > H1={}, OptionalJacobian< 3, 3 > H2={}) const |
Vector3 | localCoordinates (const OrientedPlane3 &s) const |
The local coordinates function. More... | |
Unit3 | normal (OptionalJacobian< 2, 3 > H={}) const |
Return the normal. More... | |
Vector4 | planeCoefficients () const |
Returns the plane coefficients. More... | |
OrientedPlane3 | retract (const Vector3 &v, OptionalJacobian< 3, 3 > H={}) const |
The retract function. More... | |
OrientedPlane3 | transform (const Pose3 &xr, OptionalJacobian< 3, 3 > Hp={}, OptionalJacobian< 3, 6 > Hr={}) const |
Constructors | |
OrientedPlane3 () | |
Default constructor. More... | |
OrientedPlane3 (const Unit3 &n, double d) | |
Construct from a Unit3 and a distance. More... | |
OrientedPlane3 (const Vector4 &vec) | |
Construct from a vector of plane coefficients. More... | |
OrientedPlane3 (double a, double b, double c, double d) | |
Construct from four numbers of plane coeffcients (a, b, c, d) More... | |
Testable | |
void | print (const std::string &s=std::string()) const |
The print function. More... | |
bool | equals (const OrientedPlane3 &s, double tol=1e-9) const |
The equals function with tolerance. More... | |
Static Public Member Functions | |
static size_t | Dim () |
Dimensionality of tangent space = 3 DOF. More... | |
Private Attributes | |
double | d_ |
The perpendicular distance to this plane. More... | |
Unit3 | n_ |
The direction of the planar normal. More... | |
Represents an infinite plane in 3D, which is composed of a planar normal and its perpendicular distance to the origin. Currently it provides a transform of the plane, and a norm 1 differencing of two planes. Refer to Trevor12iros for more math details.
Definition at line 36 of file OrientedPlane3.h.
anonymous enum |
Enumerator | |
---|---|
dimension |
Definition at line 42 of file OrientedPlane3.h.
|
inline |
Default constructor.
Definition at line 50 of file OrientedPlane3.h.
|
inline |
Construct from a Unit3 and a distance.
Definition at line 55 of file OrientedPlane3.h.
|
inlineexplicit |
Construct from a vector of plane coefficients.
Definition at line 60 of file OrientedPlane3.h.
|
inline |
Construct from four numbers of plane coeffcients (a, b, c, d)
Definition at line 64 of file OrientedPlane3.h.
|
inlinestatic |
Dimensionality of tangent space = 3 DOF.
Definition at line 105 of file OrientedPlane3.h.
|
inline |
Dimensionality of tangent space = 3 DOF.
Definition at line 110 of file OrientedPlane3.h.
|
inline |
Return the perpendicular distance to the origin.
Definition at line 134 of file OrientedPlane3.h.
|
inline |
The equals function with tolerance.
Definition at line 77 of file OrientedPlane3.h.
Vector3 gtsam::OrientedPlane3::errorVector | ( | const OrientedPlane3 & | other, |
OptionalJacobian< 3, 3 > | H1 = {} , |
||
OptionalJacobian< 3, 3 > | H2 = {} |
||
) | const |
Computes the error between the two planes, with derivatives. This uses Unit3::errorVector, as opposed to the other .error() in this class, which uses Unit3::localCoordinates. This one has correct derivatives. NOTE(hayk): The derivatives are zero when normals are exactly orthogonal.
other | the other plane |
Definition at line 61 of file OrientedPlane3.cpp.
Vector3 gtsam::OrientedPlane3::localCoordinates | ( | const OrientedPlane3 & | s | ) | const |
The local coordinates function.
Definition at line 92 of file OrientedPlane3.cpp.
|
inline |
Return the normal.
Definition at line 128 of file OrientedPlane3.h.
|
inline |
Returns the plane coefficients.
Definition at line 122 of file OrientedPlane3.h.
void gtsam::OrientedPlane3::print | ( | const std::string & | s = std::string() | ) | const |
The print function.
Definition at line 29 of file OrientedPlane3.cpp.
OrientedPlane3 gtsam::OrientedPlane3::retract | ( | const Vector3 & | v, |
OptionalJacobian< 3, 3 > | H = {} |
||
) | const |
The retract function.
Definition at line 81 of file OrientedPlane3.cpp.
OrientedPlane3 gtsam::OrientedPlane3::transform | ( | const Pose3 & | xr, |
OptionalJacobian< 3, 3 > | Hp = {} , |
||
OptionalJacobian< 3, 6 > | Hr = {} |
||
) | const |
Transforms a plane to the specified pose
xr | a transformation in current coordiante |
Hp | optional Jacobian wrpt the destination plane |
Hr | optional jacobian wrpt the pose transformation |
Definition at line 35 of file OrientedPlane3.cpp.
|
private |
The perpendicular distance to this plane.
Definition at line 39 of file OrientedPlane3.h.
|
private |
The direction of the planar normal.
Definition at line 38 of file OrientedPlane3.h.