Loading...
Searching...
No Matches
OrientedPlane3.h
/* ----------------------------------------------------------------------------
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/*
* @file OrientedPlane3.h
* @date Dec 19, 2013
* @author Alex Trevor
* @author Frank Dellaert
* @author Zhaoyang Lv
* @brief An infinite plane, represented by a normal direction and perpendicular distance
*/
#pragma once
#include <gtsam/geometry/Unit3.h>
#include <gtsam/geometry/Pose3.h>
#include <string>
namespace gtsam {
class GTSAM_EXPORT OrientedPlane3 {
private:
Unit3 n_;
double d_;
public:
enum {
dimension = 3
};
OrientedPlane3() :
n_(), d_(0.0) {
}
OrientedPlane3(const Unit3& n, double d) :
n_(n), d_(d) {
}
explicit OrientedPlane3(const Vector4& vec)
: n_(vec(0), vec(1), vec(2)), d_(vec(3)) {}
OrientedPlane3(double a, double b, double c, double d) {
n_ = Unit3(a, b, c);
d_ = d;
}
void print(const std::string& s = std::string()) const;
bool equals(const OrientedPlane3& s, double tol = 1e-9) const {
return (n_.equals(s.n_, tol) && (std::abs(d_ - s.d_) < tol));
}
OrientedPlane3 transform(const Pose3& xr,
OptionalJacobian<3, 3> Hp = boost::none,
OptionalJacobian<3, 6> Hr = boost::none) const;
Vector3 errorVector(const OrientedPlane3& other,
OptionalJacobian<3, 3> H1 = boost::none,
OptionalJacobian<3, 3> H2 = boost::none) const;
inline static size_t Dim() {
return 3;
}
inline size_t dim() const {
return 3;
}
OrientedPlane3 retract(const Vector3& v,
OptionalJacobian<3, 3> H = boost::none) const;
Vector3 localCoordinates(const OrientedPlane3& s) const;
inline Vector4 planeCoefficients() const {
Vector3 unit_vec = n_.unitVector();
return Vector4(unit_vec[0], unit_vec[1], unit_vec[2], d_);
}
inline Unit3 normal(OptionalJacobian<2, 3> H = boost::none) const {
if (H) *H << I_2x2, Z_2x1;
return n_;
}
inline double distance(OptionalJacobian<1, 3> H = boost::none) const {
if (H) *H << 0,0,1;
return d_;
}
};
template<> struct traits<OrientedPlane3> : public internal::Manifold<
OrientedPlane3> {
};
template<> struct traits<const OrientedPlane3> : public internal::Manifold<
OrientedPlane3> {
};
} // namespace gtsam
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:156
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::internal::Manifold
Both ManifoldTraits and Testable.
Definition Manifold.h:120
gtsam::OptionalJacobian
OptionalJacobian is an Eigen::Ref like class that can take be constructed using either a fixed size o...
Definition OptionalJacobian.h:41
gtsam::OrientedPlane3
Represents an infinite plane in 3D, which is composed of a planar normal and its perpendicular distan...
Definition OrientedPlane3.h:36
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:64
gtsam::OrientedPlane3::planeCoefficients
Vector4 planeCoefficients() const
Returns the plane coefficients.
Definition OrientedPlane3.h:122
gtsam::OrientedPlane3::OrientedPlane3
OrientedPlane3(const Unit3 &n, double d)
Construct from a Unit3 and a distance.
Definition OrientedPlane3.h:55
gtsam::OrientedPlane3::dim
size_t dim() const
Dimensionality of tangent space = 3 DOF.
Definition OrientedPlane3.h:110
gtsam::OrientedPlane3::equals
bool equals(const OrientedPlane3 &s, double tol=1e-9) const
The equals function with tolerance.
Definition OrientedPlane3.h:77
gtsam::OrientedPlane3::OrientedPlane3
OrientedPlane3(const Vector4 &vec)
Construct from a vector of plane coefficients.
Definition OrientedPlane3.h:60
gtsam::OrientedPlane3::Dim
static size_t Dim()
Dimensionality of tangent space = 3 DOF.
Definition OrientedPlane3.h:105
gtsam::OrientedPlane3::OrientedPlane3
OrientedPlane3()
Default constructor.
Definition OrientedPlane3.h:50
gtsam::OrientedPlane3::distance
double distance(OptionalJacobian< 1, 3 > H=boost::none) const
Return the perpendicular distance to the origin.
Definition OrientedPlane3.h:134
gtsam::OrientedPlane3::normal
Unit3 normal(OptionalJacobian< 2, 3 > H=boost::none) const
Return the normal.
Definition OrientedPlane3.h:128
gtsam::Pose3
A 3D pose (R,t) : (Rot3,Point3).
Definition Pose3.h:37
gtsam::Unit3
Represents a 3D point on a unit sphere.
Definition Unit3.h:43