| 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/math/Geometry.hpp" |
| 38 |
#include "dart/math/Helpers.hpp" |
| 39 |
|
| 40 |
#include "TestHelpers.hpp" |
| 41 |
|
| 42 |
using namespace dart; |
| 43 |
using namespace math; |
| 44 |
|
| 45 |
#define LIE_GROUP_OPT_TOL 1e-12 |
| 46 |
|
| 47 |
/******************************************************************************/ |
| 48 |
Eigen::Matrix4d toMatrixForm(const Eigen::Vector6d& v) |
| 49 |
{ |
| 50 |
Eigen::Matrix4d result = Eigen::Matrix4d::Zero(); |
| 51 |
|
| 52 |
result(0, 1) = -v(2); |
| 53 |
result(1, 0) = v(2); |
| 54 |
result(0, 2) = v(1); |
| 55 |
result(2, 0) = -v(1); |
| 56 |
result(1, 2) = -v(0); |
| 57 |
result(2, 1) = v(0); |
| 58 |
|
| 59 |
result(0, 3) = v(3); |
| 60 |
result(1, 3) = v(4); |
| 61 |
result(2, 3) = v(5); |
| 62 |
|
| 63 |
return result; |
| 64 |
} |
| 65 |
|
| 66 |
/******************************************************************************/ |
| 67 |
Eigen::Vector6d fromMatrixForm(const Eigen::Matrix4d& m) |
| 68 |
{ |
| 69 |
Eigen::Vector6d ret; |
| 70 |
ret << m(2, 1), m(0, 2), m(1, 0), m(0, 3), m(1, 3), m(2, 3); |
| 71 |
return ret; |
| 72 |
} |
| 73 |
|
| 74 |
/******************************************************************************/ |
| 75 |
void testEulerAngles(const Eigen::Vector3d& angle) |
| 76 |
{ |
| 77 |
Eigen::Matrix3d mat1; |
| 78 |
Eigen::Matrix3d mat2; |
| 79 |
|
| 80 |
// XYX |
| 81 |
mat1 = math::eulerXYXToMatrix(angle); |
| 82 |
mat2 = Eigen::AngleAxisd(angle(0), Eigen::Vector3d::UnitX()) |
| 83 |
* Eigen::AngleAxisd(angle(1), Eigen::Vector3d::UnitY()) |
| 84 |
* Eigen::AngleAxisd(angle(2), Eigen::Vector3d::UnitX()); |
| 85 |
|
| 86 |
EXPECT_TRUE(math::verifyRotation(mat1)); |
| 87 |
EXPECT_TRUE(math::verifyRotation(mat2)); |
| 88 |
EXPECT_TRUE(equals(mat1, mat2)); |
| 89 |
EXPECT_TRUE(equals(mat1, eulerXYXToMatrix(matrixToEulerXYX(mat1)))); |
| 90 |
|
| 91 |
// XYZ |
| 92 |
mat1 = math::eulerXYZToMatrix(angle); |
| 93 |
mat2 = Eigen::AngleAxisd(angle(0), Eigen::Vector3d::UnitX()) |
| 94 |
* Eigen::AngleAxisd(angle(1), Eigen::Vector3d::UnitY()) |
| 95 |
* Eigen::AngleAxisd(angle(2), Eigen::Vector3d::UnitZ()); |
| 96 |
|
| 97 |
EXPECT_TRUE(math::verifyRotation(mat1)); |
| 98 |
EXPECT_TRUE(math::verifyRotation(mat2)); |
| 99 |
EXPECT_TRUE(equals(mat1, mat2)); |
| 100 |
EXPECT_TRUE(equals(mat1, eulerXYZToMatrix(matrixToEulerXYZ(mat1)))); |
| 101 |
|
| 102 |
// XZX |
| 103 |
mat1 = math::eulerXZXToMatrix(angle); |
| 104 |
mat2 = Eigen::AngleAxisd(angle(0), Eigen::Vector3d::UnitX()) |
| 105 |
* Eigen::AngleAxisd(angle(1), Eigen::Vector3d::UnitZ()) |
| 106 |
* Eigen::AngleAxisd(angle(2), Eigen::Vector3d::UnitX()); |
| 107 |
|
| 108 |
EXPECT_TRUE(math::verifyRotation(mat1)); |
| 109 |
EXPECT_TRUE(math::verifyRotation(mat2)); |
| 110 |
EXPECT_TRUE(equals(mat1, mat2)); |
| 111 |
// EXPECT_TRUE(equals(mat1, eulerXZXToMatrix(matrixToEulerXZX(mat1)))); |
| 112 |
|
| 113 |
// XZY |
| 114 |
mat1 = math::eulerXZYToMatrix(angle); |
| 115 |
mat2 = Eigen::AngleAxisd(angle(0), Eigen::Vector3d::UnitX()) |
| 116 |
* Eigen::AngleAxisd(angle(1), Eigen::Vector3d::UnitZ()) |
| 117 |
* Eigen::AngleAxisd(angle(2), Eigen::Vector3d::UnitY()); |
| 118 |
|
| 119 |
EXPECT_TRUE(math::verifyRotation(mat1)); |
| 120 |
EXPECT_TRUE(math::verifyRotation(mat2)); |
| 121 |
EXPECT_TRUE(equals(mat1, mat2)); |
| 122 |
EXPECT_TRUE(equals(mat1, eulerXZYToMatrix(matrixToEulerXZY(mat1)))); |
| 123 |
|
| 124 |
// YXY |
| 125 |
mat1 = math::eulerYXYToMatrix(angle); |
| 126 |
mat2 = Eigen::AngleAxisd(angle(0), Eigen::Vector3d::UnitY()) |
| 127 |
* Eigen::AngleAxisd(angle(1), Eigen::Vector3d::UnitX()) |
| 128 |
* Eigen::AngleAxisd(angle(2), Eigen::Vector3d::UnitY()); |
| 129 |
|
| 130 |
EXPECT_TRUE(math::verifyRotation(mat1)); |
| 131 |
EXPECT_TRUE(math::verifyRotation(mat2)); |
| 132 |
EXPECT_TRUE(equals(mat1, mat2)); |
| 133 |
// EXPECT_TRUE(equals(mat1, eulerYXYToMatrix(matrixToEulerYXY(mat1)))); |
| 134 |
|
| 135 |
// YXZ |
| 136 |
mat1 = math::eulerYXZToMatrix(angle); |
| 137 |
mat2 = Eigen::AngleAxisd(angle(0), Eigen::Vector3d::UnitY()) |
| 138 |
* Eigen::AngleAxisd(angle(1), Eigen::Vector3d::UnitX()) |
| 139 |
* Eigen::AngleAxisd(angle(2), Eigen::Vector3d::UnitZ()); |
| 140 |
|
| 141 |
EXPECT_TRUE(math::verifyRotation(mat1)); |
| 142 |
EXPECT_TRUE(math::verifyRotation(mat2)); |
| 143 |
EXPECT_TRUE(equals(mat1, mat2)); |
| 144 |
EXPECT_TRUE(equals(mat1, eulerYXZToMatrix(matrixToEulerYXZ(mat1)))); |
| 145 |
|
| 146 |
// YZX |
| 147 |
mat1 = math::eulerYZXToMatrix(angle); |
| 148 |
mat2 = Eigen::AngleAxisd(angle(0), Eigen::Vector3d::UnitY()) |
| 149 |
* Eigen::AngleAxisd(angle(1), Eigen::Vector3d::UnitZ()) |
| 150 |
* Eigen::AngleAxisd(angle(2), Eigen::Vector3d::UnitX()); |
| 151 |
|
| 152 |
EXPECT_TRUE(math::verifyRotation(mat1)); |
| 153 |
EXPECT_TRUE(math::verifyRotation(mat2)); |
| 154 |
EXPECT_TRUE(equals(mat1, mat2)); |
| 155 |
EXPECT_TRUE(equals(mat1, eulerYZXToMatrix(matrixToEulerYZX(mat1)))); |
| 156 |
|
| 157 |
// YZY |
| 158 |
mat1 = math::eulerYZYToMatrix(angle); |
| 159 |
mat2 = Eigen::AngleAxisd(angle(0), Eigen::Vector3d::UnitY()) |
| 160 |
* Eigen::AngleAxisd(angle(1), Eigen::Vector3d::UnitZ()) |
| 161 |
* Eigen::AngleAxisd(angle(2), Eigen::Vector3d::UnitY()); |
| 162 |
|
| 163 |
EXPECT_TRUE(math::verifyRotation(mat1)); |
| 164 |
EXPECT_TRUE(math::verifyRotation(mat2)); |
| 165 |
EXPECT_TRUE(equals(mat1, mat2)); |
| 166 |
// EXPECT_TRUE(equals(mat1, eulerYZYToMatrix(matrixToEulerYZY(mat1)))); |
| 167 |
|
| 168 |
// ZXY |
| 169 |
mat1 = math::eulerZXYToMatrix(angle); |
| 170 |
mat2 = Eigen::AngleAxisd(angle(0), Eigen::Vector3d::UnitZ()) |
| 171 |
* Eigen::AngleAxisd(angle(1), Eigen::Vector3d::UnitX()) |
| 172 |
* Eigen::AngleAxisd(angle(2), Eigen::Vector3d::UnitY()); |
| 173 |
|
| 174 |
EXPECT_TRUE(math::verifyRotation(mat1)); |
| 175 |
EXPECT_TRUE(math::verifyRotation(mat2)); |
| 176 |
EXPECT_TRUE(equals(mat1, mat2)); |
| 177 |
EXPECT_TRUE(equals(mat1, eulerZXYToMatrix(matrixToEulerZXY(mat1)))); |
| 178 |
|
| 179 |
// ZYX |
| 180 |
mat1 = math::eulerZYXToMatrix(angle); |
| 181 |
mat2 = Eigen::AngleAxisd(angle(0), Eigen::Vector3d::UnitZ()) |
| 182 |
* Eigen::AngleAxisd(angle(1), Eigen::Vector3d::UnitY()) |
| 183 |
* Eigen::AngleAxisd(angle(2), Eigen::Vector3d::UnitX()); |
| 184 |
|
| 185 |
EXPECT_TRUE(math::verifyRotation(mat1)); |
| 186 |
EXPECT_TRUE(math::verifyRotation(mat2)); |
| 187 |
EXPECT_TRUE(equals(mat1, mat2)); |
| 188 |
EXPECT_TRUE(equals(mat1, eulerZYXToMatrix(matrixToEulerZYX(mat1)))); |
| 189 |
|
| 190 |
// ZXZ |
| 191 |
mat1 = math::eulerZXZToMatrix(angle); |
| 192 |
mat2 = Eigen::AngleAxisd(angle(0), Eigen::Vector3d::UnitZ()) |
| 193 |
* Eigen::AngleAxisd(angle(1), Eigen::Vector3d::UnitX()) |
| 194 |
* Eigen::AngleAxisd(angle(2), Eigen::Vector3d::UnitZ()); |
| 195 |
|
| 196 |
EXPECT_TRUE(math::verifyRotation(mat1)); |
| 197 |
EXPECT_TRUE(math::verifyRotation(mat2)); |
| 198 |
EXPECT_TRUE(equals(mat1, mat2)); |
| 199 |
// EXPECT_TRUE(equals(mat1, eulerZXZToMatrix(matrixToEulerZXZ(mat1)))); |
| 200 |
|
| 201 |
// ZYZ |
| 202 |
mat1 = math::eulerZYZToMatrix(angle); |
| 203 |
mat2 = Eigen::AngleAxisd(angle(0), Eigen::Vector3d::UnitZ()) |
| 204 |
* Eigen::AngleAxisd(angle(1), Eigen::Vector3d::UnitY()) |
| 205 |
* Eigen::AngleAxisd(angle(2), Eigen::Vector3d::UnitZ()); |
| 206 |
|
| 207 |
EXPECT_TRUE(math::verifyRotation(mat1)); |
| 208 |
EXPECT_TRUE(math::verifyRotation(mat2)); |
| 209 |
EXPECT_TRUE(equals(mat1, mat2)); |
| 210 |
// EXPECT_TRUE(equals(mat1, eulerZYZToMatrix(matrixToEulerZYZ(mat1)))); |
| 211 |
} |
| 212 |
|
| 213 |
/******************************************************************************/ |
| 214 |
TEST(LIE_GROUP_OPERATORS, EULER_ANGLES) |
| 215 |
{ |
| 216 |
// TODO: Special angles such as (PI, 0, 0) |
| 217 |
|
| 218 |
// |
| 219 |
int numTest = 1; |
| 220 |
for (int i = 0; i < numTest; ++i) |
| 221 |
{ |
| 222 |
Eigen::Vector3d angle = Eigen::Vector3d::Random(); |
| 223 |
testEulerAngles(angle); |
| 224 |
} |
| 225 |
} |
| 226 |
|
| 227 |
/******************************************************************************/ |
| 228 |
#define EPSILON_EXPMAP_THETA 1.0e-3 |
| 229 |
TEST(LIE_GROUP_OPERATORS, EXPONENTIAL_MAPPINGS) |
| 230 |
{ |
| 231 |
int numTest = 100; |
| 232 |
|
| 233 |
// Exp |
| 234 |
for (int i = 0; i < numTest; ++i) |
| 235 |
{ |
| 236 |
Eigen::Vector6d s = Eigen::Vector6d::Random(); |
| 237 |
Eigen::Isometry3d Exp_s = math::expMap(s); |
| 238 |
Eigen::Matrix4d Exp_s_2 = Eigen::Matrix4d::Identity(); |
| 239 |
|
| 240 |
double theta = s.head<3>().norm(); |
| 241 |
Eigen::Matrix3d R = Matrix3d::Zero(); |
| 242 |
Eigen::Matrix3d qss = math::makeSkewSymmetric(s.head<3>()); |
| 243 |
Eigen::Matrix3d qss2 = qss * qss; |
| 244 |
Eigen::Matrix3d P = Eigen::Matrix3d::Zero(); |
| 245 |
|
| 246 |
if (theta < EPSILON_EXPMAP_THETA) |
| 247 |
{ |
| 248 |
R = Matrix3d::Identity() + qss + 0.5 * qss2; |
| 249 |
P = Matrix3d::Identity() + 0.5 * qss + (1 / 6) * qss2; |
| 250 |
} |
| 251 |
else |
| 252 |
{ |
| 253 |
R = Matrix3d::Identity() + (sin(theta) / theta) * qss |
| 254 |
+ ((1 - cos(theta)) / (theta * theta)) * qss2; |
| 255 |
P = Matrix3d::Identity() + ((1 - cos(theta)) / (theta * theta)) * qss |
| 256 |
+ ((theta - sin(theta)) / (theta * theta * theta)) * qss2; |
| 257 |
} |
| 258 |
|
| 259 |
Exp_s_2.topLeftCorner<3, 3>() = R; |
| 260 |
Exp_s_2.topRightCorner<3, 1>() = P * s.tail<3>(); |
| 261 |
|
| 262 |
// |
| 263 |
for (int i = 0; i < 4; ++i) |
| 264 |
for (int j = 0; j < 4; ++j) |
| 265 |
EXPECT_NEAR(Exp_s(i, j), Exp_s_2(i, j), LIE_GROUP_OPT_TOL); |
| 266 |
} |
| 267 |
|
| 268 |
// ExpAngular |
| 269 |
for (int i = 0; i < numTest; ++i) |
| 270 |
{ |
| 271 |
Eigen::Vector6d s = Eigen::Vector6d::Random(); |
| 272 |
s.tail<3>() = Eigen::Vector3d::Zero(); |
| 273 |
Eigen::Isometry3d Exp_s = math::expAngular(s.head<3>()); |
| 274 |
Eigen::Matrix4d Exp_s_2 = Eigen::Matrix4d::Identity(); |
| 275 |
|
| 276 |
double theta = s.head<3>().norm(); |
| 277 |
Eigen::Matrix3d R = Matrix3d::Zero(); |
| 278 |
Eigen::Matrix3d qss = math::makeSkewSymmetric(s.head<3>()); |
| 279 |
Eigen::Matrix3d qss2 = qss * qss; |
| 280 |
Eigen::Matrix3d P = Eigen::Matrix3d::Zero(); |
| 281 |
|
| 282 |
if (theta < EPSILON_EXPMAP_THETA) |
| 283 |
{ |
| 284 |
R = Matrix3d::Identity() + qss + 0.5 * qss2; |
| 285 |
P = Matrix3d::Identity() + 0.5 * qss + (1 / 6) * qss2; |
| 286 |
} |
| 287 |
else |
| 288 |
{ |
| 289 |
R = Matrix3d::Identity() + (sin(theta) / theta) * qss |
| 290 |
+ ((1 - cos(theta)) / (theta * theta)) * qss2; |
| 291 |
P = Matrix3d::Identity() + ((1 - cos(theta)) / (theta * theta)) * qss |
| 292 |
+ ((theta - sin(theta)) / (theta * theta * theta)) * qss2; |
| 293 |
} |
| 294 |
|
| 295 |
Exp_s_2.topLeftCorner<3, 3>() = R; |
| 296 |
Exp_s_2.topRightCorner<3, 1>() = P * s.tail<3>(); |
| 297 |
|
| 298 |
// |
| 299 |
for (int i = 0; i < 4; ++i) |
| 300 |
for (int j = 0; j < 4; ++j) |
| 301 |
EXPECT_NEAR(Exp_s(i, j), Exp_s_2(i, j), LIE_GROUP_OPT_TOL); |
| 302 |
} |
| 303 |
|
| 304 |
// ExpLinear |
| 305 |
for (int i = 0; i < numTest; ++i) |
| 306 |
{ |
| 307 |
Eigen::Vector6d s = Eigen::Vector6d::Random(); |
| 308 |
s.head<3>() = Eigen::Vector3d::Zero(); |
| 309 |
Eigen::Isometry3d Exp_s(Eigen::Translation3d(s.tail<3>())); |
| 310 |
Eigen::Matrix4d Exp_s_2 = Eigen::Matrix4d::Identity(); |
| 311 |
|
| 312 |
double theta = s.head<3>().norm(); |
| 313 |
Eigen::Matrix3d R = Matrix3d::Zero(); |
| 314 |
Eigen::Matrix3d qss = math::makeSkewSymmetric(s.head<3>()); |
| 315 |
Eigen::Matrix3d qss2 = qss * qss; |
| 316 |
Eigen::Matrix3d P = Eigen::Matrix3d::Zero(); |
| 317 |
|
| 318 |
if (theta < EPSILON_EXPMAP_THETA) |
| 319 |
{ |
| 320 |
R = Matrix3d::Identity() + qss + 0.5 * qss2; |
| 321 |
P = Matrix3d::Identity() + 0.5 * qss + (1 / 6) * qss2; |
| 322 |
} |
| 323 |
else |
| 324 |
{ |
| 325 |
R = Matrix3d::Identity() + (sin(theta) / theta) * qss |
| 326 |
+ ((1 - cos(theta)) / (theta * theta)) * qss2; |
| 327 |
P = Matrix3d::Identity() + ((1 - cos(theta)) / (theta * theta)) * qss |
| 328 |
+ ((theta - sin(theta)) / (theta * theta * theta)) * qss2; |
| 329 |
} |
| 330 |
|
| 331 |
Exp_s_2.topLeftCorner<3, 3>() = R; |
| 332 |
Exp_s_2.topRightCorner<3, 1>() = P * s.tail<3>(); |
| 333 |
|
| 334 |
// |
| 335 |
for (int i = 0; i < 4; ++i) |
| 336 |
for (int j = 0; j < 4; ++j) |
| 337 |
EXPECT_NEAR(Exp_s(i, j), Exp_s_2(i, j), LIE_GROUP_OPT_TOL); |
| 338 |
} |
| 339 |
// Exponential mapping test with high values |
| 340 |
int numExpTests = 100; |
| 341 |
double min = -1e+128; |
| 342 |
double max = +1e+128; |
| 343 |
|
| 344 |
for (int idxTest = 0; idxTest < numExpTests; ++idxTest) |
| 345 |
{ |
| 346 |
Eigen::Vector3d randomS = Eigen::Vector3d::Zero(); |
| 347 |
|
| 348 |
for (int i = 0; i < 3; ++i) |
| 349 |
randomS[i] = Random::uniform(min, max); |
| 350 |
|
| 351 |
Eigen::Isometry3d T = math::expAngular(randomS); |
| 352 |
EXPECT_TRUE(math::verifyTransform(T)); |
| 353 |
} |
| 354 |
|
| 355 |
for (int idxTest = 0; idxTest < numExpTests; ++idxTest) |
| 356 |
{ |
| 357 |
Eigen::Vector6d randomS = Eigen::Vector6d::Zero(); |
| 358 |
|
| 359 |
for (int i = 0; i < 6; ++i) |
| 360 |
randomS[i] = Random::uniform(min, max); |
| 361 |
|
| 362 |
Eigen::Isometry3d T = math::expMap(randomS); |
| 363 |
EXPECT_TRUE(math::verifyTransform(T)); |
| 364 |
} |
| 365 |
} |
| 366 |
|
| 367 |
/******************************************************************************/ |
| 368 |
TEST(LIE_GROUP_OPERATORS, ADJOINT_MAPPINGS) |
| 369 |
{ |
| 370 |
int numTest = 100; |
| 371 |
|
| 372 |
// AdT(V) == T * V * InvT |
| 373 |
for (int i = 0; i < numTest; ++i) |
| 374 |
{ |
| 375 |
Eigen::Vector6d t = Eigen::Vector6d::Random(); |
| 376 |
Eigen::Isometry3d T = math::expMap(t); |
| 377 |
Eigen::Vector6d V = Eigen::Vector6d::Random(); |
| 378 |
|
| 379 |
Eigen::Vector6d AdTV = AdT(T, V); |
| 380 |
|
| 381 |
// Ad(T, V) = T * [V] * InvT |
| 382 |
Eigen::Matrix4d T_V_InvT |
| 383 |
= T.matrix() * toMatrixForm(V) * T.inverse().matrix(); |
| 384 |
Eigen::Vector6d T_V_InvT_se3 = fromMatrixForm(T_V_InvT); |
| 385 |
|
| 386 |
for (int j = 0; j < 6; ++j) |
| 387 |
EXPECT_NEAR(AdTV(j), T_V_InvT_se3(j), LIE_GROUP_OPT_TOL); |
| 388 |
|
| 389 |
// Ad(T, V) = [R 0; [p]R R] * V |
| 390 |
Eigen::Matrix6d AdTMatrix = Eigen::Matrix6d::Zero(); |
| 391 |
AdTMatrix.topLeftCorner<3, 3>() = T.linear(); |
| 392 |
AdTMatrix.bottomRightCorner<3, 3>() = T.linear(); |
| 393 |
AdTMatrix.bottomLeftCorner<3, 3>() |
| 394 |
= math::makeSkewSymmetric(T.translation()) * T.linear(); |
| 395 |
Eigen::Vector6d AdTMatrix_V = AdTMatrix * V; |
| 396 |
for (int j = 0; j < 6; ++j) |
| 397 |
EXPECT_NEAR(AdTV(j), AdTMatrix_V(j), LIE_GROUP_OPT_TOL); |
| 398 |
} |
| 399 |
|
| 400 |
// AdR == AdT([R 0; 0 1], V) |
| 401 |
for (int i = 0; i < numTest; ++i) |
| 402 |
{ |
| 403 |
Eigen::Vector6d t = Eigen::Vector6d::Random(); |
| 404 |
Eigen::Isometry3d T = math::expMap(t); |
| 405 |
Eigen::Isometry3d R = Eigen::Isometry3d::Identity(); |
| 406 |
R.linear() = T.linear(); |
| 407 |
Eigen::Vector6d V = Eigen::Vector6d::Random(); |
| 408 |
|
| 409 |
Eigen::Vector6d AdTV = AdT(R, V); |
| 410 |
Eigen::Vector6d AdRV = AdR(T, V); |
| 411 |
|
| 412 |
for (int j = 0; j < 6; ++j) |
| 413 |
EXPECT_NEAR(AdTV(j), AdRV(j), LIE_GROUP_OPT_TOL); |
| 414 |
} |
| 415 |
|
| 416 |
// AdTAngular == AdT(T, se3(w, 0)) |
| 417 |
for (int i = 0; i < numTest; ++i) |
| 418 |
{ |
| 419 |
Eigen::Vector6d t = Eigen::Vector6d::Random(); |
| 420 |
Eigen::Isometry3d T = math::expMap(t); |
| 421 |
Eigen::Vector3d w = Eigen::Vector3d::Random(); |
| 422 |
Eigen::Vector6d V = Eigen::Vector6d::Zero(); |
| 423 |
V.head<3>() = w; |
| 424 |
|
| 425 |
Eigen::Vector6d AdTV = AdT(T, V); |
| 426 |
Eigen::Vector6d AdTAng = AdTAngular(T, w); |
| 427 |
|
| 428 |
for (int j = 0; j < 6; ++j) |
| 429 |
EXPECT_NEAR(AdTV(j), AdTAng(j), LIE_GROUP_OPT_TOL); |
| 430 |
} |
| 431 |
|
| 432 |
// AdTLinear == AdT(T, se3(w, 0)) |
| 433 |
for (int i = 0; i < numTest; ++i) |
| 434 |
{ |
| 435 |
Eigen::Vector6d t = Eigen::Vector6d::Random(); |
| 436 |
Eigen::Isometry3d T = math::expMap(t); |
| 437 |
Eigen::Vector3d v = Eigen::Vector3d::Random(); |
| 438 |
Eigen::Vector6d V = Eigen::Vector6d::Zero(); |
| 439 |
V.tail<3>() = v; |
| 440 |
|
| 441 |
Eigen::Vector6d AdTV = AdT(T, V); |
| 442 |
Eigen::Vector6d AdTLin = AdTLinear(T, v); |
| 443 |
|
| 444 |
for (int j = 0; j < 6; ++j) |
| 445 |
EXPECT_NEAR(AdTV(j), AdTLin(j), LIE_GROUP_OPT_TOL); |
| 446 |
} |
| 447 |
|
| 448 |
// AdTJac |
| 449 |
for (int i = 0; i < numTest; ++i) |
| 450 |
{ |
| 451 |
Eigen::Vector6d t = Eigen::Vector6d::Random(); |
| 452 |
Eigen::Isometry3d T = math::expMap(t); |
| 453 |
Eigen::Vector3d v = Eigen::Vector3d::Random(); |
| 454 |
Eigen::Vector6d V = Eigen::Vector6d::Zero(); |
| 455 |
V.tail<3>() = v; |
| 456 |
|
| 457 |
Eigen::Vector6d AdTV = AdT(T, V); |
| 458 |
Eigen::Vector6d AdTLin = AdTLinear(T, v); |
| 459 |
|
| 460 |
for (int j = 0; j < 6; ++j) |
| 461 |
EXPECT_NEAR(AdTV(j), AdTLin(j), LIE_GROUP_OPT_TOL); |
| 462 |
} |
| 463 |
|
| 464 |
// AdInvT |
| 465 |
for (int i = 0; i < numTest; ++i) |
| 466 |
{ |
| 467 |
Eigen::Vector6d t = Eigen::Vector6d::Random(); |
| 468 |
Eigen::Isometry3d T = math::expMap(t); |
| 469 |
Eigen::Isometry3d InvT = T.inverse(); |
| 470 |
Eigen::Vector6d V = Eigen::Vector6d::Random(); |
| 471 |
|
| 472 |
Eigen::Vector6d Ad_InvT = AdT(InvT, V); |
| 473 |
Eigen::Vector6d AdInv_T = AdInvT(T, V); |
| 474 |
|
| 475 |
for (int j = 0; j < 6; ++j) |
| 476 |
EXPECT_NEAR(Ad_InvT(j), AdInv_T(j), LIE_GROUP_OPT_TOL); |
| 477 |
} |
| 478 |
|
| 479 |
// AdInvRLinear |
| 480 |
for (int i = 0; i < numTest; ++i) |
| 481 |
{ |
| 482 |
Eigen::Vector6d t = Eigen::Vector6d::Random(); |
| 483 |
Eigen::Isometry3d T = math::expMap(t); |
| 484 |
Eigen::Vector3d v = Eigen::Vector3d::Random(); |
| 485 |
Eigen::Vector6d V = Eigen::Vector6d::Zero(); |
| 486 |
V.tail<3>() = v; |
| 487 |
Eigen::Isometry3d R = Eigen::Isometry3d::Identity(); |
| 488 |
R.linear() = T.linear(); |
| 489 |
|
| 490 |
Eigen::Vector6d AdT_ = AdT(R.inverse(), V); |
| 491 |
Eigen::Vector6d AdInvRLinear_ = AdInvRLinear(T, v); |
| 492 |
|
| 493 |
for (int j = 0; j < 6; ++j) |
| 494 |
EXPECT_NEAR(AdT_(j), AdInvRLinear_(j), LIE_GROUP_OPT_TOL); |
| 495 |
} |
| 496 |
|
| 497 |
// dAdT |
| 498 |
for (int i = 0; i < numTest; ++i) |
| 499 |
{ |
| 500 |
Eigen::Vector6d t = Eigen::Vector6d::Random(); |
| 501 |
Eigen::Isometry3d T = math::expMap(t); |
| 502 |
Eigen::Vector6d F = Eigen::Vector6d::Random(); |
| 503 |
|
| 504 |
Eigen::Vector6d dAdTF = dAdT(T, F); |
| 505 |
|
| 506 |
// dAd(T, F) = [R 0; [p]R R]^T * F |
| 507 |
Eigen::Matrix6d AdTMatrix = Eigen::Matrix6d::Zero(); |
| 508 |
AdTMatrix.topLeftCorner<3, 3>() = T.linear(); |
| 509 |
AdTMatrix.bottomRightCorner<3, 3>() = T.linear(); |
| 510 |
AdTMatrix.bottomLeftCorner<3, 3>() |
| 511 |
= math::makeSkewSymmetric(T.translation()) * T.linear(); |
| 512 |
Eigen::Vector6d AdTTransMatrix_V = AdTMatrix.transpose() * F; |
| 513 |
for (int j = 0; j < 6; ++j) |
| 514 |
EXPECT_NEAR(dAdTF(j), AdTTransMatrix_V(j), LIE_GROUP_OPT_TOL); |
| 515 |
} |
| 516 |
|
| 517 |
// dAdInvT |
| 518 |
for (int i = 0; i < numTest; ++i) |
| 519 |
{ |
| 520 |
Eigen::Vector6d t = Eigen::Vector6d::Random(); |
| 521 |
Eigen::Isometry3d T = math::expMap(t); |
| 522 |
Eigen::Isometry3d InvT = T.inverse(); |
| 523 |
Eigen::Vector6d F = Eigen::Vector6d::Random(); |
| 524 |
|
| 525 |
Eigen::Vector6d dAdInvT_F = dAdInvT(T, F); |
| 526 |
|
| 527 |
// |
| 528 |
Eigen::Vector6d dAd_InvTF = dAdT(InvT, F); |
| 529 |
|
| 530 |
for (int j = 0; j < 6; ++j) |
| 531 |
EXPECT_NEAR(dAdInvT_F(j), dAd_InvTF(j), LIE_GROUP_OPT_TOL); |
| 532 |
|
| 533 |
// dAd(T, F) = [R 0; [p]R R]^T * F |
| 534 |
Eigen::Matrix6d AdInvTMatrix = Eigen::Matrix6d::Zero(); |
| 535 |
AdInvTMatrix.topLeftCorner<3, 3>() = InvT.linear(); |
| 536 |
AdInvTMatrix.bottomRightCorner<3, 3>() = InvT.linear(); |
| 537 |
AdInvTMatrix.bottomLeftCorner<3, 3>() |
| 538 |
= math::makeSkewSymmetric(InvT.translation()) * InvT.linear(); |
| 539 |
Eigen::Vector6d AdInvTTransMatrix_V = AdInvTMatrix.transpose() * F; |
| 540 |
for (int j = 0; j < 6; ++j) |
| 541 |
EXPECT_NEAR(dAdInvT_F(j), AdInvTTransMatrix_V(j), LIE_GROUP_OPT_TOL); |
| 542 |
} |
| 543 |
|
| 544 |
// dAdInvR |
| 545 |
for (int i = 0; i < numTest; ++i) |
| 546 |
{ |
| 547 |
Eigen::Vector6d t = Eigen::Vector6d::Random(); |
| 548 |
Eigen::Isometry3d T = math::expMap(t); |
| 549 |
Eigen::Isometry3d InvT = T.inverse(); |
| 550 |
Eigen::Isometry3d InvR = Eigen::Isometry3d::Identity(); |
| 551 |
InvR.linear() = InvT.linear(); |
| 552 |
Eigen::Vector6d F = Eigen::Vector6d::Random(); |
| 553 |
|
| 554 |
Eigen::Vector6d dAdInvR_F = dAdInvR(T, F); |
| 555 |
|
| 556 |
// |
| 557 |
Eigen::Vector6d dAd_InvTF = dAdT(InvR, F); |
| 558 |
|
| 559 |
for (int j = 0; j < 6; ++j) |
| 560 |
EXPECT_NEAR(dAdInvR_F(j), dAd_InvTF(j), LIE_GROUP_OPT_TOL); |
| 561 |
} |
| 562 |
|
| 563 |
// ad |
| 564 |
for (int i = 0; i < numTest; ++i) |
| 565 |
{ |
| 566 |
Eigen::Vector6d V = Eigen::Vector6d::Random(); |
| 567 |
Eigen::Vector6d W = Eigen::Vector6d::Random(); |
| 568 |
|
| 569 |
Eigen::Vector6d ad_V_W = ad(V, W); |
| 570 |
|
| 571 |
// |
| 572 |
Eigen::Matrix6d adV_Matrix = Eigen::Matrix6d::Zero(); |
| 573 |
adV_Matrix.topLeftCorner<3, 3>() = math::makeSkewSymmetric(V.head<3>()); |
| 574 |
adV_Matrix.bottomRightCorner<3, 3>() = math::makeSkewSymmetric(V.head<3>()); |
| 575 |
adV_Matrix.bottomLeftCorner<3, 3>() = math::makeSkewSymmetric(V.tail<3>()); |
| 576 |
Eigen::Vector6d adV_Matrix_W = adV_Matrix * W; |
| 577 |
|
| 578 |
for (int j = 0; j < 6; ++j) |
| 579 |
EXPECT_NEAR(ad_V_W(j), adV_Matrix_W(j), LIE_GROUP_OPT_TOL); |
| 580 |
} |
| 581 |
|
| 582 |
// dad |
| 583 |
for (int i = 0; i < numTest; ++i) |
| 584 |
{ |
| 585 |
Eigen::Vector6d V = Eigen::Vector6d::Random(); |
| 586 |
Eigen::Vector6d F = Eigen::Vector6d::Random(); |
| 587 |
|
| 588 |
Eigen::Vector6d dad_V_F = dad(V, F); |
| 589 |
|
| 590 |
// |
| 591 |
Eigen::Matrix6d dadV_Matrix = Eigen::Matrix6d::Zero(); |
| 592 |
dadV_Matrix.topLeftCorner<3, 3>() = math::makeSkewSymmetric(V.head<3>()); |
| 593 |
dadV_Matrix.bottomRightCorner<3, 3>() |
| 594 |
= math::makeSkewSymmetric(V.head<3>()); |
| 595 |
dadV_Matrix.bottomLeftCorner<3, 3>() = math::makeSkewSymmetric(V.tail<3>()); |
| 596 |
Eigen::Vector6d dadV_Matrix_F = dadV_Matrix.transpose() * F; |
| 597 |
|
| 598 |
for (int j = 0; j < 6; ++j) |
| 599 |
EXPECT_NEAR(dad_V_F(j), dadV_Matrix_F(j), LIE_GROUP_OPT_TOL); |
| 600 |
} |
| 601 |
} |