change: add some comments
							parent
							
								
									9404a49d42
								
							
						
					
					
						commit
						6c34ce1e80
					
				| 
						 | 
				
			
			@ -27,12 +27,12 @@
 | 
			
		|||
namespace gtsam {
 | 
			
		||||
 | 
			
		||||
/// Represents an infinite plane in 3D.
 | 
			
		||||
class OrientedPlane3: public DerivedValue<OrientedPlane3> {
 | 
			
		||||
class GTSAM_EXPORT OrientedPlane3 {
 | 
			
		||||
 | 
			
		||||
private:
 | 
			
		||||
 | 
			
		||||
  Unit3 n_; /// The direction of the planar normal
 | 
			
		||||
  double d_; /// The perpendicular distance to this plane
 | 
			
		||||
  Unit3 n_;     ///< The direction of the planar normal
 | 
			
		||||
  double d_;    ///< The perpendicular distance to this plane
 | 
			
		||||
 | 
			
		||||
public:
 | 
			
		||||
  enum {
 | 
			
		||||
| 
						 | 
				
			
			@ -51,17 +51,22 @@ public:
 | 
			
		|||
      n_(s), d_(d) {
 | 
			
		||||
  }
 | 
			
		||||
 | 
			
		||||
  /// Construct from a vector of plane coefficients
 | 
			
		||||
  OrientedPlane3(const Vector4& vec) :
 | 
			
		||||
      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) {
 | 
			
		||||
    Point3 p(a, b, c);
 | 
			
		||||
    n_ = Unit3(p);
 | 
			
		||||
    d_ = d;
 | 
			
		||||
  }
 | 
			
		||||
 | 
			
		||||
  /// @}
 | 
			
		||||
  /// @name Testable
 | 
			
		||||
  /// @{
 | 
			
		||||
 | 
			
		||||
  /// The print function
 | 
			
		||||
  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));
 | 
			
		||||
  }
 | 
			
		||||
 | 
			
		||||
  /// 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,
 | 
			
		||||
      const gtsam::Pose3& xr, OptionalJacobian<3, 6> Hr = 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;
 | 
			
		||||
 | 
			
		||||
  /// Dimensionality of tangent space = 3 DOF
 | 
			
		||||
| 
						 | 
				
			
			@ -94,7 +107,7 @@ public:
 | 
			
		|||
  /// The local coordinates function
 | 
			
		||||
  Vector3 localCoordinates(const OrientedPlane3& s) const;
 | 
			
		||||
 | 
			
		||||
  /// Returns the plane coefficients (a, b, c, d)
 | 
			
		||||
  /// Returns the plane coefficients
 | 
			
		||||
  Vector3 planeCoefficients() const;
 | 
			
		||||
 | 
			
		||||
  inline Unit3 normal() const {
 | 
			
		||||
| 
						 | 
				
			
			
 | 
			
		|||
		Loading…
	
		Reference in New Issue