geometry.hpp

Go to the documentation of this file.

 
#ifndef GEOMETRY_HPP
#define GEOMETRY_HPP
 
#include "se2.hpp"
#include "se3.hpp"
#include "so2.hpp"
#include "so3.hpp"
#include "types.hpp"
 
namespace Sophus {
 
template <class T>
Vector2<T> normalFromSO2(SO2<T> const& R_foo_line) {
  return R_foo_line.matrix().col(1);
}
 
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());
}
 
template <class T>
Vector3<T> normalFromSO3(SO3<T> const& R_foo_plane) {
  return R_foo_plane.matrix().col(2);
}
 
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;
}
 
template <class T>
SO3<T> SO3FromNormal(Vector3<T> const& normal_foo) {
  return SO3<T>(rotationFromNormal(normal_foo));
}
 
template <class T>
Line2<T> lineFromSE2(SE2<T> const& T_foo_line) {
  return Line2<T>(normalFromSO2(T_foo_line.so2()), T_foo_line.translation());
}
 
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);
}
 
template <class T>
Plane3<T> planeFromSE3(SE3<T> const& T_foo_plane) {
  return Plane3<T>(normalFromSO3(T_foo_plane.so3()), T_foo_plane.translation());
}
 
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);
}
 
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