rigid_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_RIGID_TRANSFORM_H_
18 #define CARTOGRAPHER_TRANSFORM_RIGID_TRANSFORM_H_
19 
20 #include <iostream>
21 #include <string>
22 
23 #include "Eigen/Core"
24 #include "Eigen/Geometry"
28 
29 namespace cartographer {
30 namespace transform {
31 
32 template <typename FloatType>
33 class Rigid2 {
34  public:
35  using Vector = Eigen::Matrix<FloatType, 2, 1>;
36  using Rotation2D = Eigen::Rotation2D<FloatType>;
37 
41  : translation_(translation), rotation_(rotation) {}
42  Rigid2(const Vector& translation, const double rotation)
43  : translation_(translation), rotation_(rotation) {}
44 
45  static Rigid2 Rotation(const double rotation) {
46  return Rigid2(Vector::Zero(), rotation);
47  }
48 
50  return Rigid2(Vector::Zero(), rotation);
51  }
52 
53  static Rigid2 Translation(const Vector& vector) {
54  return Rigid2(vector, Rotation2D::Identity());
55  }
56 
58  return Rigid2<FloatType>(Vector::Zero(), Rotation2D::Identity());
59  }
60 
61  template <typename OtherType>
63  return Rigid2<OtherType>(translation_.template cast<OtherType>(),
64  rotation_.template cast<OtherType>());
65  }
66 
67  const Vector& translation() const { return translation_; }
68 
69  Rotation2D rotation() const { return rotation_; }
70 
71  double normalized_angle() const {
73  }
74 
75  Rigid2 inverse() const {
76  const Rotation2D rotation = rotation_.inverse();
77  const Vector translation = -(rotation * translation_);
78  return Rigid2(translation, rotation);
79  }
80 
81  string DebugString() const {
82  string out;
83  out.append("{ t: [");
84  out.append(std::to_string(translation().x()));
85  out.append(", ");
86  out.append(std::to_string(translation().y()));
87  out.append("], r: [");
88  out.append(std::to_string(rotation().angle()));
89  out.append("] }");
90  return out;
91  }
92 
93  private:
96 };
97 
98 template <typename FloatType>
100  const Rigid2<FloatType>& rhs) {
101  return Rigid2<FloatType>(
102  lhs.rotation() * rhs.translation() + lhs.translation(),
103  lhs.rotation() * rhs.rotation());
104 }
105 
106 template <typename FloatType>
108  const Rigid2<FloatType>& rigid,
109  const typename Rigid2<FloatType>::Vector& point) {
110  return rigid.rotation() * point + rigid.translation();
111 }
112 
113 // This is needed for gmock.
114 template <typename T>
115 std::ostream& operator<<(std::ostream& os,
116  const cartographer::transform::Rigid2<T>& rigid) {
117  os << rigid.DebugString();
118  return os;
119 }
120 
123 
124 template <typename FloatType>
125 class Rigid3 {
126  public:
127  using Vector = Eigen::Matrix<FloatType, 3, 1>;
128  using Quaternion = Eigen::Quaternion<FloatType>;
129  using AngleAxis = Eigen::AngleAxis<FloatType>;
130 
134  : translation_(translation), rotation_(rotation) {}
136  : translation_(translation), rotation_(rotation) {}
137 
138  static Rigid3 Rotation(const AngleAxis& angle_axis) {
139  return Rigid3(Vector::Zero(), Quaternion(angle_axis));
140  }
141 
143  return Rigid3(Vector::Zero(), rotation);
144  }
145 
146  static Rigid3 Translation(const Vector& vector) {
147  return Rigid3(vector, Quaternion::Identity());
148  }
149 
151  return Rigid3<FloatType>(Vector::Zero(), Quaternion::Identity());
152  }
153 
154  template <typename OtherType>
156  return Rigid3<OtherType>(translation_.template cast<OtherType>(),
157  rotation_.template cast<OtherType>());
158  }
159 
160  const Vector& translation() const { return translation_; }
161  const Quaternion& rotation() const { return rotation_; }
162 
163  Rigid3 inverse() const {
164  const Quaternion rotation = rotation_.conjugate();
165  const Vector translation = -(rotation * translation_);
166  return Rigid3(translation, rotation);
167  }
168 
169  string DebugString() const {
170  string out;
171  out.append("{ t: [");
172  out.append(std::to_string(translation().x()));
173  out.append(", ");
174  out.append(std::to_string(translation().y()));
175  out.append(", ");
176  out.append(std::to_string(translation().z()));
177  out.append("], q: [");
178  out.append(std::to_string(rotation().w()));
179  out.append(", ");
180  out.append(std::to_string(rotation().x()));
181  out.append(", ");
182  out.append(std::to_string(rotation().y()));
183  out.append(", ");
184  out.append(std::to_string(rotation().z()));
185  out.append("] }");
186  return out;
187  }
188 
189  private:
192 };
193 
194 template <typename FloatType>
196  const Rigid3<FloatType>& rhs) {
197  return Rigid3<FloatType>(
198  lhs.rotation() * rhs.translation() + lhs.translation(),
199  (lhs.rotation() * rhs.rotation()).normalized());
200 }
201 
202 template <typename FloatType>
204  const Rigid3<FloatType>& rigid,
205  const typename Rigid3<FloatType>::Vector& point) {
206  return rigid.rotation() * point + rigid.translation();
207 }
208 
209 // This is needed for gmock.
210 template <typename T>
211 std::ostream& operator<<(std::ostream& os,
212  const cartographer::transform::Rigid3<T>& rigid) {
213  os << rigid.DebugString();
214  return os;
215 }
216 
219 
220 // Converts (roll, pitch, yaw) to a unit length quaternion. Based on the URDF
221 // specification http://wiki.ros.org/urdf/XML/joint.
222 Eigen::Quaterniond RollPitchYaw(double roll, double pitch, double yaw);
223 
224 // Returns an transform::Rigid3d given a 'dictionary' containing 'translation'
225 // (x, y, z) and 'rotation' which can either we an array of (roll, pitch, yaw)
226 // or a dictionary with (w, x, y, z) values as a quaternion.
228 
229 } // namespace transform
230 } // namespace cartographer
231 
232 #endif // CARTOGRAPHER_TRANSFORM_RIGID_TRANSFORM_H_
const Vector & translation() const
const Quaternion & rotation() const
static Rigid3 Rotation(const AngleAxis &angle_axis)
Eigen::Matrix< FloatType, 2, 1 > Vector
Rigid3(const Vector &translation, const Quaternion &rotation)
static Rigid2 Rotation(const double rotation)
Rigid2< OtherType > cast() const
Rigid3< OtherType > cast() const
Rigid2(const Vector &translation, const Rotation2D &rotation)
float angle
static Rigid2< FloatType > Identity()
Rigid2< FloatType > operator*(const Rigid2< FloatType > &lhs, const Rigid2< FloatType > &rhs)
static Rigid3< FloatType > Identity()
Eigen::Quaternion< double > Quaternion
Rigid3(const Vector &translation, const AngleAxis &rotation)
Eigen::Rotation2D< FloatType > Rotation2D
static Rigid3 Rotation(const Quaternion &rotation)
const Vector & translation() const
std::ostream & operator<<(std::ostream &os, const cartographer::transform::Rigid2< T > &rigid)
transform::Rigid3d FromDictionary(common::LuaParameterDictionary *dictionary)
Rigid2(const Vector &translation, const double rotation)
T NormalizeAngleDifference(T difference)
Definition: math.h:62
static Rigid2 Rotation(const Rotation2D &rotation)
static Rigid3 Translation(const Vector &vector)
Eigen::Quaterniond RollPitchYaw(const double roll, const double pitch, const double yaw)
Eigen::Matrix< double, 3, 1 > Vector
static Rigid2 Translation(const Vector &vector)


cartographer
Author(s):
autogenerated on Mon Jun 10 2019 12:51:39