lio-cpp/lio/include/sophus/geometry.hpp

/// @file
/// Transformations between poses and hyperplanes.

#ifndef GEOMETRY_HPP
#define GEOMETRY_HPP

#include "se2.hpp"
#include "se3.hpp"
#include "so2.hpp"
#include "so3.hpp"
#include "types.hpp"

namespace Sophus {

/// Takes in a rotation ``R_foo_plane`` and returns the corresponding line
/// normal along the y-axis (in reference frame ``foo``).
///
template <class T>
Vector2<T> normalFromSO2(SO2<T> const& R_foo_line) {
  return R_foo_line.matrix().col(1);
}

/// Takes in line normal in reference frame foo and constructs a corresponding
/// rotation matrix ``R_foo_line``.
///
/// Precondition: ``normal_foo`` must not be close to zero.
///
template <class T>
SO2<T> SO2FromNormal(Vector2<T> normal_foo) {
  SOPHUS_ENSURE(normal_foo.squaredNorm() > Constants<T>::epsilon(), "%",
                normal_foo.transpose());
  normal_foo.normalize();
  return SO2<T>(normal_foo.y(), -normal_foo.x());
}

/// Takes in a rotation ``R_foo_plane`` and returns the corresponding plane
/// normal along the z-axis
/// (in reference frame ``foo``).
///
template <class T>
Vector3<T> normalFromSO3(SO3<T> const& R_foo_plane) {
  return R_foo_plane.matrix().col(2);
}

/// Takes in plane normal in reference frame foo and constructs a corresponding
/// rotation matrix ``R_foo_plane``.
///
/// Note: The ``plane`` frame is defined as such that the normal points along
///       the positive z-axis. One can specify hints for the x-axis and y-axis
///       of the ``plane`` frame.
///
/// Preconditions:
/// - ``normal_foo``, ``xDirHint_foo``, ``yDirHint_foo`` must not be close to
///   zero.
/// - ``xDirHint_foo`` and ``yDirHint_foo`` must be approx. perpendicular.
///
template <class T>
Matrix3<T> rotationFromNormal(Vector3<T> const& normal_foo,
                              Vector3<T> xDirHint_foo = Vector3<T>(T(1), T(0),
                                                                   T(0)),
                              Vector3<T> yDirHint_foo = Vector3<T>(T(0), T(1),
                                                                   T(0))) {
  SOPHUS_ENSURE(xDirHint_foo.dot(yDirHint_foo) < Constants<T>::epsilon(),
                "xDirHint (%) and yDirHint (%) must be perpendicular.",
                xDirHint_foo.transpose(), yDirHint_foo.transpose());
  using std::abs;
  using std::sqrt;
  T const xDirHint_foo_sqr_length = xDirHint_foo.squaredNorm();
  T const yDirHint_foo_sqr_length = yDirHint_foo.squaredNorm();
  T const normal_foo_sqr_length = normal_foo.squaredNorm();
  SOPHUS_ENSURE(xDirHint_foo_sqr_length > Constants<T>::epsilon(), "%",
                xDirHint_foo.transpose());
  SOPHUS_ENSURE(yDirHint_foo_sqr_length > Constants<T>::epsilon(), "%",
                yDirHint_foo.transpose());
  SOPHUS_ENSURE(normal_foo_sqr_length > Constants<T>::epsilon(), "%",
                normal_foo.transpose());

  Matrix3<T> basis_foo;
  basis_foo.col(2) = normal_foo;

  if (abs(xDirHint_foo_sqr_length - T(1)) > Constants<T>::epsilon()) {
    xDirHint_foo.normalize();
  }
  if (abs(yDirHint_foo_sqr_length - T(1)) > Constants<T>::epsilon()) {
    yDirHint_foo.normalize();
  }
  if (abs(normal_foo_sqr_length - T(1)) > Constants<T>::epsilon()) {
    basis_foo.col(2).normalize();
  }

  T abs_x_dot_z = abs(basis_foo.col(2).dot(xDirHint_foo));
  T abs_y_dot_z = abs(basis_foo.col(2).dot(yDirHint_foo));
  if (abs_x_dot_z < abs_y_dot_z) {
    // basis_foo.z and xDirHint are far from parallel.
    basis_foo.col(1) = basis_foo.col(2).cross(xDirHint_foo).normalized();
    basis_foo.col(0) = basis_foo.col(1).cross(basis_foo.col(2));
  } else {
    // basis_foo.z and yDirHint are far from parallel.
    basis_foo.col(0) = yDirHint_foo.cross(basis_foo.col(2)).normalized();
    basis_foo.col(1) = basis_foo.col(2).cross(basis_foo.col(0));
  }
  T det = basis_foo.determinant();
  // sanity check
  SOPHUS_ENSURE(abs(det - T(1)) < Constants<T>::epsilon(),
                "Determinant of basis is not 1, but %. Basis is \n%\n", det,
                basis_foo);
  return basis_foo;
}

/// Takes in plane normal in reference frame foo and constructs a corresponding
/// rotation matrix ``R_foo_plane``.
///
/// See ``rotationFromNormal`` for details.
///
template <class T>
SO3<T> SO3FromNormal(Vector3<T> const& normal_foo) {
  return SO3<T>(rotationFromNormal(normal_foo));
}

/// Returns a line (wrt. to frame ``foo``), given a pose of the ``line`` in
/// reference frame ``foo``.
///
/// Note: The plane is defined by X-axis of the ``line`` frame.
///
template <class T>
Line2<T> lineFromSE2(SE2<T> const& T_foo_line) {
  return Line2<T>(normalFromSO2(T_foo_line.so2()), T_foo_line.translation());
}

/// Returns the pose ``T_foo_line``, given a line in reference frame ``foo``.
///
/// Note: The line is defined by X-axis of the frame ``line``.
///
template <class T>
SE2<T> SE2FromLine(Line2<T> const& line_foo) {
  T const d = line_foo.offset();
  Vector2<T> const n = line_foo.normal();
  SO2<T> const R_foo_plane = SO2FromNormal(n);
  return SE2<T>(R_foo_plane, -d * n);
}

/// Returns a plane (wrt. to frame ``foo``), given a pose of the ``plane`` in
/// reference frame ``foo``.
///
/// Note: The plane is defined by XY-plane of the frame ``plane``.
///
template <class T>
Plane3<T> planeFromSE3(SE3<T> const& T_foo_plane) {
  return Plane3<T>(normalFromSO3(T_foo_plane.so3()), T_foo_plane.translation());
}

/// Returns the pose ``T_foo_plane``, given a plane in reference frame ``foo``.
///
/// Note: The plane is defined by XY-plane of the frame ``plane``.
///
template <class T>
SE3<T> SE3FromPlane(Plane3<T> const& plane_foo) {
  T const d = plane_foo.offset();
  Vector3<T> const n = plane_foo.normal();
  SO3<T> const R_foo_plane = SO3FromNormal(n);
  return SE3<T>(R_foo_plane, -d * n);
}

/// Takes in a hyperplane and returns unique representation by ensuring that the
/// ``offset`` is not negative.
///
template <class T, int N>
Eigen::Hyperplane<T, N> makeHyperplaneUnique(
    Eigen::Hyperplane<T, N> const& plane) {
  if (plane.offset() >= 0) {
    return plane;
  }

  return Eigen::Hyperplane<T, N>(-plane.normal(), -plane.offset());
}

}  // namespace Sophus

#endif  // GEOMETRY_HPP