12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

change: add some comments

release/4.3a0
zhaoyang 2015-06-24 16:11:29 -04:00
parent 9404a49d42
commit 6c34ce1e80
1 changed files with 20 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

27
gtsam/geometry/OrientedPlane3.h
View File

@ -27,12 +27,12 @@
namespace gtsam { namespace gtsam {
/// Represents an infinite plane in 3D. /// Represents an infinite plane in 3D.
class OrientedPlane3: public DerivedValue<OrientedPlane3> { class GTSAM_EXPORT OrientedPlane3 {
private: private:
Unit3 n_; /// The direction of the planar normal Unit3 n_; ///< The direction of the planar normal
double d_; /// The perpendicular distance to this plane double d_; ///< The perpendicular distance to this plane
public: public:
enum { enum {
@ -51,17 +51,22 @@ public:
n_(s), d_(d) { n_(s), d_(d) {
} }
/// Construct from a vector of plane coefficients
OrientedPlane3(const Vector4& vec) : OrientedPlane3(const Vector4& vec) :
n_(vec(0), vec(1), vec(2)), d_(vec(3)) { n_(vec(0), vec(1), vec(2)), d_(vec(3)) {
} }
/// Construct from a, b, c, d /// Construct from four numbers of plane coeffcients (a, b, c, d)
OrientedPlane3(double a, double b, double c, double d) { OrientedPlane3(double a, double b, double c, double d) {
Point3 p(a, b, c); Point3 p(a, b, c);
n_ = Unit3(p); n_ = Unit3(p);
d_ = d; d_ = d;
} }
/// @}
/// @name Testable
/// @{
/// The print function /// The print function
void print(const std::string& s = std::string()) const; void print(const std::string& s = std::string()) const;
@ -70,12 +75,20 @@ public:
return (n_.equals(s.n_, tol) && (fabs(d_ - s.d_) < tol)); return (n_.equals(s.n_, tol) && (fabs(d_ - s.d_) < tol));
} }
/// Transforms a plane to the specified pose /** Transforms a plane to the specified pose
* @param The raw plane
* @param A transformation in current coordiante
* @param Hr optional jacobian wrpt incremental Pose
* @param Hp optional Jacobian wrpt the destination plane
*/
static OrientedPlane3 Transform(const gtsam::OrientedPlane3& plane, static OrientedPlane3 Transform(const gtsam::OrientedPlane3& plane,
const gtsam::Pose3& xr, OptionalJacobian<3, 6> Hr = boost::none, const gtsam::Pose3& xr, OptionalJacobian<3, 6> Hr = boost::none,
OptionalJacobian<3, 3> Hp = boost::none); OptionalJacobian<3, 3> Hp = boost::none);
/// Computes the error between two poses /** Computes the error between two poses.
* The error is a norm 1 difference in tangent space.
* @param the other plane
*/
Vector3 error(const gtsam::OrientedPlane3& plane) const; Vector3 error(const gtsam::OrientedPlane3& plane) const;
/// Dimensionality of tangent space = 3 DOF /// Dimensionality of tangent space = 3 DOF
@ -94,7 +107,7 @@ public:
/// The local coordinates function /// The local coordinates function
Vector3 localCoordinates(const OrientedPlane3& s) const; Vector3 localCoordinates(const OrientedPlane3& s) const;
/// Returns the plane coefficients (a, b, c, d) /// Returns the plane coefficients
Vector3 planeCoefficients() const; Vector3 planeCoefficients() const;
inline Unit3 normal() const { inline Unit3 normal() const {