Debian Codegraph @

unittests/unit/math/test_Math.cpp

raw
1 /*
2 * Copyright (c) 2011-2022, The DART development contributors
3 * All rights reserved.
4 *
5 * The list of contributors can be found at:
6 * https://github.com/dartsim/dart/blob/master/LICENSE
7 *
8 * This file is provided under the following "BSD-style" License:
9 * Redistribution and use in source and binary forms, with or
10 * without modification, are permitted provided that the following
11 * conditions are met:
12 * * Redistributions of source code must retain the above copyright
13 * notice, this list of conditions and the following disclaimer.
14 * * Redistributions in binary form must reproduce the above
15 * copyright notice, this list of conditions and the following
16 * disclaimer in the documentation and/or other materials provided
17 * with the distribution.
18 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND
19 * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES,
20 * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
21 * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
22 * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR
23 * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
24 * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
25 * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF
26 * USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
27 * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
28 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
29 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
30 * POSSIBILITY OF SUCH DAMAGE.
31 */
32
33 #include <iostream>
34
35 #include <gtest/gtest.h>
36
37 #include "dart/common/Stopwatch.hpp"
38 #include "dart/dynamics/RevoluteJoint.hpp"
39 #include "dart/dynamics/Skeleton.hpp"
40 #include "dart/math/Geometry.hpp"
41 #include "dart/math/Helpers.hpp"
42 #include "dart/simulation/World.hpp"
43
44 #include "TestHelpers.hpp"
45
46 using namespace dart;
47 using namespace common;
48 using namespace math;
49 using namespace dynamics;
50 using namespace simulation;
51
52 #define MATH_TOL 0.000001
53 #define MATH_EPS 0.000001
54
55 class EigenSE3
56 {
57 public:
58 explicit EigenSE3(const Eigen::Matrix4d& T) : mT(T) {}
59
60 /// \brief multiplication operator
61 inline EigenSE3 operator*(const EigenSE3& T) const
62 {
63 return EigenSE3(mT * T.mT);
64 }
65
66 /// \brief multiplication operator
67 inline const EigenSE3& operator*=(const EigenSE3& T)
68 {
69 mT *= T.mT;
70 return *this;
71 }
72
73 protected:
74 private:
75 Eigen::Matrix4d mT;
76 };
77
78 template <typename T>
79 EIGEN_DONT_INLINE void prod(const T& a, const T& b, T& c)
80 {
81 c = a * b;
82 }
83
84 template <typename T>
85 EIGEN_DONT_INLINE void inv(const T& a, T& b)
86 {
87 b = a.inverse();
88 }
89
90 EIGEN_DONT_INLINE
91 Isometry3d Inv2(const Isometry3d& I)
92 {
93 Eigen::Isometry3d ret;
94
95 ret(0, 0) = I(0, 0);
96 ret(1, 0) = I(0, 1);
97 ret(2, 0) = I(0, 2);
98
99 ret(0, 1) = I(1, 0);
100 ret(1, 1) = I(1, 1);
101 ret(2, 1) = I(1, 2);
102
103 ret(0, 2) = I(2, 0);
104 ret(1, 2) = I(2, 1);
105 ret(2, 2) = I(2, 2);
106
107 ret(0, 3) = -I(0, 0) * I(0, 3) - I(1, 0) * I(1, 3) - I(2, 0) * I(2, 3);
108 ret(1, 3) = -I(0, 1) * I(0, 3) - I(1, 1) * I(1, 3) - I(2, 1) * I(2, 3);
109 ret(2, 3) = -I(0, 2) * I(0, 3) - I(1, 2) * I(1, 3) - I(2, 2) * I(2, 3);
110
111 return ret;
112 }
113
114 template <typename T>
115 EIGEN_DONT_INLINE void inv_se3(const T& a, T& b)
116 {
117 b = Inv2(a);
118 }
119
120 EIGEN_DONT_INLINE
121 void concatenate(const Affine3d& A1, const Affine3d& A2, Affine3d& res)
122 {
123 res(0, 0) = A1(0, 0) * A2(0, 0) + A1(0, 1) * A2(1, 0) + A1(0, 2) * A2(2, 0);
124 res(1, 0) = A1(1, 0) * A2(0, 0) + A1(1, 1) * A2(1, 0) + A1(1, 2) * A2(2, 0);
125 res(2, 0) = A1(2, 0) * A2(0, 0) + A1(2, 1) * A2(1, 0) + A1(2, 2) * A2(2, 0);
126
127 res(0, 1) = A1(0, 0) * A2(0, 1) + A1(0, 1) * A2(1, 1) + A1(0, 2) * A2(2, 1);
128 res(1, 1) = A1(1, 0) * A2(0, 1) + A1(1, 1) * A2(1, 1) + A1(1, 2) * A2(2, 1);
129 res(2, 1) = A1(2, 0) * A2(0, 1) + A1(2, 1) * A2(1, 1) + A1(2, 2) * A2(2, 1);
130
131 res(0, 2) = A1(0, 0) * A2(0, 2) + A1(0, 1) * A2(1, 2) + A1(0, 2) * A2(2, 2);
132 res(1, 2) = A1(1, 0) * A2(0, 2) + A1(1, 1) * A2(1, 2) + A1(1, 2) * A2(2, 2);
133 res(2, 2) = A1(2, 0) * A2(0, 2) + A1(2, 1) * A2(1, 2) + A1(2, 2) * A2(2, 2);
134
135 res(0, 3) = A1(0, 0) * A2(0, 3) + A1(0, 1) * A2(1, 3) + A1(0, 2) * A2(2, 3)
136 + A1(0, 3);
137 res(1, 3) = A1(1, 0) * A2(0, 3) + A1(1, 1) * A2(1, 3) + A1(1, 2) * A2(2, 3)
138 + A1(1, 3);
139 res(2, 3) = A1(2, 0) * A2(0, 3) + A1(2, 1) * A2(1, 3) + A1(2, 2) * A2(2, 3)
140 + A1(2, 3);
141
142 res(3, 0) = 0.0;
143 res(3, 1) = 0.0;
144 res(3, 2) = 0.0;
145 res(3, 3) = 1.0;
146 }
147
148 EIGEN_DONT_INLINE
149 void concatenate2(const Affine3d& A1, const Affine3d& A2, Affine3d& res)
150 {
151 res(0, 0) = A1(0, 0) * A2(0, 0) + A1(0, 1) * A2(1, 0) + A1(0, 2) * A2(2, 0);
152 res(1, 0) = A1(1, 0) * A2(0, 0) + A1(1, 1) * A2(1, 0) + A1(1, 2) * A2(2, 0);
153 res(2, 0) = A1(2, 0) * A2(0, 0) + A1(2, 1) * A2(1, 0) + A1(2, 2) * A2(2, 0);
154
155 res(0, 1) = A1(0, 0) * A2(0, 1) + A1(0, 1) * A2(1, 1) + A1(0, 2) * A2(2, 1);
156 res(1, 1) = A1(1, 0) * A2(0, 1) + A1(1, 1) * A2(1, 1) + A1(1, 2) * A2(2, 1);
157 res(2, 1) = A1(2, 0) * A2(0, 1) + A1(2, 1) * A2(1, 1) + A1(2, 2) * A2(2, 1);
158
159 res(0, 2) = A1(0, 0) * A2(0, 2) + A1(0, 1) * A2(1, 2) + A1(0, 2) * A2(2, 2);
160 res(1, 2) = A1(1, 0) * A2(0, 2) + A1(1, 1) * A2(1, 2) + A1(1, 2) * A2(2, 2);
161 res(2, 2) = A1(2, 0) * A2(0, 2) + A1(2, 1) * A2(1, 2) + A1(2, 2) * A2(2, 2);
162
163 res(0, 3) = A1(0, 0) * A2(0, 3) + A1(0, 1) * A2(1, 3) + A1(0, 2) * A2(2, 3)
164 + A1(0, 3);
165 res(1, 3) = A1(1, 0) * A2(0, 3) + A1(1, 1) * A2(1, 3) + A1(1, 2) * A2(2, 3)
166 + A1(1, 3);
167 res(2, 3) = A1(2, 0) * A2(0, 3) + A1(2, 1) * A2(1, 3) + A1(2, 2) * A2(2, 3)
168 + A1(2, 3);
169
170 res(3, 0) = 0.0;
171 res(3, 1) = 0.0;
172 res(3, 2) = 0.0;
173 res(3, 3) = 1.0;
174 }
175
176 EIGEN_DONT_INLINE
177 Matrix<double, 6, 1> Ad_Affine3d_1(
178 const Affine3d& T, const Matrix<double, 6, 1>& s)
179 {
180 //--------------------------------------------------------------------------
181 // w' = R * w
182 // v' = r x R * w + R * v
183 //--------------------------------------------------------------------------
184 double Rw[3]
185 = {T.data()[0] * s.data()[0] + T.data()[4] * s.data()[1]
186 + T.data()[8] * s.data()[2],
187 T.data()[1] * s.data()[0] + T.data()[5] * s.data()[1]
188 + T.data()[9] * s.data()[2],
189 T.data()[2] * s.data()[0] + T.data()[6] * s.data()[1]
190 + T.data()[10] * s.data()[2]};
191
192 Matrix<double, 6, 1> ret;
193
194 ret << Rw[0], Rw[1], Rw[2],
195 T.data()[13] * Rw[2] - T.data()[14] * Rw[1] + T.data()[0] * s.data()[3]
196 + T.data()[4] * s.data()[4] + T.data()[8] * s.data()[5],
197 T.data()[14] * Rw[0] - T.data()[12] * Rw[2] + T.data()[1] * s.data()[3]
198 + T.data()[5] * s.data()[4] + T.data()[9] * s.data()[5],
199 T.data()[12] * Rw[1] - T.data()[13] * Rw[0] + T.data()[2] * s.data()[3]
200 + T.data()[6] * s.data()[4] + T.data()[10] * s.data()[5];
201
202 return ret;
203 }
204
205 EIGEN_DONT_INLINE
206 Matrix<double, 6, 1> Ad_Affine3d_2(
207 const Affine3d& T, const Matrix<double, 6, 1>& s)
208 {
209 //--------------------------------------------------------------------------
210 // w' = R * w
211 // v' = r x R * w + R * v
212 //--------------------------------------------------------------------------
213 double Rw[3]
214 = {T(0, 0) * s[0] + T(0, 1) * s[1] + T(0, 2) * s[2],
215 T(1, 0) * s[0] + T(1, 1) * s[1] + T(1, 2) * s[2],
216 T(2, 0) * s[0] + T(2, 1) * s[1] + T(2, 2) * s[2]};
217
218 Matrix<double, 6, 1> ret;
219
220 ret << Rw[0], Rw[1], Rw[2],
221 T(1, 3) * Rw[2] - T(2, 3) * Rw[1] + T(0, 0) * s[3] + T(0, 1) * s[4]
222 + T(0, 2) * s[5],
223 T(2, 3) * Rw[0] - T(0, 3) * Rw[2] + T(1, 0) * s[3] + T(1, 1) * s[4]
224 + T(1, 2) * s[5],
225 T(0, 3) * Rw[1] - T(1, 3) * Rw[0] + T(2, 0) * s[3] + T(2, 1) * s[4]
226 + T(2, 2) * s[5];
227
228 return ret;
229 }
230
231 EIGEN_DONT_INLINE
232 Matrix<double, 6, 1> Ad_Affine3d_3(
233 const Affine3d& T, const Matrix<double, 6, 1>& s)
234 {
235 //--------------------------------------------------------------------------
236 // w' = R * w
237 // v' = r x R * w + R * v
238 //--------------------------------------------------------------------------
239
240 Matrix<double, 6, 1> ret;
241
242 ret.topLeftCorner<3, 1>() = T.linear() * s.topLeftCorner<3, 1>();
243 ret.bottomLeftCorner<3, 1>()
244 = T.translation().cross(T.linear() * s.topLeftCorner<3, 1>())
245 + T.linear() * s.bottomLeftCorner<3, 1>();
246
247 return ret;
248 }
249
250 EIGEN_DONT_INLINE
251 Matrix<double, 6, 1> Ad_Isometry3d(
252 const Isometry3d& T, const Matrix<double, 6, 1>& s)
253 {
254 //--------------------------------------------------------------------------
255 // w' = R * w
256 // v' = r x R * w + R * v
257 //--------------------------------------------------------------------------
258 double Rw[3]
259 = {T(0, 0) * s[0] + T(0, 1) * s[1] + T(0, 2) * s[2],
260 T(1, 0) * s[0] + T(1, 1) * s[1] + T(1, 2) * s[2],
261 T(2, 0) * s[0] + T(2, 1) * s[1] + T(2, 2) * s[2]};
262
263 Matrix<double, 6, 1> ret;
264
265 ret << Rw[0], Rw[1], Rw[2],
266 T(1, 3) * Rw[2] - T(2, 3) * Rw[1] + T(0, 0) * s[3] + T(0, 1) * s[4]
267 + T(0, 2) * s[5],
268 T(2, 3) * Rw[0] - T(0, 3) * Rw[2] + T(1, 0) * s[3] + T(1, 1) * s[4]
269 + T(1, 2) * s[5],
270 T(0, 3) * Rw[1] - T(1, 3) * Rw[0] + T(2, 0) * s[3] + T(2, 1) * s[4]
271 + T(2, 2) * s[5];
272
273 return ret;
274 }
275
276 EIGEN_DONT_INLINE
277 Matrix<double, 6, 1> Ad_Matrix4d(
278 const Matrix4d& T, const Matrix<double, 6, 1>& s)
279 {
280 //--------------------------------------------------------------------------
281 // w' = R * w
282 // v' = r x R * w + R * v
283 //--------------------------------------------------------------------------
284 double Rw[3]
285 = {T(0, 0) * s[0] + T(0, 1) * s[1] + T(0, 2) * s[2],
286 T(1, 0) * s[0] + T(1, 1) * s[1] + T(1, 2) * s[2],
287 T(2, 0) * s[0] + T(2, 1) * s[1] + T(2, 2) * s[2]};
288
289 Matrix<double, 6, 1> ret;
290
291 ret << Rw[0], Rw[1], Rw[2],
292 T(1, 3) * Rw[2] - T(2, 3) * Rw[1] + T(0, 0) * s[3] + T(0, 1) * s[4]
293 + T(0, 2) * s[5],
294 T(2, 3) * Rw[0] - T(0, 3) * Rw[2] + T(1, 0) * s[3] + T(1, 1) * s[4]
295 + T(1, 2) * s[5],
296 T(0, 3) * Rw[1] - T(1, 3) * Rw[0] + T(2, 0) * s[3] + T(2, 1) * s[4]
297 + T(2, 2) * s[5];
298
299 return ret;
300 }
301
302 EIGEN_DONT_INLINE
303 Matrix<double, 6, 6> Transform_Affine3d(
304 const Affine3d& T, const Matrix<double, 6, 6>& AI)
305 {
306 // operation count: multiplication = 186, addition = 117, subtract = 21
307
308 Matrix<double, 6, 6> ret;
309
310 double d0 = AI(0, 3) + T(2, 3) * AI(3, 4) - T(1, 3) * AI(3, 5);
311 double d1 = AI(1, 3) - T(2, 3) * AI(3, 3) + T(0, 3) * AI(3, 5);
312 double d2 = AI(2, 3) + T(1, 3) * AI(3, 3) - T(0, 3) * AI(3, 4);
313 double d3 = AI(0, 4) + T(2, 3) * AI(4, 4) - T(1, 3) * AI(4, 5);
314 double d4 = AI(1, 4) - T(2, 3) * AI(3, 4) + T(0, 3) * AI(4, 5);
315 double d5 = AI(2, 4) + T(1, 3) * AI(3, 4) - T(0, 3) * AI(4, 4);
316 double d6 = AI(0, 5) + T(2, 3) * AI(4, 5) - T(1, 3) * AI(5, 5);
317 double d7 = AI(1, 5) - T(2, 3) * AI(3, 5) + T(0, 3) * AI(5, 5);
318 double d8 = AI(2, 5) + T(1, 3) * AI(3, 5) - T(0, 3) * AI(4, 5);
319 double e0 = AI(0, 0) + T(2, 3) * AI(0, 4) - T(1, 3) * AI(0, 5) + d3 * T(2, 3)
320 - d6 * T(1, 3);
321 double e3 = AI(0, 1) + T(2, 3) * AI(1, 4) - T(1, 3) * AI(1, 5) - d0 * T(2, 3)
322 + d6 * T(0, 3);
323 double e4 = AI(1, 1) - T(2, 3) * AI(1, 3) + T(0, 3) * AI(1, 5) - d1 * T(2, 3)
324 + d7 * T(0, 3);
325 double e6 = AI(0, 2) + T(2, 3) * AI(2, 4) - T(1, 3) * AI(2, 5) + d0 * T(1, 3)
326 - d3 * T(0, 3);
327 double e7 = AI(1, 2) - T(2, 3) * AI(2, 3) + T(0, 3) * AI(2, 5) + d1 * T(1, 3)
328 - d4 * T(0, 3);
329 double e8 = AI(2, 2) + T(1, 3) * AI(2, 3) - T(0, 3) * AI(2, 4) + d2 * T(1, 3)
330 - d5 * T(0, 3);
331 double f0 = T(0, 0) * e0 + T(1, 0) * e3 + T(2, 0) * e6;
332 double f1 = T(0, 0) * e3 + T(1, 0) * e4 + T(2, 0) * e7;
333 double f2 = T(0, 0) * e6 + T(1, 0) * e7 + T(2, 0) * e8;
334 double f3 = T(0, 0) * d0 + T(1, 0) * d1 + T(2, 0) * d2;
335 double f4 = T(0, 0) * d3 + T(1, 0) * d4 + T(2, 0) * d5;
336 double f5 = T(0, 0) * d6 + T(1, 0) * d7 + T(2, 0) * d8;
337 double f6 = T(0, 1) * e0 + T(1, 1) * e3 + T(2, 1) * e6;
338 double f7 = T(0, 1) * e3 + T(1, 1) * e4 + T(2, 1) * e7;
339 double f8 = T(0, 1) * e6 + T(1, 1) * e7 + T(2, 1) * e8;
340 double g0 = T(0, 1) * d0 + T(1, 1) * d1 + T(2, 1) * d2;
341 double g1 = T(0, 1) * d3 + T(1, 1) * d4 + T(2, 1) * d5;
342 double g2 = T(0, 1) * d6 + T(1, 1) * d7 + T(2, 1) * d8;
343 double g3 = T(0, 2) * d0 + T(1, 2) * d1 + T(2, 2) * d2;
344 double g4 = T(0, 2) * d3 + T(1, 2) * d4 + T(2, 2) * d5;
345 double g5 = T(0, 2) * d6 + T(1, 2) * d7 + T(2, 2) * d8;
346 double h0 = T(0, 0) * AI(3, 3) + T(1, 0) * AI(3, 4) + T(2, 0) * AI(3, 5);
347 double h1 = T(0, 0) * AI(3, 4) + T(1, 0) * AI(4, 4) + T(2, 0) * AI(4, 5);
348 double h2 = T(0, 0) * AI(3, 5) + T(1, 0) * AI(4, 5) + T(2, 0) * AI(5, 5);
349 double h3 = T(0, 1) * AI(3, 3) + T(1, 1) * AI(3, 4) + T(2, 1) * AI(3, 5);
350 double h4 = T(0, 1) * AI(3, 4) + T(1, 1) * AI(4, 4) + T(2, 1) * AI(4, 5);
351 double h5 = T(0, 1) * AI(3, 5) + T(1, 1) * AI(4, 5) + T(2, 1) * AI(5, 5);
352
353 ret(0, 0) = f0 * T(0, 0) + f1 * T(1, 0) + f2 * T(2, 0);
354 ret(0, 1) = f0 * T(0, 1) + f1 * T(1, 1) + f2 * T(2, 1);
355 ret(0, 2) = f0 * T(0, 2) + f1 * T(1, 2) + f2 * T(2, 2);
356 ret(0, 3) = f3 * T(0, 0) + f4 * T(1, 0) + f5 * T(2, 0);
357 ret(0, 4) = f3 * T(0, 1) + f4 * T(1, 1) + f5 * T(2, 1);
358 ret(0, 5) = f3 * T(0, 2) + f4 * T(1, 2) + f5 * T(2, 2);
359 ret(1, 1) = f6 * T(0, 1) + f7 * T(1, 1) + f8 * T(2, 1);
360 ret(1, 2) = f6 * T(0, 2) + f7 * T(1, 2) + f8 * T(2, 2);
361 ret(1, 3) = g0 * T(0, 0) + g1 * T(1, 0) + g2 * T(2, 0);
362 ret(1, 4) = g0 * T(0, 1) + g1 * T(1, 1) + g2 * T(2, 1);
363 ret(1, 5) = g0 * T(0, 2) + g1 * T(1, 2) + g2 * T(2, 2);
364 ret(2, 2) = (T(0, 2) * e0 + T(1, 2) * e3 + T(2, 2) * e6) * T(0, 2)
365 + (T(0, 2) * e3 + T(1, 2) * e4 + T(2, 2) * e7) * T(1, 2)
366 + (T(0, 2) * e6 + T(1, 2) * e7 + T(2, 2) * e8) * T(2, 2);
367 ret(2, 3) = g3 * T(0, 0) + g4 * T(1, 0) + g5 * T(2, 0);
368 ret(2, 4) = g3 * T(0, 1) + g4 * T(1, 1) + g5 * T(2, 1);
369 ret(2, 5) = g3 * T(0, 2) + g4 * T(1, 2) + g5 * T(2, 2);
370 ret(3, 3) = h0 * T(0, 0) + h1 * T(1, 0) + h2 * T(2, 0);
371 ret(3, 4) = h0 * T(0, 1) + h1 * T(1, 1) + h2 * T(2, 1);
372 ret(3, 5) = h0 * T(0, 2) + h1 * T(1, 2) + h2 * T(2, 2);
373 ret(4, 4) = h3 * T(0, 1) + h4 * T(1, 1) + h5 * T(2, 1);
374 ret(4, 5) = h3 * T(0, 2) + h4 * T(1, 2) + h5 * T(2, 2);
375 ret(5, 5)
376 = (T(0, 2) * AI(3, 3) + T(1, 2) * AI(3, 4) + T(2, 2) * AI(3, 5)) * T(0, 2)
377 + (T(0, 2) * AI(3, 4) + T(1, 2) * AI(4, 4) + T(2, 2) * AI(4, 5))
378 * T(1, 2)
379 + (T(0, 2) * AI(3, 5) + T(1, 2) * AI(4, 5) + T(2, 2) * AI(5, 5))
380 * T(2, 2);
381
382 ret.triangularView<StrictlyLower>() = ret.transpose();
383
384 return ret;
385 }
386
387 EIGEN_DONT_INLINE
388 Matrix<double, 6, 6> Transform_Isometry3d(
389 const Isometry3d& T, const Matrix<double, 6, 6>& AI)
390 {
391 // operation count: multiplication = 186, addition = 117, subtract = 21
392
393 Matrix<double, 6, 6> ret;
394
395 double d0 = AI(0, 3) + T(2, 3) * AI(3, 4) - T(1, 3) * AI(3, 5);
396 double d1 = AI(1, 3) - T(2, 3) * AI(3, 3) + T(0, 3) * AI(3, 5);
397 double d2 = AI(2, 3) + T(1, 3) * AI(3, 3) - T(0, 3) * AI(3, 4);
398 double d3 = AI(0, 4) + T(2, 3) * AI(4, 4) - T(1, 3) * AI(4, 5);
399 double d4 = AI(1, 4) - T(2, 3) * AI(3, 4) + T(0, 3) * AI(4, 5);
400 double d5 = AI(2, 4) + T(1, 3) * AI(3, 4) - T(0, 3) * AI(4, 4);
401 double d6 = AI(0, 5) + T(2, 3) * AI(4, 5) - T(1, 3) * AI(5, 5);
402 double d7 = AI(1, 5) - T(2, 3) * AI(3, 5) + T(0, 3) * AI(5, 5);
403 double d8 = AI(2, 5) + T(1, 3) * AI(3, 5) - T(0, 3) * AI(4, 5);
404 double e0 = AI(0, 0) + T(2, 3) * AI(0, 4) - T(1, 3) * AI(0, 5) + d3 * T(2, 3)
405 - d6 * T(1, 3);
406 double e3 = AI(0, 1) + T(2, 3) * AI(1, 4) - T(1, 3) * AI(1, 5) - d0 * T(2, 3)
407 + d6 * T(0, 3);
408 double e4 = AI(1, 1) - T(2, 3) * AI(1, 3) + T(0, 3) * AI(1, 5) - d1 * T(2, 3)
409 + d7 * T(0, 3);
410 double e6 = AI(0, 2) + T(2, 3) * AI(2, 4) - T(1, 3) * AI(2, 5) + d0 * T(1, 3)
411 - d3 * T(0, 3);
412 double e7 = AI(1, 2) - T(2, 3) * AI(2, 3) + T(0, 3) * AI(2, 5) + d1 * T(1, 3)
413 - d4 * T(0, 3);
414 double e8 = AI(2, 2) + T(1, 3) * AI(2, 3) - T(0, 3) * AI(2, 4) + d2 * T(1, 3)
415 - d5 * T(0, 3);
416 double f0 = T(0, 0) * e0 + T(1, 0) * e3 + T(2, 0) * e6;
417 double f1 = T(0, 0) * e3 + T(1, 0) * e4 + T(2, 0) * e7;
418 double f2 = T(0, 0) * e6 + T(1, 0) * e7 + T(2, 0) * e8;
419 double f3 = T(0, 0) * d0 + T(1, 0) * d1 + T(2, 0) * d2;
420 double f4 = T(0, 0) * d3 + T(1, 0) * d4 + T(2, 0) * d5;
421 double f5 = T(0, 0) * d6 + T(1, 0) * d7 + T(2, 0) * d8;
422 double f6 = T(0, 1) * e0 + T(1, 1) * e3 + T(2, 1) * e6;
423 double f7 = T(0, 1) * e3 + T(1, 1) * e4 + T(2, 1) * e7;
424 double f8 = T(0, 1) * e6 + T(1, 1) * e7 + T(2, 1) * e8;
425 double g0 = T(0, 1) * d0 + T(1, 1) * d1 + T(2, 1) * d2;
426 double g1 = T(0, 1) * d3 + T(1, 1) * d4 + T(2, 1) * d5;
427 double g2 = T(0, 1) * d6 + T(1, 1) * d7 + T(2, 1) * d8;
428 double g3 = T(0, 2) * d0 + T(1, 2) * d1 + T(2, 2) * d2;
429 double g4 = T(0, 2) * d3 + T(1, 2) * d4 + T(2, 2) * d5;
430 double g5 = T(0, 2) * d6 + T(1, 2) * d7 + T(2, 2) * d8;
431 double h0 = T(0, 0) * AI(3, 3) + T(1, 0) * AI(3, 4) + T(2, 0) * AI(3, 5);
432 double h1 = T(0, 0) * AI(3, 4) + T(1, 0) * AI(4, 4) + T(2, 0) * AI(4, 5);
433 double h2 = T(0, 0) * AI(3, 5) + T(1, 0) * AI(4, 5) + T(2, 0) * AI(5, 5);
434 double h3 = T(0, 1) * AI(3, 3) + T(1, 1) * AI(3, 4) + T(2, 1) * AI(3, 5);
435 double h4 = T(0, 1) * AI(3, 4) + T(1, 1) * AI(4, 4) + T(2, 1) * AI(4, 5);
436 double h5 = T(0, 1) * AI(3, 5) + T(1, 1) * AI(4, 5) + T(2, 1) * AI(5, 5);
437
438 ret(0, 0) = f0 * T(0, 0) + f1 * T(1, 0) + f2 * T(2, 0);
439 ret(0, 1) = f0 * T(0, 1) + f1 * T(1, 1) + f2 * T(2, 1);
440 ret(0, 2) = f0 * T(0, 2) + f1 * T(1, 2) + f2 * T(2, 2);
441 ret(0, 3) = f3 * T(0, 0) + f4 * T(1, 0) + f5 * T(2, 0);
442 ret(0, 4) = f3 * T(0, 1) + f4 * T(1, 1) + f5 * T(2, 1);
443 ret(0, 5) = f3 * T(0, 2) + f4 * T(1, 2) + f5 * T(2, 2);
444 ret(1, 1) = f6 * T(0, 1) + f7 * T(1, 1) + f8 * T(2, 1);
445 ret(1, 2) = f6 * T(0, 2) + f7 * T(1, 2) + f8 * T(2, 2);
446 ret(1, 3) = g0 * T(0, 0) + g1 * T(1, 0) + g2 * T(2, 0);
447 ret(1, 4) = g0 * T(0, 1) + g1 * T(1, 1) + g2 * T(2, 1);
448 ret(1, 5) = g0 * T(0, 2) + g1 * T(1, 2) + g2 * T(2, 2);
449 ret(2, 2) = (T(0, 2) * e0 + T(1, 2) * e3 + T(2, 2) * e6) * T(0, 2)
450 + (T(0, 2) * e3 + T(1, 2) * e4 + T(2, 2) * e7) * T(1, 2)
451 + (T(0, 2) * e6 + T(1, 2) * e7 + T(2, 2) * e8) * T(2, 2);
452 ret(2, 3) = g3 * T(0, 0) + g4 * T(1, 0) + g5 * T(2, 0);
453 ret(2, 4) = g3 * T(0, 1) + g4 * T(1, 1) + g5 * T(2, 1);
454 ret(2, 5) = g3 * T(0, 2) + g4 * T(1, 2) + g5 * T(2, 2);
455 ret(3, 3) = h0 * T(0, 0) + h1 * T(1, 0) + h2 * T(2, 0);
456 ret(3, 4) = h0 * T(0, 1) + h1 * T(1, 1) + h2 * T(2, 1);
457 ret(3, 5) = h0 * T(0, 2) + h1 * T(1, 2) + h2 * T(2, 2);
458 ret(4, 4) = h3 * T(0, 1) + h4 * T(1, 1) + h5 * T(2, 1);
459 ret(4, 5) = h3 * T(0, 2) + h4 * T(1, 2) + h5 * T(2, 2);
460 ret(5, 5)
461 = (T(0, 2) * AI(3, 3) + T(1, 2) * AI(3, 4) + T(2, 2) * AI(3, 5)) * T(0, 2)
462 + (T(0, 2) * AI(3, 4) + T(1, 2) * AI(4, 4) + T(2, 2) * AI(4, 5))
463 * T(1, 2)
464 + (T(0, 2) * AI(3, 5) + T(1, 2) * AI(4, 5) + T(2, 2) * AI(5, 5))
465 * T(2, 2);
466
467 ret.triangularView<StrictlyLower>() = ret.transpose();
468
469 return ret;
470 }
471
472 EIGEN_DONT_INLINE
473 Matrix<double, 6, 6> Transform_Isometry3d_2(
474 const Isometry3d& T, const Matrix<double, 6, 6>& AI)
475 {
476 // operation count: multiplication = 186, addition = 117, subtract = 21
477
478 Matrix<double, 6, 6> ret;
479
480 double d0 = AI(0, 3) + T(2, 3) * AI(3, 4) - T(1, 3) * AI(3, 5);
481 double d1 = AI(1, 3) - T(2, 3) * AI(3, 3) + T(0, 3) * AI(3, 5);
482 double d2 = AI(2, 3) + T(1, 3) * AI(3, 3) - T(0, 3) * AI(3, 4);
483 double d3 = AI(0, 4) + T(2, 3) * AI(4, 4) - T(1, 3) * AI(4, 5);
484 double d4 = AI(1, 4) - T(2, 3) * AI(3, 4) + T(0, 3) * AI(4, 5);
485 double d5 = AI(2, 4) + T(1, 3) * AI(3, 4) - T(0, 3) * AI(4, 4);
486 double d6 = AI(0, 5) + T(2, 3) * AI(4, 5) - T(1, 3) * AI(5, 5);
487 double d7 = AI(1, 5) - T(2, 3) * AI(3, 5) + T(0, 3) * AI(5, 5);
488 double d8 = AI(2, 5) + T(1, 3) * AI(3, 5) - T(0, 3) * AI(4, 5);
489 double e0 = AI(0, 0) + T(2, 3) * AI(0, 4) - T(1, 3) * AI(0, 5) + d3 * T(2, 3)
490 - d6 * T(1, 3);
491 double e3 = AI(0, 1) + T(2, 3) * AI(1, 4) - T(1, 3) * AI(1, 5) - d0 * T(2, 3)
492 + d6 * T(0, 3);
493 double e4 = AI(1, 1) - T(2, 3) * AI(1, 3) + T(0, 3) * AI(1, 5) - d1 * T(2, 3)
494 + d7 * T(0, 3);
495 double e6 = AI(0, 2) + T(2, 3) * AI(2, 4) - T(1, 3) * AI(2, 5) + d0 * T(1, 3)
496 - d3 * T(0, 3);
497 double e7 = AI(1, 2) - T(2, 3) * AI(2, 3) + T(0, 3) * AI(2, 5) + d1 * T(1, 3)
498 - d4 * T(0, 3);
499 double e8 = AI(2, 2) + T(1, 3) * AI(2, 3) - T(0, 3) * AI(2, 4) + d2 * T(1, 3)
500 - d5 * T(0, 3);
501 double f0 = T(0, 0) * e0 + T(1, 0) * e3 + T(2, 0) * e6;
502 double f1 = T(0, 0) * e3 + T(1, 0) * e4 + T(2, 0) * e7;
503 double f2 = T(0, 0) * e6 + T(1, 0) * e7 + T(2, 0) * e8;
504 double f3 = T(0, 0) * d0 + T(1, 0) * d1 + T(2, 0) * d2;
505 double f4 = T(0, 0) * d3 + T(1, 0) * d4 + T(2, 0) * d5;
506 double f5 = T(0, 0) * d6 + T(1, 0) * d7 + T(2, 0) * d8;
507 double f6 = T(0, 1) * e0 + T(1, 1) * e3 + T(2, 1) * e6;
508 double f7 = T(0, 1) * e3 + T(1, 1) * e4 + T(2, 1) * e7;
509 double f8 = T(0, 1) * e6 + T(1, 1) * e7 + T(2, 1) * e8;
510 double g0 = T(0, 1) * d0 + T(1, 1) * d1 + T(2, 1) * d2;
511 double g1 = T(0, 1) * d3 + T(1, 1) * d4 + T(2, 1) * d5;
512 double g2 = T(0, 1) * d6 + T(1, 1) * d7 + T(2, 1) * d8;
513 double g3 = T(0, 2) * d0 + T(1, 2) * d1 + T(2, 2) * d2;
514 double g4 = T(0, 2) * d3 + T(1, 2) * d4 + T(2, 2) * d5;
515 double g5 = T(0, 2) * d6 + T(1, 2) * d7 + T(2, 2) * d8;
516 double h0 = T(0, 0) * AI(3, 3) + T(1, 0) * AI(3, 4) + T(2, 0) * AI(3, 5);
517 double h1 = T(0, 0) * AI(3, 4) + T(1, 0) * AI(4, 4) + T(2, 0) * AI(4, 5);
518 double h2 = T(0, 0) * AI(3, 5) + T(1, 0) * AI(4, 5) + T(2, 0) * AI(5, 5);
519 double h3 = T(0, 1) * AI(3, 3) + T(1, 1) * AI(3, 4) + T(2, 1) * AI(3, 5);
520 double h4 = T(0, 1) * AI(3, 4) + T(1, 1) * AI(4, 4) + T(2, 1) * AI(4, 5);
521 double h5 = T(0, 1) * AI(3, 5) + T(1, 1) * AI(4, 5) + T(2, 1) * AI(5, 5);
522
523 ret(0, 0) = f0 * T(0, 0) + f1 * T(1, 0) + f2 * T(2, 0);
524 ret(0, 1) = f0 * T(0, 1) + f1 * T(1, 1) + f2 * T(2, 1);
525 ret(0, 2) = f0 * T(0, 2) + f1 * T(1, 2) + f2 * T(2, 2);
526 ret(0, 3) = f3 * T(0, 0) + f4 * T(1, 0) + f5 * T(2, 0);
527 ret(0, 4) = f3 * T(0, 1) + f4 * T(1, 1) + f5 * T(2, 1);
528 ret(0, 5) = f3 * T(0, 2) + f4 * T(1, 2) + f5 * T(2, 2);
529 ret(1, 0) = ret(0, 1);
530 ret(1, 1) = f6 * T(0, 1) + f7 * T(1, 1) + f8 * T(2, 1);
531 ret(1, 2) = f6 * T(0, 2) + f7 * T(1, 2) + f8 * T(2, 2);
532 ret(1, 3) = g0 * T(0, 0) + g1 * T(1, 0) + g2 * T(2, 0);
533 ret(1, 4) = g0 * T(0, 1) + g1 * T(1, 1) + g2 * T(2, 1);
534 ret(1, 5) = g0 * T(0, 2) + g1 * T(1, 2) + g2 * T(2, 2);
535 ret(2, 0) = ret(0, 2);
536 ret(2, 1) = ret(1, 2);
537 ret(2, 2) = (T(0, 2) * e0 + T(1, 2) * e3 + T(2, 2) * e6) * T(0, 2)
538 + (T(0, 2) * e3 + T(1, 2) * e4 + T(2, 2) * e7) * T(1, 2)
539 + (T(0, 2) * e6 + T(1, 2) * e7 + T(2, 2) * e8) * T(2, 2);
540 ret(2, 3) = g3 * T(0, 0) + g4 * T(1, 0) + g5 * T(2, 0);
541 ret(2, 4) = g3 * T(0, 1) + g4 * T(1, 1) + g5 * T(2, 1);
542 ret(2, 5) = g3 * T(0, 2) + g4 * T(1, 2) + g5 * T(2, 2);
543 ret(3, 0) = ret(0, 3);
544 ret(3, 1) = ret(1, 3);
545 ret(3, 2) = ret(2, 3);
546 ret(3, 3) = h0 * T(0, 0) + h1 * T(1, 0) + h2 * T(2, 0);
547 ret(3, 4) = h0 * T(0, 1) + h1 * T(1, 1) + h2 * T(2, 1);
548 ret(3, 5) = h0 * T(0, 2) + h1 * T(1, 2) + h2 * T(2, 2);
549 ret(4, 0) = ret(0, 4);
550 ret(4, 1) = ret(1, 4);
551 ret(4, 2) = ret(2, 4);
552 ret(4, 3) = ret(3, 4);
553 ret(4, 4) = h3 * T(0, 1) + h4 * T(1, 1) + h5 * T(2, 1);
554 ret(4, 5) = h3 * T(0, 2) + h4 * T(1, 2) + h5 * T(2, 2);
555 ret(5, 0) = ret(0, 5);
556 ret(5, 1) = ret(1, 5);
557 ret(5, 2) = ret(2, 5);
558 ret(5, 3) = ret(3, 5);
559 ret(5, 4) = ret(4, 5);
560 ret(5, 5)
561 = (T(0, 2) * AI(3, 3) + T(1, 2) * AI(3, 4) + T(2, 2) * AI(3, 5)) * T(0, 2)
562 + (T(0, 2) * AI(3, 4) + T(1, 2) * AI(4, 4) + T(2, 2) * AI(4, 5))
563 * T(1, 2)
564 + (T(0, 2) * AI(3, 5) + T(1, 2) * AI(4, 5) + T(2, 2) * AI(5, 5))
565 * T(2, 2);
566
567 return ret;
568 }
569
570 EIGEN_DONT_INLINE
571 Matrix<double, 6, 6> Transform_Isometry3d_3(
572 const Isometry3d& T, const Matrix<double, 6, 6>& AI)
573 {
574 // operation count: multiplication = 186, addition = 117, subtract = 21
575
576 Matrix<double, 6, 6> ret;
577
578 double d0 = AI(0, 3) + T(2, 3) * AI(3, 4) - T(1, 3) * AI(3, 5);
579 double d1 = AI(1, 3) - T(2, 3) * AI(3, 3) + T(0, 3) * AI(3, 5);
580 double d2 = AI(2, 3) + T(1, 3) * AI(3, 3) - T(0, 3) * AI(3, 4);
581 double d3 = AI(0, 4) + T(2, 3) * AI(4, 4) - T(1, 3) * AI(4, 5);
582 double d4 = AI(1, 4) - T(2, 3) * AI(3, 4) + T(0, 3) * AI(4, 5);
583 double d5 = AI(2, 4) + T(1, 3) * AI(3, 4) - T(0, 3) * AI(4, 4);
584 double d6 = AI(0, 5) + T(2, 3) * AI(4, 5) - T(1, 3) * AI(5, 5);
585 double d7 = AI(1, 5) - T(2, 3) * AI(3, 5) + T(0, 3) * AI(5, 5);
586 double d8 = AI(2, 5) + T(1, 3) * AI(3, 5) - T(0, 3) * AI(4, 5);
587 double e0 = AI(0, 0) + T(2, 3) * AI(0, 4) - T(1, 3) * AI(0, 5) + d3 * T(2, 3)
588 - d6 * T(1, 3);
589 double e3 = AI(0, 1) + T(2, 3) * AI(1, 4) - T(1, 3) * AI(1, 5) - d0 * T(2, 3)
590 + d6 * T(0, 3);
591 double e4 = AI(1, 1) - T(2, 3) * AI(1, 3) + T(0, 3) * AI(1, 5) - d1 * T(2, 3)
592 + d7 * T(0, 3);
593 double e6 = AI(0, 2) + T(2, 3) * AI(2, 4) - T(1, 3) * AI(2, 5) + d0 * T(1, 3)
594 - d3 * T(0, 3);
595 double e7 = AI(1, 2) - T(2, 3) * AI(2, 3) + T(0, 3) * AI(2, 5) + d1 * T(1, 3)
596 - d4 * T(0, 3);
597 double e8 = AI(2, 2) + T(1, 3) * AI(2, 3) - T(0, 3) * AI(2, 4) + d2 * T(1, 3)
598 - d5 * T(0, 3);
599 double f0 = T(0, 0) * e0 + T(1, 0) * e3 + T(2, 0) * e6;
600 double f1 = T(0, 0) * e3 + T(1, 0) * e4 + T(2, 0) * e7;
601 double f2 = T(0, 0) * e6 + T(1, 0) * e7 + T(2, 0) * e8;
602 double f3 = T(0, 0) * d0 + T(1, 0) * d1 + T(2, 0) * d2;
603 double f4 = T(0, 0) * d3 + T(1, 0) * d4 + T(2, 0) * d5;
604 double f5 = T(0, 0) * d6 + T(1, 0) * d7 + T(2, 0) * d8;
605 double f6 = T(0, 1) * e0 + T(1, 1) * e3 + T(2, 1) * e6;
606 double f7 = T(0, 1) * e3 + T(1, 1) * e4 + T(2, 1) * e7;
607 double f8 = T(0, 1) * e6 + T(1, 1) * e7 + T(2, 1) * e8;
608 double g0 = T(0, 1) * d0 + T(1, 1) * d1 + T(2, 1) * d2;
609 double g1 = T(0, 1) * d3 + T(1, 1) * d4 + T(2, 1) * d5;
610 double g2 = T(0, 1) * d6 + T(1, 1) * d7 + T(2, 1) * d8;
611 double g3 = T(0, 2) * d0 + T(1, 2) * d1 + T(2, 2) * d2;
612 double g4 = T(0, 2) * d3 + T(1, 2) * d4 + T(2, 2) * d5;
613 double g5 = T(0, 2) * d6 + T(1, 2) * d7 + T(2, 2) * d8;
614 double h0 = T(0, 0) * AI(3, 3) + T(1, 0) * AI(3, 4) + T(2, 0) * AI(3, 5);
615 double h1 = T(0, 0) * AI(3, 4) + T(1, 0) * AI(4, 4) + T(2, 0) * AI(4, 5);
616 double h2 = T(0, 0) * AI(3, 5) + T(1, 0) * AI(4, 5) + T(2, 0) * AI(5, 5);
617 double h3 = T(0, 1) * AI(3, 3) + T(1, 1) * AI(3, 4) + T(2, 1) * AI(3, 5);
618 double h4 = T(0, 1) * AI(3, 4) + T(1, 1) * AI(4, 4) + T(2, 1) * AI(4, 5);
619 double h5 = T(0, 1) * AI(3, 5) + T(1, 1) * AI(4, 5) + T(2, 1) * AI(5, 5);
620
621 ret << f0 * T(0, 0) + f1 * T(1, 0) + f2 * T(2, 0),
622 f0 * T(0, 1) + f1 * T(1, 1) + f2 * T(2, 1),
623 f0 * T(0, 2) + f1 * T(1, 2) + f2 * T(2, 2),
624 f3 * T(0, 0) + f4 * T(1, 0) + f5 * T(2, 0),
625 f3 * T(0, 1) + f4 * T(1, 1) + f5 * T(2, 1),
626 f3 * T(0, 2) + f4 * T(1, 2) + f5 * T(2, 2), ret(0, 1),
627 f6 * T(0, 1) + f7 * T(1, 1) + f8 * T(2, 1),
628 f6 * T(0, 2) + f7 * T(1, 2) + f8 * T(2, 2),
629 g0 * T(0, 0) + g1 * T(1, 0) + g2 * T(2, 0),
630 g0 * T(0, 1) + g1 * T(1, 1) + g2 * T(2, 1),
631 g0 * T(0, 2) + g1 * T(1, 2) + g2 * T(2, 2), ret(0, 2), ret(1, 2),
632 (T(0, 2) * e0 + T(1, 2) * e3 + T(2, 2) * e6) * T(0, 2)
633 + (T(0, 2) * e3 + T(1, 2) * e4 + T(2, 2) * e7) * T(1, 2)
634 + (T(0, 2) * e6 + T(1, 2) * e7 + T(2, 2) * e8) * T(2, 2),
635 g3 * T(0, 0) + g4 * T(1, 0) + g5 * T(2, 0),
636 g3 * T(0, 1) + g4 * T(1, 1) + g5 * T(2, 1),
637 g3 * T(0, 2) + g4 * T(1, 2) + g5 * T(2, 2), ret(0, 3), ret(1, 3),
638 ret(2, 3), h0 * T(0, 0) + h1 * T(1, 0) + h2 * T(2, 0),
639 h0 * T(0, 1) + h1 * T(1, 1) + h2 * T(2, 1),
640 h0 * T(0, 2) + h1 * T(1, 2) + h2 * T(2, 2), ret(0, 4), ret(1, 4),
641 ret(2, 4), ret(3, 4), h3 * T(0, 1) + h4 * T(1, 1) + h5 * T(2, 1),
642 h3 * T(0, 2) + h4 * T(1, 2) + h5 * T(2, 2), ret(0, 5), ret(1, 5),
643 ret(2, 5), ret(3, 5), ret(4, 5),
644 (T(0, 2) * AI(3, 3) + T(1, 2) * AI(3, 4) + T(2, 2) * AI(3, 5)) * T(0, 2)
645 + (T(0, 2) * AI(3, 4) + T(1, 2) * AI(4, 4) + T(2, 2) * AI(4, 5))
646 * T(1, 2)
647 + (T(0, 2) * AI(3, 5) + T(1, 2) * AI(4, 5) + T(2, 2) * AI(5, 5))
648 * T(2, 2);
649
650 return ret;
651 }
652
653 EIGEN_DONT_INLINE
654 Matrix<double, 6, 6> Transform_Matrix4d(
655 const Matrix4d& T, const Matrix<double, 6, 6>& AI)
656 {
657 // operation count: multiplication = 186, addition = 117, subtract = 21
658
659 Matrix<double, 6, 6> ret;
660
661 double d0 = AI(0, 3) + T(2, 3) * AI(3, 4) - T(1, 3) * AI(3, 5);
662 double d1 = AI(1, 3) - T(2, 3) * AI(3, 3) + T(0, 3) * AI(3, 5);
663 double d2 = AI(2, 3) + T(1, 3) * AI(3, 3) - T(0, 3) * AI(3, 4);
664 double d3 = AI(0, 4) + T(2, 3) * AI(4, 4) - T(1, 3) * AI(4, 5);
665 double d4 = AI(1, 4) - T(2, 3) * AI(3, 4) + T(0, 3) * AI(4, 5);
666 double d5 = AI(2, 4) + T(1, 3) * AI(3, 4) - T(0, 3) * AI(4, 4);
667 double d6 = AI(0, 5) + T(2, 3) * AI(4, 5) - T(1, 3) * AI(5, 5);
668 double d7 = AI(1, 5) - T(2, 3) * AI(3, 5) + T(0, 3) * AI(5, 5);
669 double d8 = AI(2, 5) + T(1, 3) * AI(3, 5) - T(0, 3) * AI(4, 5);
670 double e0 = AI(0, 0) + T(2, 3) * AI(0, 4) - T(1, 3) * AI(0, 5) + d3 * T(2, 3)
671 - d6 * T(1, 3);
672 double e3 = AI(0, 1) + T(2, 3) * AI(1, 4) - T(1, 3) * AI(1, 5) - d0 * T(2, 3)
673 + d6 * T(0, 3);
674 double e4 = AI(1, 1) - T(2, 3) * AI(1, 3) + T(0, 3) * AI(1, 5) - d1 * T(2, 3)
675 + d7 * T(0, 3);
676 double e6 = AI(0, 2) + T(2, 3) * AI(2, 4) - T(1, 3) * AI(2, 5) + d0 * T(1, 3)
677 - d3 * T(0, 3);
678 double e7 = AI(1, 2) - T(2, 3) * AI(2, 3) + T(0, 3) * AI(2, 5) + d1 * T(1, 3)
679 - d4 * T(0, 3);
680 double e8 = AI(2, 2) + T(1, 3) * AI(2, 3) - T(0, 3) * AI(2, 4) + d2 * T(1, 3)
681 - d5 * T(0, 3);
682 double f0 = T(0, 0) * e0 + T(1, 0) * e3 + T(2, 0) * e6;
683 double f1 = T(0, 0) * e3 + T(1, 0) * e4 + T(2, 0) * e7;
684 double f2 = T(0, 0) * e6 + T(1, 0) * e7 + T(2, 0) * e8;
685 double f3 = T(0, 0) * d0 + T(1, 0) * d1 + T(2, 0) * d2;
686 double f4 = T(0, 0) * d3 + T(1, 0) * d4 + T(2, 0) * d5;
687 double f5 = T(0, 0) * d6 + T(1, 0) * d7 + T(2, 0) * d8;
688 double f6 = T(0, 1) * e0 + T(1, 1) * e3 + T(2, 1) * e6;
689 double f7 = T(0, 1) * e3 + T(1, 1) * e4 + T(2, 1) * e7;
690 double f8 = T(0, 1) * e6 + T(1, 1) * e7 + T(2, 1) * e8;
691 double g0 = T(0, 1) * d0 + T(1, 1) * d1 + T(2, 1) * d2;
692 double g1 = T(0, 1) * d3 + T(1, 1) * d4 + T(2, 1) * d5;
693 double g2 = T(0, 1) * d6 + T(1, 1) * d7 + T(2, 1) * d8;
694 double g3 = T(0, 2) * d0 + T(1, 2) * d1 + T(2, 2) * d2;
695 double g4 = T(0, 2) * d3 + T(1, 2) * d4 + T(2, 2) * d5;
696 double g5 = T(0, 2) * d6 + T(1, 2) * d7 + T(2, 2) * d8;
697 double h0 = T(0, 0) * AI(3, 3) + T(1, 0) * AI(3, 4) + T(2, 0) * AI(3, 5);
698 double h1 = T(0, 0) * AI(3, 4) + T(1, 0) * AI(4, 4) + T(2, 0) * AI(4, 5);
699 double h2 = T(0, 0) * AI(3, 5) + T(1, 0) * AI(4, 5) + T(2, 0) * AI(5, 5);
700 double h3 = T(0, 1) * AI(3, 3) + T(1, 1) * AI(3, 4) + T(2, 1) * AI(3, 5);
701 double h4 = T(0, 1) * AI(3, 4) + T(1, 1) * AI(4, 4) + T(2, 1) * AI(4, 5);
702 double h5 = T(0, 1) * AI(3, 5) + T(1, 1) * AI(4, 5) + T(2, 1) * AI(5, 5);
703
704 ret(0, 0) = f0 * T(0, 0) + f1 * T(1, 0) + f2 * T(2, 0);
705 ret(0, 1) = f0 * T(0, 1) + f1 * T(1, 1) + f2 * T(2, 1);
706 ret(0, 2) = f0 * T(0, 2) + f1 * T(1, 2) + f2 * T(2, 2);
707 ret(0, 3) = f3 * T(0, 0) + f4 * T(1, 0) + f5 * T(2, 0);
708 ret(0, 4) = f3 * T(0, 1) + f4 * T(1, 1) + f5 * T(2, 1);
709 ret(0, 5) = f3 * T(0, 2) + f4 * T(1, 2) + f5 * T(2, 2);
710 ret(1, 1) = f6 * T(0, 1) + f7 * T(1, 1) + f8 * T(2, 1);
711 ret(1, 2) = f6 * T(0, 2) + f7 * T(1, 2) + f8 * T(2, 2);
712 ret(1, 3) = g0 * T(0, 0) + g1 * T(1, 0) + g2 * T(2, 0);
713 ret(1, 4) = g0 * T(0, 1) + g1 * T(1, 1) + g2 * T(2, 1);
714 ret(1, 5) = g0 * T(0, 2) + g1 * T(1, 2) + g2 * T(2, 2);
715 ret(2, 2) = (T(0, 2) * e0 + T(1, 2) * e3 + T(2, 2) * e6) * T(0, 2)
716 + (T(0, 2) * e3 + T(1, 2) * e4 + T(2, 2) * e7) * T(1, 2)
717 + (T(0, 2) * e6 + T(1, 2) * e7 + T(2, 2) * e8) * T(2, 2);
718 ret(2, 3) = g3 * T(0, 0) + g4 * T(1, 0) + g5 * T(2, 0);
719 ret(2, 4) = g3 * T(0, 1) + g4 * T(1, 1) + g5 * T(2, 1);
720 ret(2, 5) = g3 * T(0, 2) + g4 * T(1, 2) + g5 * T(2, 2);
721 ret(3, 3) = h0 * T(0, 0) + h1 * T(1, 0) + h2 * T(2, 0);
722 ret(3, 4) = h0 * T(0, 1) + h1 * T(1, 1) + h2 * T(2, 1);
723 ret(3, 5) = h0 * T(0, 2) + h1 * T(1, 2) + h2 * T(2, 2);
724 ret(4, 4) = h3 * T(0, 1) + h4 * T(1, 1) + h5 * T(2, 1);
725 ret(4, 5) = h3 * T(0, 2) + h4 * T(1, 2) + h5 * T(2, 2);
726 ret(5, 5)
727 = (T(0, 2) * AI(3, 3) + T(1, 2) * AI(3, 4) + T(2, 2) * AI(3, 5)) * T(0, 2)
728 + (T(0, 2) * AI(3, 4) + T(1, 2) * AI(4, 4) + T(2, 2) * AI(4, 5))
729 * T(1, 2)
730 + (T(0, 2) * AI(3, 5) + T(1, 2) * AI(4, 5) + T(2, 2) * AI(5, 5))
731 * T(2, 2);
732
733 ret.triangularView<StrictlyLower>() = ret.transpose();
734
735 return ret;
736 }
737
738 TEST(MATH, ARTICULATED_INERTIA_TRANSFORM)
739 {
740 const int iterations = 100000;
741
742 Affine3d A1
743 = Translation3d(0.1, 0.2, 0.3)
744 * AngleAxisd(0.5, Vector3d(1.0 / sqrt(2.0), 1.0 / sqrt(2.0), 0.0));
745 Matrix<double, 6, 6> a1 = Matrix<double, 6, 6>::Identity();
746 Matrix<double, 6, 6> a2;
747
748 Isometry3d I1;
749 I1.matrix() = A1.matrix();
750 Matrix<double, 6, 6> i1 = Matrix<double, 6, 6>::Identity();
751 Matrix<double, 6, 6> i2;
752
753 Matrix4d M1;
754 M1 = A1.matrix();
755 Matrix<double, 6, 6> m1 = Matrix<double, 6, 6>::Identity();
756 Matrix<double, 6, 6> m2;
757
758 clock_t start = clock();
759 for (int i = 0; i < iterations; i++)
760 {
761 a2 = Transform_Affine3d(A1, a1);
762 }
763 std::cout << "Transform_Affine3d: "
764 << (double)(clock() - start) / CLOCKS_PER_SEC << " s\n";
765 // cout << "result: " << a2 << endl;
766
767 start = clock();
768 for (int i = 0; i < iterations; i++)
769 {
770 i2 = Transform_Isometry3d(I1, i1);
771 }
772 std::cout << "Transform_Isometry3d: "
773 << (double)(clock() - start) / CLOCKS_PER_SEC << " s\n";
774 // cout << "result: " << i2 << endl;
775
776 start = clock();
777 for (int i = 0; i < iterations; i++)
778 {
779 i2 = Transform_Isometry3d_2(I1, i1);
780 }
781 std::cout << "Transform_Isometry3d_2: "
782 << (double)(clock() - start) / CLOCKS_PER_SEC << " s\n";
783 // cout << "result: " << i2 << endl;
784
785 start = clock();
786 for (int i = 0; i < iterations; i++)
787 {
788 i2 = Transform_Isometry3d_3(I1, i1);
789 }
790 std::cout << "Transform_Isometry3d_3: "
791 << (double)(clock() - start) / CLOCKS_PER_SEC << " s\n";
792 // cout << "result: " << i2 << endl;
793
794 start = clock();
795 for (int i = 0; i < iterations; i++)
796 {
797 m2 = Transform_Matrix4d(M1, m1);
798 }
799 std::cout << "Transform_Matrix4d: "
800 << (double)(clock() - start) / CLOCKS_PER_SEC << " s\n";
801 // cout << "result: " << m2 << endl;
802 }
803
804 TEST(MATH, ADJOINT_MAPPING)
805 {
806 const int iterations = 10000;
807
808 Affine3d A1
809 = Translation3d(0.1, 0.2, 0.3)
810 * AngleAxisd(0.5, Vector3d(1.0 / sqrt(2.0), 1.0 / sqrt(2.0), 0.0));
811 Matrix<double, 6, 1> a1, a2;
812 a1 << 1, 2, 3, 4, 5, 6;
813
814 Affine3d A2
815 = Translation3d(0.1, 0.2, 0.3)
816 * AngleAxisd(0.5, Vector3d(1.0 / sqrt(2.0), 1.0 / sqrt(2.0), 0.0));
817 Matrix<double, 6, 1> a3, a4;
818 a3 << 1, 2, 3, 4, 5, 6;
819
820 // Affine3d A3 = Translation3d(0.1, 0.2, 0.3) * AngleAxisd(0.5, Vector3d(1.0 /
821 // sqrt(2.0), 1.0 / sqrt(2.0), 0.0));
822 Matrix<double, 6, 1> a5, a6;
823 a5 << 1, 2, 3, 4, 5, 6;
824
825 Isometry3d I1;
826 I1.matrix() = A1.matrix();
827 Matrix<double, 6, 1> i1, i2;
828 i1 << 1, 2, 3, 4, 5, 6;
829
830 Matrix4d M1;
831 M1 = A1.matrix();
832 Matrix<double, 6, 1> m1, m2;
833 m1 << 1, 2, 3, 4, 5, 6;
834
835 clock_t start = clock();
836 for (int i = 0; i < iterations; i++)
837 {
838 a2 = Ad_Affine3d_1(A1, a1);
839 }
840 std::cout << "Ad_Affine3d_1: " << (double)(clock() - start) / CLOCKS_PER_SEC
841 << " s\n";
842 // cout << "result: " << a2 << endl;
843
844 start = clock();
845 for (int i = 0; i < iterations; i++)
846 {
847 a4 = Ad_Affine3d_2(A2, a3);
848 }
849 std::cout << "Ad_Affine3d_2: " << (double)(clock() - start) / CLOCKS_PER_SEC
850 << " s\n";
851 // cout << "result: " << a4 << endl;
852
853 // start = clock();
854 // for (int i = 0; i < iterations; i++) {
855 // a6 = Ad_Affine3d_3(A3, a5);
856 // }
857 // cout << "Ad_Affine3d_3: " << (double)(clock() - start) / CLOCKS_PER_SEC
858 // << " s\n";
859 // //cout << "result: " << a6 << endl;
860
861 start = clock();
862 for (int i = 0; i < iterations; i++)
863 {
864 i2 = Ad_Isometry3d(I1, i1);
865 }
866 std::cout << "Ad_Isometry3d: " << (double)(clock() - start) / CLOCKS_PER_SEC
867 << " s\n";
868 // cout << "result: " << i2 << endl;
869
870 start = clock();
871 for (int i = 0; i < iterations; i++)
872 {
873 m2 = Ad_Matrix4d(M1, m1);
874 }
875 std::cout << "Ad_Matrix4d: " << (double)(clock() - start) / CLOCKS_PER_SEC
876 << " s\n";
877 // cout << "result: " << im << endl;
878 }
879
880 TEST(MATH, TRANSFORMATION)
881 {
882 const int iterations = 10000;
883
884 Affine3d A1
885 = Translation3d(0.1, 0.2, 0.3)
886 * AngleAxisd(0.5, Vector3d(1.0 / sqrt(2.0), 1.0 / sqrt(2.0), 0.0));
887 Affine3d A2 = A1, A3;
888
889 Isometry3d I1;
890 I1.matrix() = A1.matrix();
891 Isometry3d I2 = I1, I3;
892
893 Matrix4d M1 = A1.matrix();
894 Matrix4d M2 = M1, M3;
895
896 EigenSE3 ES1(A1.matrix());
897 EigenSE3 ES2(A1.matrix()), ES3(A1.matrix());
898
899 clock_t start = clock();
900 for (int i = 0; i < iterations; i++)
901 {
902 prod(ES2, ES1, ES3);
903 }
904 std::cout << "EigenSE3: " << (double)(clock() - start) / CLOCKS_PER_SEC
905 << " s\n";
906
907 start = clock();
908 for (int i = 0; i < iterations; i++)
909 {
910 prod(A2, A1, A3);
911 }
912 std::cout << "Affine3d: " << (double)(clock() - start) / CLOCKS_PER_SEC
913 << " s\n";
914
915 start = clock();
916 for (int i = 0; i < iterations; i++)
917 {
918 prod(I2, I1, I3);
919 }
920 std::cout << "Isometry3d: " << (double)(clock() - start) / CLOCKS_PER_SEC
921 << " s\n";
922
923 start = clock();
924 for (int i = 0; i < iterations; i++)
925 {
926 prod(M2, M1, M3);
927 }
928 std::cout << "Matrix4d: " << (double)(clock() - start) / CLOCKS_PER_SEC
929 << " s\n";
930
931 A2 = A1;
932 start = clock();
933 for (int i = 0; i < iterations; i++)
934 {
935 concatenate(A2, A1, A3);
936 }
937 std::cout << "Hard-coded: " << (double)(clock() - start) / CLOCKS_PER_SEC
938 << " s\n";
939 }
940
941 TEST(MATH, INVERSION)
942 {
943 const int iterations = 10000;
944
945 Affine3d A1
946 = Translation3d(0.1, 0.2, 0.3)
947 * AngleAxisd(0.5, Vector3d(1.0 / sqrt(2.0), 1.0 / sqrt(2.0), 0.0));
948 Affine3d A2 = A1, A3;
949
950 Isometry3d I1;
951 I1.matrix() = A1.matrix();
952 Isometry3d I2 = I1, I3;
953
954 Matrix4d M1 = A1.matrix();
955 Matrix4d M2 = M1, M3;
956
957 clock_t start = clock();
958 for (int i = 0; i < iterations; i++)
959 {
960 inv(A1, A2);
961 }
962 std::cout << "Affine3d: " << (double)(clock() - start) / CLOCKS_PER_SEC
963 << " s\n";
964
965 start = clock();
966 for (int i = 0; i < iterations; i++)
967 {
968 inv(I1, I2);
969 }
970 std::cout << "Isometry3d_1: " << (double)(clock() - start) / CLOCKS_PER_SEC
971 << " s\n";
972
973 start = clock();
974 for (int i = 0; i < iterations; i++)
975 {
976 inv_se3(I1, I2);
977 }
978 std::cout << "Isometry3d_2: " << (double)(clock() - start) / CLOCKS_PER_SEC
979 << " s\n";
980
981 start = clock();
982 for (int i = 0; i < iterations; i++)
983 {
984 inv(M1, M2);
985 }
986 std::cout << "Matrix4d: " << (double)(clock() - start) / CLOCKS_PER_SEC
987 << " s\n";
988 }
989
990 /******************************************************************************/
991 // TEST(MATH, SE3_VS_EIGENMATRIX4D)
992 //{
993 // int n = 10000;
994 // double min = -100;
995 // double max = 100;
996
997 // SE3 T1 = Exp(se3(random(min,max), random(min,max), random(min,max),
998 // random(min,max), random(min,max), random(min,max)));
999 // SE3 T2 = Exp(se3(random(min,max), random(min,max), random(min,max),
1000 // random(min,max), random(min,max), random(min,max)));
1001 // SE3 T3 = Exp(se3(random(min,max), random(min,max), random(min,max),
1002 // random(min,max), random(min,max), random(min,max)));
1003 // SE3 T4 = Exp(se3(random(min,max), random(min,max), random(min,max),
1004 // random(min,max), random(min,max), random(min,max)));
1005
1006 // Eigen::Matrix4d E1 = T1;
1007 // Eigen::Matrix4d E2 = T2;
1008 // Eigen::Matrix4d E3 = T3;
1009 // Eigen::Matrix4d E4 = T4;
1010
1011 // Eigen::Affine3d A1(E1);
1012 // Eigen::Affine3d A2(E2);
1013 // Eigen::Affine3d A3(E3);
1014 // Eigen::Affine3d A4(E4);
1015
1016 // Eigen::Isometry3d I1(E1);
1017 // Eigen::Isometry3d I2(E2);
1018 // Eigen::Isometry3d I3(E3);
1019 // Eigen::Isometry3d I4(E4);
1020
1021 // EigenSE3 ESE3_1(E1);
1022 // EigenSE3 ESE3_2(E2);
1023 // EigenSE3 ESE3_3(E3);
1024 // EigenSE3 ESE3_4(E4);
1025
1026 // {
1027 // Timer SE3Timer("SE3 timer");
1028 // SE3Timer.startTimer();
1029 // for (int i = 0; i < n; ++i)
1030 // for (int j = 0; j < n; ++j)
1031 // T3 = T3 * T2;
1032 // SE3Timer.stopTimer();
1033 // }
1034 // std::cout << T3 << std::endl;
1035
1036 // {
1037 // Timer EigenTimer("Eigen::Matrix4d timer");
1038 // EigenTimer.startTimer();
1039 // for (int i = 0; i < n; ++i)
1040 // for (int j = 0; j < n; ++j)
1041 // E3 = E3 * E2;
1042 // EigenTimer.stopTimer();
1043 // }
1044 // std::cout << E3 << std::endl;
1045
1046 //// {
1047 //// Timer EigenSE3Timer("EigenSE3 timer");
1048 //// EigenSE3Timer.startTimer();
1049 //// for (int i = 0; i < n; ++i)
1050 //// for (int j = 0; j < n; ++j)
1051 //// ESE3_3 *= ESE3_1 * ESE3_2;
1052 //// EigenSE3Timer.stopTimer();
1053 //// }
1054 //// //std::cout << E3 << std::endl;
1055
1056 // {
1057 // Timer AffineTimer("Eigen::Affine3d timer");
1058 // AffineTimer.startTimer();
1059 // for (int i = 0; i < n; ++i)
1060 // for (int j = 0; j < n; ++j)
1061 // A3 = A3 * A2;
1062 // AffineTimer.stopTimer();
1063 // }
1064 // std::cout << A3.matrix() << std::endl;
1065
1066 // {
1067 // Timer IsometryTimer("Eigen::Isometry3d timer");
1068 // IsometryTimer.startTimer();
1069 // for (int i = 0; i < n; ++i)
1070 // for (int j = 0; j < n; ++j)
1071 // I3 = I3 * I2;
1072 // IsometryTimer.stopTimer();
1073 // }
1074 // std::cout << I3.matrix() << std::endl;
1075
1076 //// {
1077 //// Timer SE3Timer("SE3 inverse timer");
1078 //// SE3Timer.startTimer();
1079 //// for (int i = 0; i < n; ++i)
1080 //// for (int j = 0; j < n; ++j)
1081 //// T4 *= Inv(T1) * Inv(T2);
1082 //// SE3Timer.stopTimer();
1083 //// }
1084 //// std::cout << T4 << std::endl;
1085
1086 //// {
1087 //// Timer EigenTimer("Eigen::Matrix4d inverse timer");
1088 //// EigenTimer.startTimer();
1089 //// for (int i = 0; i < n; ++i)
1090 //// for (int j = 0; j < n; ++j)
1091 //// E4 *= E1.inverse() * E2.inverse();
1092 //// EigenTimer.stopTimer();
1093 //// }
1094 //// std::cout << E4 << std::endl;
1095 //}
1096
1097 /******************************************************************************/
1098 // TEST(MATH, SO3)
1099 //{
1100 // // Exponential and Logarithm mapping
1101 // for (int i = 0; i < 100; ++i)
1102 // {
1103 // double min = -100;
1104 // double max = 100;
1105
1106 // // Log(Exp(w)) = w
1107 // so3 w(random(min,max), random(min,max), random(min,max));
1108 // SO3 Expw = Exp(w);
1109 // so3 LogExpw = Log(Expw);
1110 // SO3 ExpLogExpw = Exp(LogExpw);
1111 // so3 LogExpLogExpw = Log(ExpLogExpw);
1112
1113 // for (int j = 0; j < 3; ++j)
1114 // {
1115 // for (int k = 0; k < 3; ++k)
1116 // {
1117 // EXPECT_NEAR(Expw(j,k), ExpLogExpw(j,k), MATH_EPS);
1118 // }
1119 // }
1120
1121 // for (int j = 0; j < 3; ++j)
1122 // EXPECT_NEAR(LogExpLogExpw[j], LogExpw[j], MATH_EPS);
1123
1124 // }
1125 //}
1126
1127 ///******************************************************************************/
1128 // TEST(MATH, SE3)
1129 //{
1130 // // Exponential and Logarithm mapping
1131 // for (int i = 0; i < 100; ++i)
1132 // {
1133 // double min = -100;
1134 // double max = 100;
1135
1136 // // Log(Exp(S)) = S
1137 // se3 S(random(min,max), random(min,max), random(min,max),
1138 // random(min,max), random(min,max), random(min,max));
1139 // SE3 ExpS = Exp(S);
1140 // se3 LogExpS = Log(ExpS);
1141 // SE3 ExpLogExpS = Exp(S);
1142 // se3 ExpLogLogExpS = Log(ExpS);
1143
1144 // for (int j = 0; j < 4; ++j)
1145 // {
1146 // for (int k = 0; k < 4; ++k)
1147 // EXPECT_NEAR(ExpS(j,k), ExpLogExpS(j,k), MATH_EPS);
1148 // }
1149
1150 // for (int j = 0; j < 6; ++j)
1151 // EXPECT_NEAR(LogExpS(j), ExpLogLogExpS(j), MATH_EPS);
1152 // }
1153
1154 //// // Exp(Log(T)) = T
1155
1156 //// // Ad(T,V) == T * V * invT
1157 //// se3 V(random(-1,1), random(-1,1), random(-1,1),
1158 //// random(-1,1), random(-1,1), random(-1,1));
1159 //// SE3 T = Exp(V);
1160
1161 //// se3 AdTV = Ad(T,V);
1162 //// Matrix4d AdTVmat_eig = AdTV;
1163
1164 //// Matrix4d Teig = T;
1165 //// Matrix4d Veig = V;
1166 //// Matrix4d invTeig = Inv(T);
1167 //// Matrix4d TVinvT = Teig * Veig * invTeig;
1168
1169 //// //EXPECT_EQ(AdTVmat_eig, TVinvT);
1170
1171 //// EXPECT_EQ(Ad(T, Ad(Inv(T), V)), V);
1172 //}
1173
1174 ///// \brief
1175 // Eigen::Matrix<double,6,6> Ad(const SE3& T)
1176 //{
1177 // Eigen::Matrix<double,6,6> AdT = Eigen::Matrix<double,6,6>::Zero();
1178
1179 // // R
1180 // AdT(0,0) = T(0,0); AdT(0,1) = T(0,1); AdT(0,2) = T(0,2);
1181 // AdT(1,0) = T(1,0); AdT(1,1) = T(1,1); AdT(1,2) = T(1,2);
1182 // AdT(2,0) = T(2,0); AdT(2,1) = T(2,1); AdT(2,2) = T(2,2);
1183
1184 // // R
1185 // AdT(3,3) = T(0,0); AdT(3,4) = T(0,1); AdT(3,5) = T(0,2);
1186 // AdT(4,3) = T(1,0); AdT(4,4) = T(1,1); AdT(4,5) = T(1,2);
1187 // AdT(5,3) = T(2,0); AdT(5,4) = T(2,1); AdT(5,5) = T(2,2);
1188
1189 // // [p]R
1190 // AdT(3,0) = T(1,3)*T(2,0) - T(2,3)*T(1,0); AdT(3,1) = T(1,3)*T(2,1) -
1191 // T(2,3)*T(1,1); AdT(3,2) = T(1,3)*T(2,2) - T(2,3)*T(1,2); AdT(4,0) =
1192 // T(2,3)*T(0,0) - T(0,3)*T(2,0); AdT(4,1) = T(2,3)*T(0,1) - T(0,3)*T(2,1);
1193 // AdT(4,2) = T(2,3)*T(0,2) - T(0,3)*T(2,2); AdT(5,0) = T(0,3)*T(1,0) -
1194 // T(1,3)*T(0,0); AdT(5,1) = T(0,3)*T(1,1) - T(1,3)*T(0,1); AdT(5,2) =
1195 // T(0,3)*T(1,2) - T(1,3)*T(0,2);
1196
1197 // return AdT;
1198 //}
1199
1200 ///// \brief
1201 // Eigen::Matrix<double,6,6> dAd(const SE3& T)
1202 //{
1203 // return Ad(T).transpose();
1204 //}
1205
1206 ///******************************************************************************/
1207 // TEST(MATH, ADJOINT_MAPPING)
1208 //{
1209 // double min = -100;
1210 // double max = 100;
1211 // math::se3 t(random(min,max), random(min,max), random(min,max),
1212 // random(min,max), random(min,max), random(min,max));
1213 // math::SE3 T = math::Exp(t);
1214 // math::se3 S(random(min,max), random(min,max), random(min,max),
1215 // random(min,max), random(min,max), random(min,max));
1216
1217 // Eigen::VectorXd AdT_S = math::Ad(T, S).getEigenVector();
1218 // Eigen::VectorXd AdT_S2 = Ad(T) * S.getEigenVector();
1219
1220 // Eigen::MatrixXd I = Ad(T) * Ad(math::Inv(T));
1221 // Eigen::MatrixXd SE3Identity = T * (math::Inv(T));
1222
1223 // for (int i = 0; i < 6; i++)
1224 // EXPECT_NEAR(AdT_S(i), AdT_S2(i), MATH_TOL);
1225 //}
1226
1227 ///******************************************************************************/
1228 // TEST(MATH, INERTIA)
1229 //{
1230 // for (int k = 0; k < 100; ++k)
1231 // {
1232 // double min = -10;
1233 // double max = 10;
1234 // math::Vec3 r(random(min,max), random(min,max), random(min,max));
1235 // math::SE3 Tr(r);
1236 // math::Inertia I(random(0.1,max), random(0.1,max), random(0.1,max),
1237 // random(0.1,max), random(0.1,max), random(0.1,max),
1238 // random(min,max), random(min,max), random(min,max),
1239 // random(0.1,max));
1240 // math::Inertia Ioffset = I;
1241
1242 // I.setOffset(math::Vec3(0, 0, 0));
1243 // Ioffset.setOffset(r);
1244
1245 // Eigen::MatrixXd G = I.toTensor();
1246 // Eigen::MatrixXd Goffset = Ioffset.toTensor();
1247 // Eigen::MatrixXd dAdinvTGAdinvT = dAd(math::Inv(Tr)) * G *
1248 // Ad(math::Inv(Tr));
1249
1250 // for (int i = 0; i < 6; i++)
1251 // for (int j = 0; j < 6; j++)
1252 // EXPECT_NEAR(Goffset(i,j), dAdinvTGAdinvT(i,j), MATH_TOL);
1253 // }
1254 //}
1255
1256 /******************************************************************************/
1257 TEST(MATH, ROTATION)
1258 {
1259 using namespace dart;
1260 using namespace math;
1261
1262 // Create Initial ExpMap
1263 Eigen::Vector3d axis(2.0, 1.0, 1.0);
1264 axis.normalize();
1265 double angle = 1.2;
1266 EXPECT_DOUBLE_EQ(axis.norm(), 1.0);
1267 Eigen::Vector3d expmap = axis * angle;
1268
1269 // Test conversion between expmap and quaternion
1270 Eigen::Quaterniond q = expToQuat(expmap);
1271 Eigen::Vector3d expmap2 = quatToExp(q);
1272
1273 EXPECT_NEAR((expmap - expmap2).norm(), 0.0, MATH_EPS)
1274 << "Orig: " << expmap << " Reconstructed: " << expmap2;
1275
1276 // Test conversion between matrix and euler
1277 Eigen::Matrix3d m = q.toRotationMatrix();
1278 Eigen::Vector3d e = matrixToEulerXYZ(m);
1279 Eigen::Matrix3d m2 = eulerXYZToMatrix(e);
1280
1281 EXPECT_NEAR((m - m2).norm(), 0.0, MATH_EPS)
1282 << "Orig: " << m << " Reconstructed: " << m2;
1283 }
1284
1285 /******************************************************************************/
1286 TEST(MATH, UTILS)
1287 {
1288 // Test CR Matrix
1289 EXPECT_DOUBLE_EQ(dart::math::CR(0, 1), -1.0);
1290
1291 // Test randomize function
1292 double x = dart::math::Random::uniform(0.0, 2.0);
1293 EXPECT_LT(0.0, x);
1294 EXPECT_LT(x, 2.0);
1295
1296 // Test transform
1297 Eigen::Isometry3d M = Eigen::Isometry3d::Identity();
1298 M.translation() = Eigen::Vector3d(3.0, 2.0, 1.0);
1299 Eigen::Vector3d pt(1.0, 0.5, 1.0);
1300 Eigen::Vector3d result = M * pt;
1301 Eigen::Vector3d expected(4.0, 2.5, 2.0);
1302 EXPECT_NEAR((result - expected).norm(), 0.0, MATH_EPS)
1303 << "result = " << result << " expected = " << expected;
1304 }
1305
1306 //==============================================================================
1307 Jacobian AdTJac1(const Eigen::Isometry3d& _T, const Jacobian& _J)
1308 {
1309 Jacobian res = Jacobian::Zero(6, _J.cols());
1310
1311 for (int i = 0; i < _J.cols(); ++i)
1312 res.col(i) = AdT(_T, _J.col(i));
1313 return res;
1314 }
1315
1316 //==============================================================================
1317 template <typename Derived>
1318 typename Derived::PlainObject AdTJac2(
1319 const Eigen::Isometry3d& _T, const Eigen::MatrixBase<Derived>& _J)
1320 {
1321 // EIGEN_STATIC_ASSERT_FIXED_SIZE(Derived);
1322 EIGEN_STATIC_ASSERT(
1323 Derived::RowsAtCompileTime == 6,
1324 THIS_METHOD_IS_ONLY_FOR_MATRICES_OF_A_SPECIFIC_SIZE);
1325
1326 typename Derived::PlainObject ret(_J.rows(), _J.cols());
1327
1328 for (int i = 0; i < _J.cols(); ++i)
1329 ret.col(i) = AdT(_T, _J.col(i));
1330
1331 return ret;
1332 }
1333
1334 //==============================================================================
1335 template <typename Derived>
1336 typename Derived::PlainObject AdTJac3(
1337 const Eigen::Isometry3d& _T, const Eigen::MatrixBase<Derived>& _J)
1338 {
1339 // EIGEN_STATIC_ASSERT_FIXED_SIZE(Derived);
1340 EIGEN_STATIC_ASSERT(
1341 Derived::RowsAtCompileTime == 6,
1342 THIS_METHOD_IS_ONLY_FOR_MATRICES_OF_A_SPECIFIC_SIZE);
1343
1344 typename Derived::PlainObject ret(_J.rows(), _J.cols());
1345
1346 ret.template topRows<3>().noalias() = _T.linear() * _J.template topRows<3>();
1347 ret.template bottomRows<3>().noalias()
1348 = -ret.template topRows<3>().colwise().cross(_T.translation())
1349 + _T.linear() * _J.template bottomRows<3>();
1350
1351 return ret;
1352 }
1353
1354 //==============================================================================
1355 TEST(MATH, PerformanceComparisonOfAdTJac)
1356 {
1357 #ifndef NDEBUG
1358 int testCount = 1e+2;
1359 #else
1360 int testCount = 1e+6;
1361 #endif
1362 int m = 3;
1363
1364 Vector6d t = Vector6d::Random();
1365 Isometry3d T = expMap(t);
1366 Jacobian dynamicJ = Jacobian::Random(6, m);
1367 Matrix<double, 6, 3> fixedJ = dynamicJ; // Matrix<double, 6, 3>::Random();
1368
1369 // Test1: verify the results
1370 for (int i = 0; i < testCount; ++i)
1371 {
1372 Jacobian resJ1 = AdTJac1(T, dynamicJ);
1373 Jacobian resJ2 = AdTJac2(T, dynamicJ);
1374 Jacobian resJ3 = AdTJac3(T, fixedJ);
1375
1376 EXPECT_TRUE(equals(resJ1, resJ2));
1377 EXPECT_TRUE(equals(resJ2, resJ3));
1378
1379 Jacobian resJ4 = AdInvTJac(T, dynamicJ);
1380 Jacobian resJ5 = AdInvTJacFixed(T, fixedJ);
1381
1382 EXPECT_TRUE(equals(resJ4, resJ5));
1383 }
1384
1385 // Test2: performance
1386 tic();
1387 for (int i = 0; i < testCount; ++i)
1388 {
1389 Jacobian resJ1 = AdTJac1(T, dynamicJ);
1390 }
1391 std::cout << "AdTJac1 - dynamic: " << toc() << " s\n";
1392
1393 tic();
1394 for (int i = 0; i < testCount; ++i)
1395 {
1396 Jacobian resJ1 = AdTJac1(T, fixedJ);
1397 }
1398 std::cout << "AdTJac1 - fixed : " << toc() << " s\n";
1399
1400 tic();
1401 for (int i = 0; i < testCount; ++i)
1402 {
1403 Jacobian resJ2 = AdTJac2(T, dynamicJ);
1404 }
1405 std::cout << "AdTJac2 - dynamic: " << toc() << " s\n";
1406
1407 tic();
1408 for (int i = 0; i < testCount; ++i)
1409 {
1410 Jacobian resJ2 = AdTJac2(T, fixedJ);
1411 }
1412 std::cout << "AdTJac2 - fixed : " << toc() << " s\n";
1413
1414 tic();
1415 for (int i = 0; i < testCount; ++i)
1416 {
1417 Jacobian resJ3 = AdTJac3(T, dynamicJ);
1418 }
1419 std::cout << "AdTJac3 - dynamic: " << toc() << " s\n";
1420
1421 tic();
1422 for (int i = 0; i < testCount; ++i)
1423 {
1424 Jacobian resJ3 = AdTJac3(T, fixedJ);
1425 }
1426 std::cout << "AdTJac3 - fixed : " << toc() << " s\n";
1427
1428 // Note: The best function for dynamic size Jacobian is AdTJac2, and the best
1429 // function for fixed size Jacobian is AdTJac3
1430 }