- Aldebaran documentation |
- C++ Libraries |
- index

libalmath 2.8.7.4

- Main Page
- Related Pages
- Modules
- Namespaces
- Classes
- Files

- File List
- File Members

All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Groups Pages
almatheigen.h
Go to the documentation of this file.
1 /*
2 * Copyright (c) 2015 Aldebaran Robotics. All rights reserved.
3 * Use of this source code is governed by a BSD-style license that can be
4 * found in the COPYING file.
5 */
6
7 #pragma once
8 #ifndef _LIBALMATH_ALMATH_SCENEGRAPH_ALMATHEIGEN_H_
9 #define _LIBALMATH_ALMATH_SCENEGRAPH_ALMATHEIGEN_H_
10
11 #include <almath/types/altransform.h>
12 #include <almath/types/alposition3d.h>
13 #include <almath/types/alvelocity6d.h>
14 #include <boost/range/begin.hpp>
15 #include <boost/range/size.hpp>
16 #include <Eigen/Dense>
17
18 namespace AL {
19 namespace Math {
20
21 typedef Eigen::Matrix<float, 3, 4> Matrix34f;
22 typedef Eigen::Matrix<float, 3, 4, Eigen::RowMajor> Matrix34frm;
23 typedef Eigen::Matrix<float, 6, 1> Vector6f;
24
25 inline
26 Eigen::Map<const Matrix34frm> toEigenMapMatrix34(const Math::Transform &tr) {
27 return Eigen::Map<const Matrix34frm>(&tr.r1_c1);
28 }
29
30 inline Matrix34f toEigenMatrix34(const Math::Transform &tr) {
31 return Matrix34f(Eigen::Map<const Matrix34frm>(&tr.r1_c1));
32 }
33
34 inline Eigen::Matrix3f toEigenMatrix3(const Math::Transform &tr) {
35 return Eigen::Matrix3f(
36 Eigen::Map<const Matrix34frm>(&tr.r1_c1).block<3, 3>(0, 0));
37 }
38
39 template <typename T>
40 inline Eigen::Quaternion<T> toEigenQuaternion(const Math::Transform &tr) {
41 return Eigen::Quaternion<T>(
42 Eigen::Map<const Matrix34frm>(&tr.r1_c1).block<3, 3>(0, 0));
43 }
44
45 inline Eigen::Vector3f toEigenVector3(const Math::Position3D &v) {
46 return Eigen::Vector3f(Eigen::Map<const Eigen::Vector3f>(&v.x));
47 }
48
49 inline Eigen::AffineCompact3f toEigenAffineCompact3(
50 const Math::Transform &tr) {
51 return Eigen::AffineCompact3f(Eigen::Map<const Matrix34frm>(&tr.r1_c1));
52 }
53
54 template <typename Derived0>
55 void toALMathTransform(const Eigen::MatrixBase<Derived0> &in, Transform &out) {
56 EIGEN_STATIC_ASSERT(
57 (Eigen::internal::is_same<float, typename Derived0::Scalar>::value),
58 YOU_MIXED_DIFFERENT_NUMERIC_TYPES__YOU_NEED_TO_USE_THE_CAST_METHOD_OF_MATRIXBASE_TO_CAST_NUMERIC_TYPES_EXPLICITLY)
59 // will fail if in has the wrong size
60 Eigen::Map<Matrix34frm>(&out.r1_c1) = in;
61 }
62
63 template <typename Derived0>
64 Transform toALMathTransform(const Eigen::MatrixBase<Derived0> &m) {
65 Transform tr;
66 toALMathTransform(m, tr);
67 return tr;
68 }
69
70 template <int _Mode, int _Options>
71 void toALMathTransform(const Eigen::Transform<float, 3, _Mode, _Options> &in,
72 Transform &out) {
73 Eigen::Map<Matrix34frm>(&out.r1_c1) = in.affine();
74 }
75
76 template <int _Mode, int _Options>
77 Transform toALMathTransform(
78 const Eigen::Transform<float, 3, _Mode, _Options> &tr) {
79 Math::Transform out;
80 toALMathTransform(tr, out);
81 return out;
82 }
83
84 template <typename Derived0>
85 Position3D toALMathPosition3D(const Eigen::MatrixBase<Derived0> &in) {
86 EIGEN_STATIC_ASSERT(
87 (Eigen::internal::is_same<float, typename Derived0::Scalar>::value),
88 YOU_MIXED_DIFFERENT_NUMERIC_TYPES__YOU_NEED_TO_USE_THE_CAST_METHOD_OF_MATRIXBASE_TO_CAST_NUMERIC_TYPES_EXPLICITLY)
89 EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Derived0, 3, 1)
90 return Position3D(in[0], in[1], in[2]);
91 }
92
93 template <typename Derived0>
94 void toALMathVelocity6D(const Eigen::MatrixBase<Derived0> &in,
95 Velocity6D &out) {
96 EIGEN_STATIC_ASSERT(
97 (Eigen::internal::is_same<float, typename Derived0::Scalar>::value),
98 YOU_MIXED_DIFFERENT_NUMERIC_TYPES__YOU_NEED_TO_USE_THE_CAST_METHOD_OF_MATRIXBASE_TO_CAST_NUMERIC_TYPES_EXPLICITLY)
99 EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Derived0, 6, 1)
100 assert((in.rows() == 6) && (in.cols() == 1));
101 Eigen::Map<Vector6f>(&out.xd) = in;
102 }
103
104 template <typename Derived0>
105 Velocity6D toALMathVelocity6D(const Eigen::MatrixBase<Derived0> &in) {
106 EIGEN_STATIC_ASSERT(
107 (Eigen::internal::is_same<float, typename Derived0::Scalar>::value),
108 YOU_MIXED_DIFFERENT_NUMERIC_TYPES__YOU_NEED_TO_USE_THE_CAST_METHOD_OF_MATRIXBASE_TO_CAST_NUMERIC_TYPES_EXPLICITLY)
109 EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Derived0, 6, 1)
110 return Velocity6D(in[0], in[1], in[2], in[3], in[4], in[5]);
111 }
112
113 // Compute the average of a range of AL::Math::Transform.
114 // From: http://wiki.unity3d.com/index.php/Averaging_Quaternions_and_Vectors
115 // Throw if the range is empty.
116 template <class RandomAccessRange>
117 Math::Transform averageTransforms(const RandomAccessRange &range)
118 {
119 const auto numCumulated = boost::size(range);
120 if (numCumulated == 0)
121 {
122 throw std::runtime_error("Invalid empty range");
123 }
124 float averageFactor = 1.f / static_cast<float>(numCumulated);
125
126 const auto firstQuat = Math::toEigenQuaternion<float>(*boost::begin(range));
127 Eigen::Vector3f cumulatedPosition = Eigen::Vector3f::Zero();
128 Eigen::Quaternion<float> cumulatedQuaternion{0.f, 0.f, 0.f, 0.f};
129 for (const auto &tf : range)
130 {
131 // If the new quaternion is not close enough to the first rotation
132 // of the range of transforms, they cannot be averaged, so we consider -q
133 // instead of q.
134 auto newRotation = Math::toEigenQuaternion<float>(tf);
135 const auto areQuaternionsClose = newRotation.dot(firstQuat) >= 0.f;
136 if (!areQuaternionsClose)
137 {
138 // Consider -q instead of q.
139 newRotation.coeffs() = -newRotation.coeffs();
140 }
141 cumulatedQuaternion.coeffs() += newRotation.coeffs();
142 cumulatedPosition +=
143 Eigen::Map<const Math::Matrix34frm>(&tf.r1_c1).block<3, 1>(0, 3);
144 }
145 cumulatedQuaternion.normalize();
146 Math::Transform ret;
147 Eigen::Map<Matrix34frm> retm(&ret.r1_c1);
148 retm.block<3, 3>(0, 0) = cumulatedQuaternion.toRotationMatrix();
149 retm.block<3, 1>(0, 3) = cumulatedPosition * averageFactor;
150 return ret;
151 }
152 }
153 }
154 #endif
AL::Math::averageTransforms
Math::Transform averageTransforms(const RandomAccessRange &range)
Definition: almatheigen.h:117
AL::Math::Velocity6D
Create and play with a Velocity6D.
Definition: alvelocity6d.h:25
AL::Math::Matrix34frm
Eigen::Matrix< float, 3, 4, Eigen::RowMajor > Matrix34frm
Definition: almatheigen.h:22
AL::Math::toALMathTransform
void toALMathTransform(const Eigen::MatrixBase< Derived0 > &in, Transform &out)
Definition: almatheigen.h:55
alposition3d.h
AL::Math::toEigenMatrix34
Matrix34f toEigenMatrix34(const Math::Transform &tr)
Definition: almatheigen.h:30
AL::Math::Position3D::x
float x
Definition: alposition3d.h:26
AL::Math::toEigenMatrix3
Eigen::Matrix3f toEigenMatrix3(const Math::Transform &tr)
Definition: almatheigen.h:34
alvelocity6d.h
AL::Math::toALMathVelocity6D
void toALMathVelocity6D(const Eigen::MatrixBase< Derived0 > &in, Velocity6D &out)
Definition: almatheigen.h:94
AL::Math::toEigenQuaternion
Eigen::Quaternion< T > toEigenQuaternion(const Math::Transform &tr)
Definition: almatheigen.h:40
AL::Math::Position3D
Create and play with a Position3D.
Definition: alposition3d.h:24
altransform.h
AL::Math::Transform
A homogenous transformation matrix.
Definition: altransform.h:23
AL::Math::toEigenVector3
Eigen::Vector3f toEigenVector3(const Math::Position3D &v)
Definition: almatheigen.h:45
AL::Math::Velocity6D::xd
float xd
Definition: alvelocity6d.h:27
AL::Math::Matrix34f
Eigen::Matrix< float, 3, 4 > Matrix34f
Definition: almatheigen.h:21
AL::Math::toEigenMapMatrix34
Eigen::Map< const Matrix34frm > toEigenMapMatrix34(const Math::Transform &tr)
Definition: almatheigen.h:26
AL::Math::toALMathPosition3D
Position3D toALMathPosition3D(const Eigen::MatrixBase< Derived0 > &in)
Definition: almatheigen.h:85
AL::Math::toEigenAffineCompact3
Eigen::AffineCompact3f toEigenAffineCompact3(const Math::Transform &tr)
Definition: almatheigen.h:49
AL::Math::Vector6f
Eigen::Matrix< float, 6, 1 > Vector6f
Definition: almatheigen.h:23