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 = {},
                           OptionalJacobian<3, 6> Hr = {}) const;

  Vector3 errorVector(const OrientedPlane3& other,
                      OptionalJacobian<3, 3> H1 = {},
                      OptionalJacobian<3, 3> H2 = {}) const;

  inline static size_t Dim() {
    return 3;
  }

  inline size_t dim() const {
    return 3;
  }

  OrientedPlane3 retract(const Vector3& v,
                        OptionalJacobian<3, 3> H = {}) 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 = {}) const {
    if (H) *H << I_2x2, Z_2x1;
    return n_;
  }

  inline double distance(OptionalJacobian<1, 3> H = {}) 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