Ignition Math

API Reference

6.4.0
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 IGNITION_MATH_EIGEN3_CONVERSIONS_HH_
19 #define IGNITION_MATH_EIGEN3_CONVERSIONS_HH_
20 
21 #include <Eigen/Geometry>
23 #include <ignition/math/Matrix3.hh>
24 #include <ignition/math/Pose3.hh>
26 #include <ignition/math/Vector3.hh>
27 
28 namespace ignition
29 {
30  namespace math
31  {
32  namespace eigen3
33  {
38  {
39  return Eigen::Vector3d(_v[0], _v[1], _v[2]);
40  }
41 
46  inline Eigen::AlignedBox3d convert(
48  {
49  return Eigen::AlignedBox3d(convert(_b.Min()), convert(_b.Max()));
50  }
51 
56  {
57  Eigen::Matrix3d matrix;
58  for (std::size_t i=0; i < 3; ++i)
59  {
60  for (std::size_t j=0; j < 3; ++j)
61  {
62  matrix(i, j) = _m(i, j);
63  }
64  }
65 
66  return matrix;
67  }
68 
73  {
74  Eigen::Quaterniond quat;
75  quat.w() = _q.W();
76  quat.x() = _q.X();
77  quat.y() = _q.Y();
78  quat.z() = _q.Z();
79 
80  return quat;
81  }
82 
86  inline Eigen::Isometry3d convert(const ignition::math::Pose3d &_pose)
87  {
88  Eigen::Isometry3d tf = Eigen::Isometry3d::Identity();
89  tf.translation() = convert(_pose.Pos());
90  tf.linear() = Eigen::Matrix3d(convert(_pose.Rot()));
91 
92  return tf;
93  }
94 
99  {
101  vec.X() = _v[0];
102  vec.Y() = _v[1];
103  vec.Z() = _v[2];
104 
105  return vec;
106  }
107 
112  const Eigen::AlignedBox3d &_b)
113  {
115  box.Min() = convert(_b.min());
116  box.Max() = convert(_b.max());
117 
118  return box;
119  }
120 
125  {
127  for (std::size_t i=0; i < 3; ++i)
128  {
129  for (std::size_t j=0; j < 3; ++j)
130  {
131  matrix(i, j) = _m(i, j);
132  }
133  }
134 
135  return matrix;
136  }
137 
142  {
144  quat.W() = _q.w();
145  quat.X() = _q.x();
146  quat.Y() = _q.y();
147  quat.Z() = _q.z();
148 
149  return quat;
150  }
151 
155  inline ignition::math::Pose3d convert(const Eigen::Isometry3d &_tf)
156  {
158  pose.Pos() = convert(Eigen::Vector3d(_tf.translation()));
159  pose.Rot() = convert(Eigen::Quaterniond(_tf.linear()));
160 
161  return pose;
162  }
163  }
164  }
165 }
166 
167 #endif
Quaternion< double > Quaterniond
Definition: Quaternion.hh:1082
const T & W() const
Get the w component.
Definition: Quaternion.hh:949
const T & Z() const
Get the z component.
Definition: Quaternion.hh:970
const Vector3d & Max() const
Get the maximum corner.
Encapsulates a position and rotation in three space.
Definition: Pose3.hh:34
Matrix3< double > Matrix3d
Definition: Matrix3.hh:545
const T & X() const
Get the x component.
Definition: Quaternion.hh:956
const T & Y() const
Get the y component.
Definition: Quaternion.hh:963
T X() const
Get the x value.
Definition: Vector3.hh:648
T Z() const
Get the z value.
Definition: Vector3.hh:662
T Y() const
Get the y value.
Definition: Vector3.hh:655
A 3x3 matrix class.
Definition: Matrix3.hh:40
const Quaternion< T > & Rot() const
Get the rotation.
Definition: Pose3.hh:418
The Vector3 class represents the generic vector containing 3 elements. Since it&#39;s commonly used to ke...
Definition: Vector3.hh:40
Vector3< double > Vector3d
Definition: Vector3.hh:757
const Vector3< T > & Pos() const
Get the position.
Definition: Pose3.hh:359
Definition: Angle.hh:42
const Vector3d & Min() const
Get the minimum corner.
A quaternion class.
Definition: Matrix3.hh:35
Eigen::Vector3d convert(const Vector3d &_v)
Convert from Vector3d to Eigen::Vector3d.
Definition: Conversions.hh:37
Mathematical representation of a box that is aligned along an X,Y,Z axis.
Definition: AxisAlignedBox.hh:42