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/base/MatrixConstants.h>
#include <gtsam/base/VectorConstants.h>
#include <gtsam/geometry/Pose3.h>
#include <gtsam/geometry/Unit3.h>
 
#include <string>
 
namespace gtsam {

class GTSAM_EXPORT OrientedPlane3 {
private:
  Unit3 n_;     
  double d_;    
 
public:
  inline constexpr static auto 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 = Matrix13{{0, 0, 1}};
    return d_;
  }
};
 
template<> struct traits<OrientedPlane3> : public internal::Manifold<
OrientedPlane3> {
};
 
template<> struct traits<const OrientedPlane3> : public internal::Manifold<
OrientedPlane3> {
};
 
}  // namespace gtsam