Gazebo Math

API Reference

7.4.0
gz/math/eigen3/Conversions.hh
Go to the documentation of this file.
1/*
2 * Copyright (C) 2018 Open Source Robotics Foundation
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
18#ifndef GZ_MATH_EIGEN3_CONVERSIONS_HH_
19#define GZ_MATH_EIGEN3_CONVERSIONS_HH_
20
21#include <Eigen/Geometry>
23#include <gz/math/Matrix3.hh>
24#include <gz/math/Matrix6.hh>
25#include <gz/math/Pose3.hh>
26#include <gz/math/Quaternion.hh>
27#include <gz/math/Vector3.hh>
28
29namespace gz
30{
31 namespace math
32 {
33 namespace eigen3
34 {
38 inline Eigen::Vector3d convert(const gz::math::Vector3d &_v)
39 {
40 return Eigen::Vector3d(_v[0], _v[1], _v[2]);
41 }
42
47 inline Eigen::AlignedBox3d convert(
49 {
50 return Eigen::AlignedBox3d(convert(_b.Min()), convert(_b.Max()));
51 }
52
56 inline Eigen::Matrix3d convert(const gz::math::Matrix3d &_m)
57 {
58 Eigen::Matrix3d matrix;
59 for (std::size_t i=0; i < 3; ++i)
60 {
61 for (std::size_t j=0; j < 3; ++j)
62 {
63 matrix(i, j) = _m(i, j);
64 }
65 }
66
67 return matrix;
68 }
69
75 template<typename Precision>
76 inline
77 Eigen::Matrix<Precision, 6, 6> convert(const Matrix6<Precision> &_m)
78 {
79 Eigen::Matrix<Precision, 6, 6> matrix;
80 for (std::size_t i = 0; i < 6; ++i)
81 {
82 for (std::size_t j = 0; j < 6; ++j)
83 {
84 matrix(i, j) = _m(i, j);
85 }
86 }
87
88 return matrix;
89 }
90
94 inline Eigen::Quaterniond convert(const gz::math::Quaterniond &_q)
95 {
96 Eigen::Quaterniond quat;
97 quat.w() = _q.W();
98 quat.x() = _q.X();
99 quat.y() = _q.Y();
100 quat.z() = _q.Z();
101
102 return quat;
103 }
104
108 inline Eigen::Isometry3d convert(const gz::math::Pose3d &_pose)
109 {
110 Eigen::Isometry3d tf = Eigen::Isometry3d::Identity();
111 tf.translation() = convert(_pose.Pos());
112 tf.linear() = Eigen::Matrix3d(convert(_pose.Rot()));
113
114 return tf;
115 }
116
120 inline gz::math::Vector3d convert(const Eigen::Vector3d &_v)
121 {
123 vec.X() = _v[0];
124 vec.Y() = _v[1];
125 vec.Z() = _v[2];
126
127 return vec;
128 }
129
134 const Eigen::AlignedBox3d &_b)
135 {
137 box.Min() = convert(_b.min());
138 box.Max() = convert(_b.max());
139
140 return box;
141 }
142
146 inline gz::math::Matrix3d convert(const Eigen::Matrix3d &_m)
147 {
148 gz::math::Matrix3d matrix;
149 for (std::size_t i=0; i < 3; ++i)
150 {
151 for (std::size_t j=0; j < 3; ++j)
152 {
153 matrix(i, j) = _m(i, j);
154 }
155 }
156
157 return matrix;
158 }
159
165 template<typename Precision>
166 inline
167 Matrix6<Precision> convert(const Eigen::Matrix<Precision, 6, 6> &_m)
168 {
169 Matrix6<Precision> matrix;
170 for (std::size_t i = 0; i < 6; ++i)
171 {
172 for (std::size_t j = 0; j < 6; ++j)
173 {
174 matrix(i, j) = _m(i, j);
175 }
176 }
177
178 return matrix;
179 }
180
184 inline gz::math::Quaterniond convert(const Eigen::Quaterniond &_q)
185 {
187 quat.W() = _q.w();
188 quat.X() = _q.x();
189 quat.Y() = _q.y();
190 quat.Z() = _q.z();
191
192 return quat;
193 }
194
198 inline gz::math::Pose3d convert(const Eigen::Isometry3d &_tf)
199 {
200 gz::math::Pose3d pose;
201 pose.Pos() = convert(Eigen::Vector3d(_tf.translation()));
202 pose.Rot() = convert(Eigen::Quaterniond(_tf.linear()));
203
204 return pose;
205 }
206 }
207 }
208}
209
210#endif