Main MRPT website > C++ reference for MRPT 1.5.3
geometry.hpp
Go to the documentation of this file.
1 #ifndef GEOMETRY_HPP
2 #define GEOMETRY_HPP
3 
4 #include "se2.hpp"
5 #include "se3.hpp"
6 #include "so2.hpp"
7 #include "so3.hpp"
8 #include "types.hpp"
9 
10 namespace Sophus {
11 
12 // Takes in a rotation ``R_foo_plane`` and returns the corresponding line
13 // normal along the y-axis (in reference frame ``foo``).
14 //
15 template <class T>
16 Vector2<T> normalFromSO2(SO2<T> const& R_foo_line) {
17  return R_foo_line.matrix().col(1);
18 }
19 
20 // Takes in line normal in reference frame foo and constructs a corresponding
21 // rotation matrix ``R_foo_line``.
22 //
23 // Precondition: ``normal_foo`` must not be close to zero.
24 //
25 template <class T>
27  SOPHUS_ENSURE(normal_foo.squaredNorm() > Constants<T>::epsilon(), "%",
28  normal_foo.transpose());
29  normal_foo.normalize();
30  return SO2<T>(normal_foo.y(), -normal_foo.x());
31 }
32 
33 // Takes in a rotation ``R_foo_plane`` and returns the corresponding plane
34 // normal along the z-axis
35 // (in reference frame ``foo``).
36 //
37 template <class T>
38 Vector3<T> normalFromSO3(SO3<T> const& R_foo_plane) {
39  return R_foo_plane.matrix().col(2);
40 }
41 
42 // Takes in plane normal in reference frame foo and constructs a corresponding
43 // rotation matrix ``R_foo_plane``.
44 //
45 // Note: The ``plane`` frame is defined as such that the normal points along the
46 // positive z-axis. One can specify hints for the x-axis and y-axis of the
47 // ``plane`` frame.
48 //
49 // Preconditions:
50 // - ``normal_foo``, ``xDirHint_foo``, ``yDirHint_foo`` must not be close to
51 // zero.
52 // - ``xDirHint_foo`` and ``yDirHint_foo`` must be approx. perpendicular.
53 //
54 template <class T>
56  Vector3<T> xDirHint_foo = Vector3<T>(T(1), T(0),
57  T(0)),
58  Vector3<T> yDirHint_foo = Vector3<T>(T(0), T(1),
59  T(0))) {
60  SOPHUS_ENSURE(xDirHint_foo.dot(yDirHint_foo) < Constants<T>::epsilon(),
61  "xDirHint (%) and yDirHint (%) must be perpendicular.",
62  xDirHint_foo.transpose(), yDirHint_foo.transpose());
63  using std::abs;
64  using std::sqrt;
65  T const xDirHint_foo_sqr_length = xDirHint_foo.squaredNorm();
66  T const yDirHint_foo_sqr_length = yDirHint_foo.squaredNorm();
67  T const normal_foo_sqr_length = normal_foo.squaredNorm();
68  SOPHUS_ENSURE(xDirHint_foo_sqr_length > Constants<T>::epsilon(), "%",
69  xDirHint_foo.transpose());
70  SOPHUS_ENSURE(yDirHint_foo_sqr_length > Constants<T>::epsilon(), "%",
71  yDirHint_foo.transpose());
72  SOPHUS_ENSURE(normal_foo_sqr_length > Constants<T>::epsilon(), "%",
73  normal_foo.transpose());
74 
75  Matrix3<T> basis_foo;
76  basis_foo.col(2) = normal_foo;
77 
78  if (abs(xDirHint_foo_sqr_length - T(1)) > Constants<T>::epsilon()) {
79  xDirHint_foo.normalize();
80  }
81  if (abs(yDirHint_foo_sqr_length - T(1)) > Constants<T>::epsilon()) {
82  yDirHint_foo.normalize();
83  }
84  if (abs(normal_foo_sqr_length - T(1)) > Constants<T>::epsilon()) {
85  basis_foo.col(2).normalize();
86  }
87 
88  T abs_x_dot_z = abs(basis_foo.col(2).dot(xDirHint_foo));
89  T abs_y_dot_z = abs(basis_foo.col(2).dot(yDirHint_foo));
90  if (abs_x_dot_z < abs_y_dot_z) {
91  // basis_foo.z and xDirHint are far from parallel.
92  basis_foo.col(1) = basis_foo.col(2).cross(xDirHint_foo).normalized();
93  basis_foo.col(0) = basis_foo.col(1).cross(basis_foo.col(2));
94  } else {
95  // basis_foo.z and yDirHint are far from parallel.
96  basis_foo.col(0) = yDirHint_foo.cross(basis_foo.col(2)).normalized();
97  basis_foo.col(1) = basis_foo.col(2).cross(basis_foo.col(0));
98  }
99  T det = basis_foo.determinant();
100  // sanity check
101  SOPHUS_ENSURE(abs(det - T(1)) < Constants<T>::epsilon(),
102  "Determinant of basis is not 1, but %. Basis is \n%\n", det,
103  basis_foo);
104  return basis_foo;
105 }
106 
107 // Takes in plane normal in reference frame foo and constructs a corresponding
108 // rotation matrix ``R_foo_plane``.
109 //
110 // See ``rotationFromNormal`` for details.
111 //
112 template <class T>
113 SO3<T> SO3FromNormal(Vector3<T> const& normal_foo) {
114  return SO3<T>(rotationFromNormal(normal_foo));
115 }
116 
117 // Returns a line (wrt. to frame ``foo``), given a pose of the ``line`` in
118 // reference frame ``foo``.
119 //
120 // Note: The plane is defined by X-axis of the ``line`` frame.
121 //
122 template <class T>
123 Line2<T> lineFromSE2(SE2<T> const& T_foo_line) {
124  return Line2<T>(normalFromSO2(T_foo_line.so2()), T_foo_line.translation());
125 }
126 
127 // Returns the pose ``T_foo_line``, given a line in reference frame ``foo``.
128 //
129 // Note: The line is defined by X-axis of the frame ``line``.
130 //
131 template <class T>
132 SE2<T> SE2FromLine(Line2<T> const& line_foo) {
133  T const d = line_foo.offset();
134  Vector2<T> const n = line_foo.normal();
135  SO2<T> const R_foo_plane = SO2FromNormal(n);
136  return SE2<T>(R_foo_plane, -d * n);
137 }
138 
139 // Returns a plane (wrt. to frame ``foo``), given a pose of the ``plane`` in
140 // reference frame ``foo``.
141 //
142 // Note: The plane is defined by XY-plane of the frame ``plane``.
143 //
144 template <class T>
145 Plane3<T> planeFromSE3(SE3<T> const& T_foo_plane) {
146  return Plane3<T>(normalFromSO3(T_foo_plane.so3()), T_foo_plane.translation());
147 }
148 
149 // Returns the pose ``T_foo_plane``, given a plane in reference frame ``foo``.
150 //
151 // Note: The plane is defined by XY-plane of the frame ``plane``.
152 //
153 template <class T>
154 SE3<T> SE3FromPlane(Plane3<T> const& plane_foo) {
155  T const d = plane_foo.offset();
156  Vector3<T> const n = plane_foo.normal();
157  SO3<T> const R_foo_plane = SO3FromNormal(n);
158  return SE3<T>(R_foo_plane, -d * n);
159 }
160 
161 // Takes in a hyperplane and returns unique representation by ensuring that the
162 // ``offset`` is not negative.
163 //
164 template <class T, int N>
165 Eigen::Hyperplane<T, N> makeHyperplaneUnique(
166  const Eigen::Hyperplane<T, N>& plane) {
167  if (plane.offset() >= 0) {
168  return plane;
169  }
170 
171  return Eigen::Hyperplane<T, N>(-plane.normal(), -plane.offset());
172 }
173 
174 } // namespace Sophus
175 
176 #endif // GEOMETRY_HPP
Eigen::Hyperplane< T, 3 > Plane3
Definition: types.hpp:156
EIGEN_STRONG_INLINE Scalar det() const
Eigen::Hyperplane< T, 2 > Line2
Definition: types.hpp:162
#define SOPHUS_ENSURE(expr, description,...)
Definition: common.hpp:129
SOPHUS_FUNC TranslationMember & translation()
Definition: se2.hpp:537
Vector2< T > normalFromSO2(SO2< T > const &R_foo_line)
Definition: geometry.hpp:16
Vector< Scalar, 2, Options > Vector2
Definition: types.hpp:13
static SOPHUS_FUNC Scalar epsilon()
Definition: common.hpp:139
Vector3< T > normalFromSO3(SO3< T > const &R_foo_plane)
Definition: geometry.hpp:38
SOPHUS_FUNC SO2Member & so2()
Definition: se2.hpp:529
Vector< Scalar, 3, Options > Vector3
Definition: types.hpp:18
Eigen::Hyperplane< T, N > makeHyperplaneUnique(const Eigen::Hyperplane< T, N > &plane)
Definition: geometry.hpp:165
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)))
Definition: geometry.hpp:55
SOPHUS_FUNC Transformation matrix() const
Definition: so3.hpp:172
Matrix< Scalar, 3, 3 > Matrix3
Definition: types.hpp:46
SO3< T > SO3FromNormal(Vector3< T > const &normal_foo)
Definition: geometry.hpp:113
SOPHUS_FUNC SO3Member & so3()
Definition: se3.hpp:625
SE2< T > SE2FromLine(Line2< T > const &line_foo)
Definition: geometry.hpp:132
SOPHUS_FUNC Transformation matrix() const
Definition: so2.hpp:148
Plane3< T > planeFromSE3(SE3< T > const &T_foo_plane)
Definition: geometry.hpp:145
SOPHUS_FUNC TranslationMember & translation()
Definition: se3.hpp:633
SE3< T > SE3FromPlane(Plane3< T > const &plane_foo)
Definition: geometry.hpp:154
Line2< T > lineFromSE2(SE2< T > const &T_foo_line)
Definition: geometry.hpp:123
SO2< T > SO2FromNormal(Vector2< T > normal_foo)
Definition: geometry.hpp:26



Page generated by Doxygen 1.8.13 for MRPT 1.5.3 at Tue Aug 22 01:03:35 UTC 2017