.. _program_listing_file_sophus_geometry.hpp:

Program Listing for File geometry.hpp
=====================================

|exhale_lsh| :ref:`Return to documentation for file <file_sophus_geometry.hpp>` (``sophus/geometry.hpp``)

.. |exhale_lsh| unicode:: U+021B0 .. UPWARDS ARROW WITH TIP LEFTWARDS

.. code-block:: cpp

   
   #pragma once
   
   #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(), "{}",
                   SOPHUS_FMT_ARG(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.",
                   SOPHUS_FMT_ARG(xDirHint_foo.transpose()),
                   SOPHUS_FMT_ARG(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(), "{}",
                   SOPHUS_FMT_ARG(xDirHint_foo.transpose()));
     SOPHUS_ENSURE(yDirHint_foo_sqr_length > Constants<T>::epsilon(), "{}",
                   SOPHUS_FMT_ARG(yDirHint_foo.transpose()));
     SOPHUS_ENSURE(normal_foo_sqr_length > Constants<T>::epsilon(), "{}",
                   SOPHUS_FMT_ARG(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",
                   SOPHUS_FMT_ARG(det), SOPHUS_FMT_ARG(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