Public Types | Public Member Functions | Static Public Member Functions | Private Attributes | List of all members
gtsam::OrientedPlane3 Class Reference

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...
 

Detailed Description

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.

Member Enumeration Documentation

◆ anonymous enum

anonymous enum
Enumerator
dimension 

Definition at line 42 of file OrientedPlane3.h.

Constructor & Destructor Documentation

◆ OrientedPlane3() [1/4]

gtsam::OrientedPlane3::OrientedPlane3 ( )
inline

Default constructor.

Definition at line 50 of file OrientedPlane3.h.

◆ OrientedPlane3() [2/4]

gtsam::OrientedPlane3::OrientedPlane3 ( const Unit3 n,
double  d 
)
inline

Construct from a Unit3 and a distance.

Definition at line 55 of file OrientedPlane3.h.

◆ OrientedPlane3() [3/4]

gtsam::OrientedPlane3::OrientedPlane3 ( const Vector4 &  vec)
inlineexplicit

Construct from a vector of plane coefficients.

Definition at line 60 of file OrientedPlane3.h.

◆ OrientedPlane3() [4/4]

gtsam::OrientedPlane3::OrientedPlane3 ( double  a,
double  b,
double  c,
double  d 
)
inline

Construct from four numbers of plane coeffcients (a, b, c, d)

Definition at line 64 of file OrientedPlane3.h.

Member Function Documentation

◆ Dim()

static size_t gtsam::OrientedPlane3::Dim ( )
inlinestatic

Dimensionality of tangent space = 3 DOF.

Definition at line 105 of file OrientedPlane3.h.

◆ dim()

size_t gtsam::OrientedPlane3::dim ( ) const
inline

Dimensionality of tangent space = 3 DOF.

Definition at line 110 of file OrientedPlane3.h.

◆ distance()

double gtsam::OrientedPlane3::distance ( OptionalJacobian< 1, 3 >  H = {}) const
inline

Return the perpendicular distance to the origin.

Definition at line 134 of file OrientedPlane3.h.

◆ equals()

bool gtsam::OrientedPlane3::equals ( const OrientedPlane3 s,
double  tol = 1e-9 
) const
inline

The equals function with tolerance.

Definition at line 77 of file OrientedPlane3.h.

◆ errorVector()

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.

Parameters
otherthe other plane

Definition at line 61 of file OrientedPlane3.cpp.

◆ localCoordinates()

Vector3 gtsam::OrientedPlane3::localCoordinates ( const OrientedPlane3 s) const

The local coordinates function.

Definition at line 92 of file OrientedPlane3.cpp.

◆ normal()

Unit3 gtsam::OrientedPlane3::normal ( OptionalJacobian< 2, 3 >  H = {}) const
inline

Return the normal.

Definition at line 128 of file OrientedPlane3.h.

◆ planeCoefficients()

Vector4 gtsam::OrientedPlane3::planeCoefficients ( ) const
inline

Returns the plane coefficients.

Definition at line 122 of file OrientedPlane3.h.

◆ print()

void gtsam::OrientedPlane3::print ( const std::string &  s = std::string()) const

The print function.

Definition at line 29 of file OrientedPlane3.cpp.

◆ retract()

OrientedPlane3 gtsam::OrientedPlane3::retract ( const Vector3 v,
OptionalJacobian< 3, 3 >  H = {} 
) const

The retract function.

Definition at line 81 of file OrientedPlane3.cpp.

◆ transform()

OrientedPlane3 gtsam::OrientedPlane3::transform ( const Pose3 xr,
OptionalJacobian< 3, 3 >  Hp = {},
OptionalJacobian< 3, 6 >  Hr = {} 
) const

Transforms a plane to the specified pose

Parameters
xra transformation in current coordiante
Hpoptional Jacobian wrpt the destination plane
Hroptional jacobian wrpt the pose transformation
Returns
the transformed plane

Definition at line 35 of file OrientedPlane3.cpp.

Member Data Documentation

◆ d_

double gtsam::OrientedPlane3::d_
private

The perpendicular distance to this plane.

Definition at line 39 of file OrientedPlane3.h.

◆ n_

Unit3 gtsam::OrientedPlane3::n_
private

The direction of the planar normal.

Definition at line 38 of file OrientedPlane3.h.


The documentation for this class was generated from the following files:


gtsam
Author(s):
autogenerated on Tue Jul 4 2023 02:47:05