gtsam 4.1.1 gtsam
OrientedPlane3.h
1/* ----------------------------------------------------------------------------
2
3 * Atlanta, Georgia 30332-0415
5 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
7
9
10 * -------------------------------------------------------------------------- */
11
12/*
13 * @file OrientedPlane3.h
14 * @date Dec 19, 2013
15 * @author Alex Trevor
16 * @author Frank Dellaert
17 * @author Zhaoyang Lv
18 * @brief An infinite plane, represented by a normal direction and perpendicular distance
19 */
20
21#pragma once
22
23#include <gtsam/geometry/Unit3.h>
25#include <string>
26
27namespace gtsam {
28
36class GTSAM_EXPORT OrientedPlane3 {
37private:
38 Unit3 n_;
39 double d_;
40
41public:
42 enum {
43 dimension = 3
44 };
45
48
51 n_(), d_(0.0) {
52 }
53
55 OrientedPlane3(const Unit3& n, double d) :
56 n_(n), d_(d) {
57 }
58
60 explicit OrientedPlane3(const Vector4& vec)
61 : n_(vec(0), vec(1), vec(2)), d_(vec(3)) {}
62
64 OrientedPlane3(double a, double b, double c, double d) {
65 n_ = Unit3(a, b, c);
66 d_ = d;
67 }
68
72
74 void print(const std::string& s = std::string()) const;
75
77 bool equals(const OrientedPlane3& s, double tol = 1e-9) const {
78 return (n_.equals(s.n_, tol) && (std::abs(d_ - s.d_) < tol));
79 }
80
82
89 OrientedPlane3 transform(const Pose3& xr,
90 OptionalJacobian<3, 3> Hp = boost::none,
91 OptionalJacobian<3, 6> Hr = boost::none) const;
92
100 Vector3 errorVector(const OrientedPlane3& other,
101 OptionalJacobian<3, 3> H1 = boost::none,
102 OptionalJacobian<3, 3> H2 = boost::none) const;
103
105 inline static size_t Dim() {
106 return 3;
107 }
108
110 inline size_t dim() const {
111 return 3;
112 }
113
115 OrientedPlane3 retract(const Vector3& v,
116 OptionalJacobian<3, 3> H = boost::none) const;
117
119 Vector3 localCoordinates(const OrientedPlane3& s) const;
120
122 inline Vector4 planeCoefficients() const {
123 Vector3 unit_vec = n_.unitVector();
124 return Vector4(unit_vec[0], unit_vec[1], unit_vec[2], d_);
125 }
126
128 inline Unit3 normal() const {
129 return n_;
130 }
131
133 inline double distance() const {
134 return d_;
135 }
136
138};
139
140template<> struct traits<OrientedPlane3> : public internal::Manifold<
141OrientedPlane3> {
142};
143
144template<> struct traits<const OrientedPlane3> : public internal::Manifold<
145OrientedPlane3> {
146};
147
148} // namespace gtsam
149
3D Pose
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:155
A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
Definition: concepts.h:30
Both ManifoldTraits and Testable.
Definition: Manifold.h:120
OptionalJacobian is an Eigen::Ref like class that can take be constructed using either a fixed size o...
Definition: OptionalJacobian.h:39
Represents an infinite plane in 3D, which is composed of a planar normal and its perpendicular distan...
Definition: OrientedPlane3.h:36
OrientedPlane3(double a, double b, double c, double d)
Construct from four numbers of plane coeffcients (a, b, c, d)
Definition: OrientedPlane3.h:64
Unit3 normal() const
Return the normal.
Definition: OrientedPlane3.h:128
Vector4 planeCoefficients() const
Returns the plane coefficients.
Definition: OrientedPlane3.h:122
OrientedPlane3(const Unit3 &n, double d)
Construct from a Unit3 and a distance.
Definition: OrientedPlane3.h:55
size_t dim() const
Dimensionality of tangent space = 3 DOF.
Definition: OrientedPlane3.h:110
bool equals(const OrientedPlane3 &s, double tol=1e-9) const
The equals function with tolerance.
Definition: OrientedPlane3.h:77
OrientedPlane3(const Vector4 &vec)
Construct from a vector of plane coefficients.
Definition: OrientedPlane3.h:60
static size_t Dim()
Dimensionality of tangent space = 3 DOF.
Definition: OrientedPlane3.h:105
OrientedPlane3()
Default constructor.
Definition: OrientedPlane3.h:50
double distance() const
Return the perpendicular distance to the origin.
Definition: OrientedPlane3.h:133
Definition: Pose3.h:37
Represents a 3D point on a unit sphere.
Definition: Unit3.h:42
GTSAM_EXPORT Vector3 unitVector(OptionalJacobian< 3, 2 > H=boost::none) const
Return unit-norm Vector.
Definition: Unit3.cpp:143
bool equals(const Unit3 &s, double tol=1e-9) const
The equals function with tolerance.
Definition: Unit3.h:121