transform.h
Go to the documentation of this file.
1 /*
2  * Copyright 2016 The Cartographer Authors
3  *
4  * Licensed under the Apache License, Version 2.0 (the "License");
5  * you may not use this file except in compliance with the License.
6  * You may obtain a copy of the License at
7  *
8  * http://www.apache.org/licenses/LICENSE-2.0
9  *
10  * Unless required by applicable law or agreed to in writing, software
11  * distributed under the License is distributed on an "AS IS" BASIS,
12  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13  * See the License for the specific language governing permissions and
14  * limitations under the License.
15  */
16 
17 #ifndef CARTOGRAPHER_TRANSFORM_TRANSFORM_H_
18 #define CARTOGRAPHER_TRANSFORM_TRANSFORM_H_
19 
20 #include <cmath>
21 
22 #include "Eigen/Core"
23 #include "Eigen/Geometry"
25 #include "cartographer/transform/proto/transform.pb.h"
27 
28 namespace cartographer {
29 namespace transform {
30 
31 // Returns the non-negative rotation angle in radians of the 3D transformation
32 // 'transform'.
33 template <typename FloatType>
34 FloatType GetAngle(const Rigid3<FloatType>& transform) {
35  return FloatType(2) * std::atan2(transform.rotation().vec().norm(),
36  std::abs(transform.rotation().w()));
37 }
38 
39 // Returns the yaw component in radians of the given 3D 'rotation'. Assuming
40 // 'rotation' is composed of three rotations around X, then Y, then Z, returns
41 // the angle of the Z rotation.
42 template <typename T>
43 T GetYaw(const Eigen::Quaternion<T>& rotation) {
44  const Eigen::Matrix<T, 3, 1> direction =
45  rotation * Eigen::Matrix<T, 3, 1>::UnitX();
46  return atan2(direction.y(), direction.x());
47 }
48 
49 // Returns the yaw component in radians of the given 3D transformation
50 // 'transform'.
51 template <typename T>
52 T GetYaw(const Rigid3<T>& transform) {
53  return GetYaw(transform.rotation());
54 }
55 
56 // Returns an angle-axis vector (a vector with the length of the rotation angle
57 // pointing to the direction of the rotation axis) representing the same
58 // rotation as the given 'quaternion'.
59 template <typename T>
60 Eigen::Matrix<T, 3, 1> RotationQuaternionToAngleAxisVector(
61  const Eigen::Quaternion<T>& quaternion) {
62  Eigen::Quaternion<T> normalized_quaternion = quaternion.normalized();
63  // We choose the quaternion with positive 'w', i.e., the one with a smaller
64  // angle that represents this orientation.
65  if (normalized_quaternion.w() < 0.) {
66  // Multiply by -1. http://eigen.tuxfamily.org/bz/show_bug.cgi?id=560
67  normalized_quaternion.w() = -1. * normalized_quaternion.w();
68  normalized_quaternion.x() = -1. * normalized_quaternion.x();
69  normalized_quaternion.y() = -1. * normalized_quaternion.y();
70  normalized_quaternion.z() = -1. * normalized_quaternion.z();
71  }
72  // We convert the normalized_quaternion into a vector along the rotation axis
73  // with length of the rotation angle.
74  const T angle =
75  2. * atan2(normalized_quaternion.vec().norm(), normalized_quaternion.w());
76  constexpr double kCutoffAngle = 1e-7; // We linearize below this angle.
77  const T scale = angle < kCutoffAngle ? T(2.) : angle / sin(angle / 2.);
78  return Eigen::Matrix<T, 3, 1>(scale * normalized_quaternion.x(),
79  scale * normalized_quaternion.y(),
80  scale * normalized_quaternion.z());
81 }
82 
83 // Returns a quaternion representing the same rotation as the given 'angle_axis'
84 // vector.
85 template <typename T>
87  const Eigen::Matrix<T, 3, 1>& angle_axis) {
88  T scale = T(0.5);
89  T w = T(1.);
90  constexpr double kCutoffAngle = 1e-8; // We linearize below this angle.
91  if (angle_axis.squaredNorm() > kCutoffAngle) {
92  const T norm = angle_axis.norm();
93  scale = sin(norm / 2.) / norm;
94  w = cos(norm / 2.);
95  }
96  const Eigen::Matrix<T, 3, 1> quaternion_xyz = scale * angle_axis;
97  return Eigen::Quaternion<T>(w, quaternion_xyz.x(), quaternion_xyz.y(),
98  quaternion_xyz.z());
99 }
100 
101 // Projects 'transform' onto the XY plane.
102 template <typename T>
103 Rigid2<T> Project2D(const Rigid3<T>& transform) {
104  return Rigid2<T>(transform.translation().template head<2>(),
105  GetYaw(transform));
106 }
107 
108 // Embeds 'transform' into 3D space in the XY plane.
109 template <typename T>
110 Rigid3<T> Embed3D(const Rigid2<T>& transform) {
111  return Rigid3<T>(
112  {transform.translation().x(), transform.translation().y(), T(0)},
113  Eigen::AngleAxis<T>(transform.rotation().angle(),
114  Eigen::Matrix<T, 3, 1>::UnitZ()));
115 }
116 
117 // Conversions between Eigen and proto.
118 Rigid2d ToRigid2(const proto::Rigid2d& transform);
119 Eigen::Vector2d ToEigen(const proto::Vector2d& vector);
120 Eigen::Vector3f ToEigen(const proto::Vector3f& vector);
121 Eigen::Vector4f ToEigen(const proto::Vector4f& vector);
122 Eigen::Vector3d ToEigen(const proto::Vector3d& vector);
123 Eigen::Quaterniond ToEigen(const proto::Quaterniond& quaternion);
124 proto::Rigid2d ToProto(const Rigid2d& transform);
125 proto::Rigid2f ToProto(const Rigid2f& transform);
126 proto::Rigid3d ToProto(const Rigid3d& rigid);
127 Rigid3d ToRigid3(const proto::Rigid3d& rigid);
128 proto::Rigid3f ToProto(const Rigid3f& rigid);
129 proto::Vector2d ToProto(const Eigen::Vector2d& vector);
130 proto::Vector3f ToProto(const Eigen::Vector3f& vector);
131 proto::Vector4f ToProto(const Eigen::Vector4f& vector);
132 proto::Vector3d ToProto(const Eigen::Vector3d& vector);
133 proto::Quaternionf ToProto(const Eigen::Quaternionf& quaternion);
134 proto::Quaterniond ToProto(const Eigen::Quaterniond& quaternion);
135 
136 } // namespace transform
137 } // namespace cartographer
138 
139 #endif // CARTOGRAPHER_TRANSFORM_TRANSFORM_H_
Eigen::Quaternion< T > AngleAxisVectorToRotationQuaternion(const Eigen::Matrix< T, 3, 1 > &angle_axis)
Definition: transform.h:86
Rigid3< double > Rigid3d
Eigen::Matrix< T, 3, 1 > RotationQuaternionToAngleAxisVector(const Eigen::Quaternion< T > &quaternion)
Definition: transform.h:60
T atan2(const Eigen::Matrix< T, 2, 1 > &vector)
Definition: math.h:70
Rigid2d ToRigid2(const proto::Rigid2d &transform)
Definition: transform.cc:22
Rigid3< T > Embed3D(const Rigid2< T > &transform)
Definition: transform.h:110
T GetYaw(const Eigen::Quaternion< T > &rotation)
Definition: transform.h:43
transform::Rigid3d ToRigid3(const proto::Rigid3d &rigid)
Definition: transform.cc:71
Rigid2< T > Project2D(const Rigid3< T > &transform)
Definition: transform.h:103
const Quaternion & rotation() const
Rigid2< double > Rigid2d
proto::Rigid2d ToProto(const transform::Rigid2d &transform)
Definition: transform.cc:48
const Vector & translation() const
const Vector & translation() const
FloatType GetAngle(const Rigid3< FloatType > &transform)
Definition: transform.h:34
Eigen::Transform< T, 2, Eigen::Affine > ToEigen(const Rigid2< T > &rigid2)


cartographer
Author(s): The Cartographer Authors
autogenerated on Mon Feb 28 2022 22:00:59