gtsam  4.1.0
gtsam
OrientedPlane3.h
1 /* ----------------------------------------------------------------------------
2 
3  * Atlanta, Georgia 30332-0415
4  * All Rights Reserved
5  * GTSAM Copyright 2010, Georgia Tech Research Corporation,
6  * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
7 
8  * See LICENSE for the license information
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/Rot3.h>
24 #include <gtsam/geometry/Unit3.h>
25 #include <gtsam/geometry/Pose3.h>
26 
27 namespace gtsam {
28 
35 class GTSAM_EXPORT OrientedPlane3 {
36 
37 private:
38 
39  Unit3 n_;
40  double d_;
41 
42 public:
43  enum {
44  dimension = 3
45  };
46 
49 
52  n_(), d_(0.0) {
53  }
54 
56  OrientedPlane3(const Unit3& s, double d) :
57  n_(s), d_(d) {
58  }
59 
61  OrientedPlane3(const Vector4& vec) :
62  n_(vec(0), vec(1), vec(2)), d_(vec(3)) {
63  }
64 
66  OrientedPlane3(double a, double b, double c, double d) {
67  Point3 p(a, b, c);
68  n_ = Unit3(p);
69  d_ = d;
70  }
71 
75 
77  void print(const std::string& s = std::string()) const;
78 
80  bool equals(const OrientedPlane3& s, double tol = 1e-9) const {
81  return (n_.equals(s.n_, tol) && (std::abs(d_ - s.d_) < tol));
82  }
83 
85 
92  OrientedPlane3 transform(const Pose3& xr,
93  OptionalJacobian<3, 3> Hp = boost::none,
94  OptionalJacobian<3, 6> Hr = boost::none) const;
95 
106  const Pose3& xr, OptionalJacobian<3, 6> Hr = boost::none,
107  OptionalJacobian<3, 3> Hp = boost::none) {
108  return plane.transform(xr, Hp, Hr);
109  }
110 
115  Vector3 error(const OrientedPlane3& plane) const;
116 
123  Vector3 errorVector(const OrientedPlane3& other, OptionalJacobian<3, 3> H1 = boost::none, //
124  OptionalJacobian<3, 3> H2 = boost::none) const;
125 
127  inline static size_t Dim() {
128  return 3;
129  }
130 
132  inline size_t dim() const {
133  return 3;
134  }
135 
137  OrientedPlane3 retract(const Vector3& v, OptionalJacobian<3,3> H = boost::none) const;
138 
140  Vector3 localCoordinates(const OrientedPlane3& s) const;
141 
143  inline Vector4 planeCoefficients() const {
144  Vector3 unit_vec = n_.unitVector();
145  return Vector4(unit_vec[0], unit_vec[1], unit_vec[2], d_);
146  }
147 
149  inline Unit3 normal() const {
150  return n_;
151  }
152 
154  inline double distance() const {
155  return d_;
156  }
157 
159 };
160 
161 template<> struct traits<OrientedPlane3> : public internal::Manifold<
162 OrientedPlane3> {
163 };
164 
165 template<> struct traits<const OrientedPlane3> : public internal::Manifold<
166 OrientedPlane3> {
167 };
168 
169 } // namespace gtsam
170 
gtsam::OrientedPlane3::OrientedPlane3
OrientedPlane3(double a, double b, double c, double d)
Construct from four numbers of plane coeffcients (a, b, c, d)
Definition: OrientedPlane3.h:66
gtsam::OrientedPlane3::equals
bool equals(const OrientedPlane3 &s, double tol=1e-9) const
The equals function with tolerance.
Definition: OrientedPlane3.h:80
gtsam::OptionalJacobian
OptionalJacobian is an Eigen::Ref like class that can take be constructed using either a fixed size o...
Definition: OptionalJacobian.h:39
gtsam::traits
A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
Definition: concepts.h:30
gtsam::OrientedPlane3::normal
Unit3 normal() const
Return the normal.
Definition: OrientedPlane3.h:149
gtsam::OrientedPlane3::planeCoefficients
Vector4 planeCoefficients() const
Returns the plane coefficients.
Definition: OrientedPlane3.h:143
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::OrientedPlane3::OrientedPlane3
OrientedPlane3(const Vector4 &vec)
Construct from a vector of plane coefficients.
Definition: OrientedPlane3.h:61
Pose3.h
3D Pose
gtsam
Global functions in a separate testing namespace.
Definition: chartTesting.h:28
gtsam::print
void print(const Matrix &A, const string &s, ostream &stream)
print without optional string, must specify cout yourself
Definition: Matrix.cpp:155
gtsam::OrientedPlane3::OrientedPlane3
OrientedPlane3()
Default constructor.
Definition: OrientedPlane3.h:51
gtsam::internal::Manifold
Both ManifoldTraits and Testable.
Definition: Manifold.h:120
gtsam::Unit3
Represents a 3D point on a unit sphere.
Definition: Unit3.h:42
gtsam::Pose3
Definition: Pose3.h:37
gtsam::OrientedPlane3::OrientedPlane3
OrientedPlane3(const Unit3 &s, double d)
Construct from a Unit3 and a distance.
Definition: OrientedPlane3.h:56
gtsam::Unit3::equals
bool equals(const Unit3 &s, double tol=1e-9) const
The equals function with tolerance.
Definition: Unit3.h:121
gtsam::Point3
Vector3 Point3
As of GTSAM 4, in order to make GTSAM more lean, it is now possible to just typedef Point3 to Vector3...
Definition: Point3.h:35
gtsam::OrientedPlane3::transform
OrientedPlane3 transform(const Pose3 &xr, OptionalJacobian< 3, 3 > Hp=boost::none, OptionalJacobian< 3, 6 > Hr=boost::none) const
Transforms a plane to the specified pose.
Definition: OrientedPlane3.cpp:35
gtsam::Unit3::unitVector
GTSAM_EXPORT Vector3 unitVector(OptionalJacobian< 3, 2 > H=boost::none) const
Return unit-norm Vector.
Definition: Unit3.cpp:143
gtsam::OrientedPlane3::Dim
static size_t Dim()
Dimensionality of tangent space = 3 DOF.
Definition: OrientedPlane3.h:127
gtsam::OrientedPlane3::distance
double distance() const
Return the perpendicular distance to the origin.
Definition: OrientedPlane3.h:154
gtsam::OrientedPlane3
Represents an infinite plane in 3D, which is composed of a planar normal and its perpendicular distan...
Definition: OrientedPlane3.h:35
gtsam::OrientedPlane3::dim
size_t dim() const
Dimensionality of tangent space = 3 DOF.
Definition: OrientedPlane3.h:132
Rot3.h
3D rotation represented as a rotation matrix or quaternion