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/Rot3.h>
#include <gtsam/geometry/Unit3.h>
#include <gtsam/geometry/Pose3.h>

namespace gtsam {

class GTSAM_EXPORT OrientedPlane3 {

private:

  Unit3 n_;     
  double d_;    

public:
  enum {
    dimension = 3
  };


  OrientedPlane3() :
    n_(), d_(0.0) {
  }

  OrientedPlane3(const Unit3& s, double d) :
    n_(s), d_(d) {
  }

  OrientedPlane3(const Vector4& vec) :
    n_(vec(0), vec(1), vec(2)), d_(vec(3)) {
  }

  OrientedPlane3(double a, double b, double c, double d) {
    Point3 p(a, b, c);
    n_ = Unit3(p);
    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) && (fabs(d_ - s.d_) < tol));
  }


  OrientedPlane3 transform(const Pose3& xr,
      OptionalJacobian<3, 3> Hp = boost::none,
      OptionalJacobian<3, 6> Hr = boost::none) const;

  static OrientedPlane3 Transform(const OrientedPlane3& plane,
      const Pose3& xr, OptionalJacobian<3, 6> Hr = boost::none,
      OptionalJacobian<3, 3> Hp = boost::none) {
      return plane.transform(xr, Hp, Hr);
  }

  Vector3 error(const OrientedPlane3& plane) 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() const {
    return n_;
  }

  inline double distance() const {
    return d_;
  }

};

template<> struct traits<OrientedPlane3> : public internal::Manifold<
OrientedPlane3> {
};

template<> struct traits<const OrientedPlane3> : public internal::Manifold<
OrientedPlane3> {
};

} // namespace gtsam