gtsam/gtsam/geometry/OrientedPlane3.cpp

/* ----------------------------------------------------------------------------

 * 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.cpp
 * @date Dec 19, 2013
 * @author Alex Trevor
 * @brief  A plane, represented by a normal direction and perpendicular distance
 */

#include <gtsam/geometry/OrientedPlane3.h>
#include <gtsam/geometry/Point2.h>
#include <iostream>

using namespace std;

namespace gtsam {

/* ************************************************************************* */
/// The print fuction
void OrientedPlane3::print(const std::string& s) const {
  Vector coeffs = planeCoefficients();
  cout << s << " : " << coeffs << endl;
}

/* ************************************************************************* */
OrientedPlane3 OrientedPlane3::Transform(const gtsam::OrientedPlane3& plane,
    const gtsam::Pose3& xr, OptionalJacobian<3, 6> Hr,
    OptionalJacobian<3, 3> Hp) {
  Matrix n_hr;
  Matrix n_hp;
  Unit3 n_rotated = xr.rotation().unrotate(plane.n_, n_hr, n_hp);

  Vector n_unit = plane.n_.unitVector();
  Vector unit_vec = n_rotated.unitVector();
  double pred_d = n_unit.dot(xr.translation().vector()) + plane.d_;
  OrientedPlane3 transformed_plane(unit_vec(0), unit_vec(1), unit_vec(2),
      pred_d);

  if (Hr) {
    *Hr = gtsam::zeros(3, 6);
    (*Hr).block<2, 3>(0, 0) = n_hr;
    (*Hr).block<1, 3>(2, 3) = unit_vec;
  }
  if (Hp) {
    Vector xrp = xr.translation().vector();
    Matrix n_basis = plane.n_.basis();
    Vector hpp = n_basis.transpose() * xrp;
    *Hp = gtsam::zeros(3, 3);
    (*Hp).block<2, 2>(0, 0) = n_hp;
    (*Hp).block<1, 2>(2, 0) = hpp;
    (*Hp)(2, 2) = 1;
  }

  return (transformed_plane);
}

/* ************************************************************************* */
Vector3 OrientedPlane3::error(const gtsam::OrientedPlane3& plane) const {
  Vector2 n_error = -n_.localCoordinates(plane.n_);
  double d_error = d_ - plane.d_;
  Vector3 e;
  e << n_error(0), n_error(1), d_error;
  return (e);
}

/* ************************************************************************* */
OrientedPlane3 OrientedPlane3::retract(const Vector3& v) const {
  // Retract the Unit3
  Vector2 n_v(v(0), v(1));
  Unit3 n_retracted = n_.retract(n_v);
  double d_retracted = d_ + v(2);
  return OrientedPlane3(n_retracted, d_retracted);
}

/* ************************************************************************* */
Vector3 OrientedPlane3::localCoordinates(const OrientedPlane3& y) const {
  Vector2 n_local = n_.localCoordinates(y.n_);
  double d_local = d_ - y.d_;
  Vector3 e;
  e << n_local(0), n_local(1), -d_local;
  return e;
}

}