1.本文要点说明
介绍视觉定位导航中位姿变换的基础知识，包括：
(1)位姿变换模型
(2)左乘右乘法则
(3)模型几何解释
(4)基本运算公式
(5)旋转转换公式
2.实验测试代码
依赖于OpenCV、Ceres和Spdlog，封装在类ModelRotation：
(0)pi、deg2rad、rad2deg、tsns、tsus、tsms、tsse、tsmi、tsho
(1)RVec2Quat、Quat2RMat、RVec2RMat、Euler2RMat
(2)Quat2RVec、RMat2Quat、RMat2RVec、RMat2Euler
(3)SolidPose：getRot、setPos、setRot、inv、mul、print
1 #ifndef __ModelRotation_h__ 2 #define __ModelRotation_h__ 3 4 #include <ceres/ceres.h> 5 #include <ceres/rotation.h> 6 #include <spdlog/spdlog.h> 7 #include <opencv2/opencv.hpp> 8 using namespace std; 9 using namespace cv; 10 11 constexpr static double pi = 3.1415926535897932384626433832795; 12 constexpr static double deg2rad = (pi * 0.0055555555555555556); 13 constexpr static double rad2deg = (180 * 0.3183098861837906715); 14 15 #ifndef tsns 16 #define tsns chrono::time_point_cast<chrono::nanoseconds>(chrono::system_clock::now()).time_since_epoch().count() 17 #define tsus chrono::time_point_cast<chrono::microseconds>(chrono::system_clock::now()).time_since_epoch().count() 18 #define tsms chrono::time_point_cast<chrono::milliseconds>(chrono::system_clock::now()).time_since_epoch().count() 19 #define tsse chrono::time_point_cast<chrono::seconds>(chrono::system_clock::now()).time_since_epoch().count() 20 #define tsmi chrono::time_point_cast<chrono::minutes>(chrono::system_clock::now()).time_since_epoch().count() 21 #define tsho chrono::time_point_cast<chrono::hours>(chrono::system_clock::now()).time_since_epoch().count() 22 #endif 23 24 class ModelRotation 25 { 26 public: 27 constexpr static double L1EPS = 1E-12; 28 constexpr static double L2EPS = 1E-18; 29 30 public://RVec2Quat 31 struct RVec2Quat 32 { 33 bool useCeres; 34 RVec2Quat(bool useCeres0) : useCeres(useCeres0) {} 35 template <typename tp> bool operator()(const tp* const src, tp* dst) const 36 { 37 if (useCeres) { ceres::AngleAxisToQuaternion(src, dst); return true; } 38 tp theta2 = src[0] * src[0] + src[1] * src[1] + src[2] * src[2]; 39 if (theta2 > ModelRotation::L2EPS) 40 { 41 tp theta = sqrt(theta2); 42 tp thetaa = theta * 0.5; 43 tp coeff = sin(thetaa) / theta; 44 dst[0] = cos(thetaa); 45 dst[1] = src[0] * coeff; 46 dst[2] = src[1] * coeff; 47 dst[3] = src[2] * coeff; 48 } 49 else //first order taylor 50 { 51 dst[0] = tp(1.); 52 dst[1] = src[0] * 0.5; 53 dst[2] = src[1] * 0.5; 54 dst[3] = src[2] * 0.5; 55 } 56 return true; 57 } 58 static void TestMe(int argc = 0, char** argv = 0) 59 { 60 for (int k = 0; k < 999; ++k) 61 { 62 //0.GenerateData 63 Matx31d r = r.randu(-999, 999); 64 65 //1.CalcByCeres 66 Vec4d q1; 67 Mat_<double> dqdr1(4, 3); 68 ceres::CostFunction* costFun1 = new ceres::AutoDiffCostFunction<RVec2Quat, 4, 3>(new RVec2Quat(true)); 69 costFun1->Evaluate(vector<double*>{r.val}.data(), q1.val, vector<double*>{dqdr1.ptr<double>()}.data()); 70 71 //2.CalcByManu 72 Vec4d q2; 73 Mat_<double> dqdr2(4, 3); 74 ceres::CostFunction* costFun2 = new ceres::AutoDiffCostFunction<RVec2Quat, 4, 3>(new RVec2Quat(false)); 75 costFun2->Evaluate(vector<double*>{r.val}.data(), q2.val, vector<double*>{dqdr2.ptr<double>()}.data()); 76 77 //3.AnalyzeError 78 double infq1q2 = cv::norm(q1, q2, NORM_INF); 79 double infdqdr1dqdr2 = cv::norm(dqdr1, dqdr2, NORM_INF); 80 81 //4.PrintError 82 cout << endl << "LoopCount: " << k << endl; 83 if (infq1q2 > 1E-12 || infdqdr1dqdr2 > 1E-12) 84 { 85 cout << endl << "infq1q2: " << infq1q2 << endl; 86 cout << endl << "infdqdr1dqdr2: " << infdqdr1dqdr2 << endl; 87 if (0) 88 { 89 cout << endl << "q1: " << q1 << endl; 90 cout << endl << "q2: " << q2 << endl; 91 cout << endl << "dqdr1: " << dqdr1 << endl; 92 cout << endl << "dqdr2: " << dqdr2 << endl; 93 } 94 cout << endl << "Press any key to continue" << endl; getchar(); 95 } 96 } 97 } 98 }; 99 struct Quat2RVec 100 { 101 bool useCeres; 102 Quat2RVec(bool useCeres0) : useCeres(useCeres0) {} 103 template <typename tp> bool operator()(const tp* const src, tp* dst) const 104 { 105 if (useCeres) { ceres::QuaternionToAngleAxis(src, dst); return true; } 106 tp thetaa2 = src[1] * src[1] + src[2] * src[2] + src[3] * src[3]; 107 if (thetaa2 > ModelRotation::L2EPS) 108 { 109 tp cs = src[0]; 110 tp sn = sqrt(thetaa2); 111 tp theta = 2.0 * (cs < 0.0 ? atan2(-sn, -cs) : atan2(sn, cs));//refer to ceres comments 112 tp coeff = theta / sn; 113 dst[0] = src[1] * coeff; 114 dst[1] = src[2] * coeff; 115 dst[2] = src[3] * coeff; 116 } 117 else //first order taylor 118 { 119 dst[0] = 2.0 * src[1]; 120 dst[1] = 2.0 * src[2]; 121 dst[2] = 2.0 * src[3]; 122 } 123 return true; 124 } 125 static void TestMe(int argc = 0, char** argv = 0) 126 { 127 for (int k = 0; k < 999; ++k) 128 { 129 //0.GenerateData 130 Matx41d q = q.randu(-999, 999); 131 132 //1.CalcByCeres 133 Matx33d r1; 134 Mat_<double> drdq1(3, 4); 135 ceres::CostFunction* costFun1 = new ceres::AutoDiffCostFunction<Quat2RVec, 3, 4>(new Quat2RVec(true)); 136 costFun1->Evaluate(vector<double*>{q.val}.data(), r1.val, vector<double*>{drdq1.ptr<double>()}.data()); 137 138 //2.CalcByManu 139 Matx33d r2; 140 Mat_<double> drdq2(3, 4); 141 ceres::CostFunction* costFun2 = new ceres::AutoDiffCostFunction<Quat2RVec, 3, 4>(new Quat2RVec(false)); 142 costFun2->Evaluate(vector<double*>{q.val}.data(), r2.val, vector<double*>{drdq2.ptr<double>()}.data()); 143 144 //3.AnalyzeError 145 double infr1r2 = cv::norm(r1, r2, NORM_INF); 146 double infdrdq1drdq2 = cv::norm(drdq1, drdq2, NORM_INF); 147 148 //4.PrintError 149 cout << endl << "LoopCount: " << k << endl; 150 if (infr1r2 > 1E-12 || infdrdq1drdq2 > 1E-12) 151 { 152 cout << endl << "infr1r2: " << infr1r2 << endl; 153 cout << endl << "infdrdq1drdq2: " << infdrdq1drdq2 << endl; 154 if (0) 155 { 156 cout << endl << "r1: " << r1 << endl; 157 cout << endl << "r2: " << r2 << endl; 158 cout << endl << "drdq1: " << drdq1 << endl; 159 cout << endl << "drdq2: " << drdq2 << endl; 160 } 161 cout << endl << "Press any key to continue" << endl; getchar(); 162 } 163 } 164 } 165 }; 166 167 public://Quat2RMat 168 struct Quat2RMat 169 { 170 bool useCeres; 171 Quat2RMat(bool useCeres0) : useCeres(useCeres0) {} 172 template <typename tp> bool operator()(const tp* const src, tp* dst) const 173 { 174 if (useCeres) { ceres::QuaternionToRotation(src, dst); return true; } 175 tp ww = src[0] * src[0]; 176 tp wx = src[0] * src[1]; 177 tp wy = src[0] * src[2]; 178 tp wz = src[0] * src[3]; 179 180 tp xx = src[1] * src[1]; 181 tp xy = src[1] * src[2]; 182 tp xz = src[1] * src[3]; 183 184 tp yy = src[2] * src[2]; 185 tp yz = src[2] * src[3]; 186 187 tp zz = src[3] * src[3]; 188 tp L2 = ww + xx + yy + zz; 189 if (L2 < ModelRotation::L2EPS) { dst[0] = dst[4] = dst[8] = tp(1.); dst[1] = dst[2] = dst[3] = dst[5] = dst[6] = dst[7] = tp(0.); return true; } 190 tp iL2 = 1. / L2; 191 192 dst[0] = ww + xx - yy - zz; dst[1] = 2.0 * (xy - wz); dst[2] = 2.0 * (wy + xz); 193 dst[3] = 2.0 * (wz + xy); dst[4] = ww - xx + yy - zz; dst[5] = 2.0 * (yz - wx); 194 dst[6] = 2.0 * (xz - wy); dst[7] = 2.0 * (wx + yz); dst[8] = ww - xx - yy + zz; 195 for (int k = 0; k < 9; ++k) dst[k] = dst[k] * iL2;//for not unit quaternion 196 197 return true; 198 } 199 static void TestMe(int argc = 0, char** argv = 0) 200 { 201 for (int k = 0; k < 999; ++k) 202 { 203 //0.GenerateData 204 Matx41d q = q.randu(-999, 999); 205 206 //1.CalcByCeres 207 Matx33d R1; 208 Mat_<double> dRdq1(9, 4); 209 ceres::CostFunction* costFun1 = new ceres::AutoDiffCostFunction<Quat2RMat, 9, 4>(new Quat2RMat(true)); 210 costFun1->Evaluate(vector<double*>{q.val}.data(), R1.val, vector<double*>{dRdq1.ptr<double>()}.data()); 211 212 //2.CalcByManu 213 Matx33d R2; 214 Mat_<double> dRdq2(9, 4); 215 ceres::CostFunction* costFun2 = new ceres::AutoDiffCostFunction<Quat2RMat, 9, 4>(new Quat2RMat(false)); 216 costFun2->Evaluate(vector<double*>{q.val}.data(), R2.val, vector<double*>{dRdq2.ptr<double>()}.data()); 217 218 //3.AnalyzeError 219 double infR1R2 = cv::norm(R1, R2, NORM_INF); 220 double infdRdq1dRdq2 = cv::norm(dRdq1, dRdq2, NORM_INF); 221 222 //4.PrintError 223 cout << endl << "LoopCount: " << k << endl; 224 if (infR1R2 > 1E-12 || infdRdq1dRdq2 > 1E-12) 225 { 226 cout << endl << "infR1R2: " << infR1R2 << endl; 227 cout << endl << "infdRdq1dRdq2: " << infdRdq1dRdq2 << endl; 228 if (0) 229 { 230 cout << endl << "R1: " << R1 << endl; 231 cout << endl << "R2: " << R2 << endl; 232 cout << endl << "dRdq1: " << dRdq1 << endl; 233 cout << endl << "dRdq2: " << dRdq2 << endl; 234 } 235 cout << endl << "Press any key to continue" << endl; getchar(); 236 } 237 } 238 } 239 }; 240 struct RMat2Quat 241 { 242 bool useCeres; 243 RMat2Quat(bool useCeres0) : useCeres(useCeres0) {} 244 template <typename tp> bool operator()(const tp* const src, tp* dst) const 245 { 246 if (useCeres) { ceres::RotationMatrixToQuaternion(ceres::RowMajorAdapter3x3(src), dst); return true; } 247 tp trace = src[0] + src[4] + src[8];//Strictly 1+trace>0 for rotation matrix. In ceres, This leads to different jacobians between ceres' rmat2rvec and cv::Rodrigues(rmat, rvec) 248 if (trace >= 0.0) 249 { 250 tp trace1 = sqrt(1. + trace); 251 dst[0] = 0.5 * trace1; 252 tp itrace1 = 0.5 / trace1; 253 dst[1] = (src[7] - src[5]) * itrace1; 254 dst[2] = (src[2] - src[6]) * itrace1; 255 dst[3] = (src[3] - src[1]) * itrace1; 256 } 257 else 258 { 259 int a = 0; 260 if (src[4] > src[0]) a = 1; 261 if (src[8] > src[a * 3 + a]) a = 2; 262 int b = (a + 1) % 3; 263 int c = (b + 1) % 3; 264 265 tp trace1 = sqrt(1.0 + src[a * 3 + a] - src[b * 3 + b] - src[c * 3 + c]); 266 dst[a + 1] = 0.5 * trace1; 267 tp itrace1 = 0.5 / trace1; 268 dst[0] = (src[c * 3 + b] - src[b * 3 + c]) * itrace1; 269 dst[b + 1] = (src[b * 3 + a] + src[a * 3 + b]) * itrace1; 270 dst[c + 1] = (src[c * 3 + a] + src[a * 3 + c]) * itrace1; 271 } 272 return true; 273 } 274 static void TestMe(int argc, char** argv) 275 { 276 for (int k = 0; k < 999; ++k) 277 { 278 //0.GenerateData 279 Matx33d R; ceres::EulerAnglesToRotationMatrix(Matx31d::randu(-180, 180).val, 0, R.val); 280 281 //1.CalcByCeres 282 Vec4d q1; 283 Mat_<double> dqdR1(4, 9); 284 ceres::CostFunction* costFun1 = new ceres::AutoDiffCostFunction<RMat2Quat, 4, 9>(new RMat2Quat(true)); 285 costFun1->Evaluate(vector<double*>{R.val}.data(), q1.val, vector<double*>{dqdR1.ptr<double>()}.data()); 286 287 //2.CalcByManu 288 Vec4d q2; 289 Mat_<double> dqdR2(4, 9); 290 ceres::CostFunction* costFun2 = new ceres::AutoDiffCostFunction<RMat2Quat, 4, 9>(new RMat2Quat(false)); 291 costFun2->Evaluate(vector<double*>{R.val}.data(), q2.val, vector<double*>{dqdR2.ptr<double>()}.data()); 292 293 //4.AnalyzeError 294 double infq1q2 = cv::norm(q1, q2, NORM_INF); 295 double infdqdR1dqdR2 = cv::norm(dqdR1, dqdR2, NORM_INF); 296 297 //5.PrintError 298 cout << endl << "LoopCount: " << k << endl; 299 if (infq1q2 > 1E-9 || infdqdR1dqdR2 > 1E-9) 300 { 301 cout << endl << "infq1q2: " << infq1q2 << endl; 302 cout << endl << "infdqdR1dqdR2: " << infq1q2 << endl; 303 if (0) 304 { 305 cout << endl << "q1:" << q1 << endl; 306 cout << endl << "q2:" << q2 << endl; 307 cout << endl << "dqdR1:" << dqdR1 << endl; 308 cout << endl << "dqdR2:" << dqdR2 << endl; 309 } 310 cout << endl << "Press any key to continue" << endl; getchar(); 311 } 312 } 313 } 314 }; 315 316 public://RVec2RMat 317 struct RVec2RMat 318 { 319 bool useCeres; 320 RVec2RMat(bool useCeres0) : useCeres(useCeres0) {} 321 template <typename tp> bool operator()(const tp* const src, tp* dst) const 322 { 323 if (useCeres) { ceres::AngleAxisToRotationMatrix(src, ceres::RowMajorAdapter3x3(dst)); return true; } 324 tp theta2 = src[0] * src[0] + src[1] * src[1] + src[2] * src[2]; 325 if (theta2 > ModelRotation::L2EPS) 326 { 327 tp theta = sqrt(theta2); 328 tp itheta = 1. / theta; 329 tp cs = cos(theta); 330 tp sn = sin(theta); 331 tp nx = src[0] * itheta; 332 tp ny = src[1] * itheta; 333 tp nz = src[2] * itheta; 334 335 dst[0] = nx * nx * (1. - cs) + cs; 336 dst[3] = nx * ny * (1. - cs) + nz * sn; 337 dst[6] = nx * nz * (1. - cs) - ny * sn; 338 339 dst[1] = nx * ny * (1. - cs) - nz * sn; 340 dst[4] = ny * ny * (1. - cs) + cs; 341 dst[7] = ny * nz * (1. - cs) + nx * sn; 342 343 dst[2] = nx * nz * (1. - cs) + ny * sn; 344 dst[5] = ny * nz * (1. - cs) - nx * sn; 345 dst[8] = nz * nz * (1. - cs) + cs; 346 } 347 else //first order taylor 348 { 349 dst[0] = tp(1.); 350 dst[3] = src[2]; 351 dst[6] = -src[1]; 352 353 dst[1] = -src[2]; 354 dst[4] = tp(1.); 355 dst[7] = src[0]; 356 357 dst[2] = src[1]; 358 dst[5] = -src[0]; 359 dst[8] = tp(1.); 360 } 361 return true; 362 } 363 static void TestMe(int argc = 0, char** argv = 0) 364 { 365 for (int k = 0; k < 999; ++k) 366 { 367 //0.GenerateData 368 Matx31d r = r.randu(-999, 999); 369 370 //1.CalcByCeres 371 Matx33d R1; 372 Mat_<double> dRdr1(9, 3); 373 ceres::CostFunction* costFun1 = new ceres::AutoDiffCostFunction<RVec2RMat, 9, 3>(new RVec2RMat(true)); 374 costFun1->Evaluate(vector<double*>{r.val}.data(), R1.val, vector<double*>{dRdr1.ptr<double>()}.data()); 375 376 //2.CalcByManu 377 Matx33d R2; 378 Mat_<double> dRdr2(9, 3); 379 ceres::CostFunction* costFun2 = new ceres::AutoDiffCostFunction<RVec2RMat, 9, 3>(new RVec2RMat(false)); 380 costFun2->Evaluate(vector<double*>{r.val}.data(), R2.val, vector<double*>{dRdr2.ptr<double>()}.data()); 381 382 //3.CalcByOpenCV 383 Matx33d R3; 384 Mat_<double> dRdr3; 385 cv::Rodrigues(r, R3, dRdr3); dRdr3 = dRdr3.t(); 386 387 //4.AnalyzeError 388 double infR1R2 = cv::norm(R1, R2, NORM_INF); 389 double infR1R3 = cv::norm(R1, R3, NORM_INF); 390 double infdRdr1dRdr2 = cv::norm(dRdr1, dRdr2, NORM_INF); 391 double infdRdr1dRdr3 = cv::norm(dRdr1, dRdr3, NORM_INF); 392 393 //5.PrintError 394 cout << endl << "LoopCount: " << k << endl; 395 if (infR1R2 > 1E-12 || infR1R3 > 1E-12 || infdRdr1dRdr2 > 1E-12 || infdRdr1dRdr3 > 1E-12) 396 { 397 cout << endl << "infR1R2: " << infR1R2 << endl; 398 cout << endl << "infR1R3: " << infR1R3 << endl; 399 cout << endl << "infdRdr3dRdr1: " << infdRdr1dRdr2 << endl; 400 cout << endl << "infdRdr3dRdr2: " << infdRdr1dRdr3 << endl; 401 if (0) 402 { 403 cout << endl << "R1: " << R1 << endl; 404 cout << endl << "R2: " << R2 << endl; 405 cout << endl << "R3: " << R3 << endl; 406 cout << endl << "dRdr1: " << dRdr1 << endl; 407 cout << endl << "dRdr2: " << dRdr2 << endl; 408 cout << endl << "dRdr3: " << dRdr3 << endl; 409 } 410 cout << endl << "Press any key to continue" << endl; getchar(); 411 } 412 } 413 } 414 }; 415 struct RMat2RVec 416 { 417 bool useCeres; 418 RMat2RVec(bool useCeres0) : useCeres(useCeres0) {} 419 template <typename tp> bool operator()(const tp* const src, tp* dst) const 420 { 421 if (useCeres) { ceres::RotationMatrixToAngleAxis(ceres::RowMajorAdapter3x3(src), dst); return true; } 422 tp quat[4]; 423 RMat2Quat(false)(src, quat); 424 Quat2RVec(false)(quat, dst); 425 return true; 426 } 427 static void TestMe(int argc, char** argv) 428 { 429 for (int k = 0; k < 999; ++k) 430 { 431 //0.GenerateData 432 Matx33d R; ceres::EulerAnglesToRotationMatrix(Matx31d::randu(-180, 180).val, 0, R.val); 433 434 //1.CalcByCeres 435 Vec3d r1; 436 Mat_<double> drdR1(3, 9); 437 ceres::CostFunction* costFun1 = new ceres::AutoDiffCostFunction<RMat2RVec, 3, 9>(new RMat2RVec(true)); 438 costFun1->Evaluate(vector<double*>{R.val}.data(), r1.val, vector<double*>{drdR1.ptr<double>()}.data()); 439 440 //2.CalcByManu 441 Vec3d r2; 442 Mat_<double> drdR2(3, 9); 443 ceres::CostFunction* costFun2 = new ceres::AutoDiffCostFunction<RMat2RVec, 3, 9>(new RMat2RVec(false)); 444 costFun2->Evaluate(vector<double*>{R.val}.data(), r2.val, vector<double*>{drdR2.ptr<double>()}.data()); 445 446 //3.AnalyzeError 447 double infr1r2 = cv::norm(r1, r2, NORM_INF); 448 double infdrdR1drdR2 = cv::norm(drdR1, drdR2, NORM_INF); 449 450 //5.PrintError 451 cout << endl << "LoopCount: " << k << endl; 452 if (infr1r2 > 1E-9 || infdrdR1drdR2 > 1E-9) 453 { 454 cout << endl << "infr1r2: " << infr1r2 << endl; 455 cout << endl << "infdrdR1drdR2: " << infdrdR1drdR2 << endl; 456 if (0) 457 { 458 cout << endl << "r1:" << r1 << endl; 459 cout << endl << "r2:" << r2 << endl; 460 cout << endl << "drdR1:" << drdR1 << endl; 461 cout << endl << "drdR2:" << drdR2 << endl; 462 } 463 cout << endl << "Press any key to continue" << endl; getchar(); 464 } 465 } 466 } 467 }; 468 469 public://Euler2RMat 470 struct Euler2RMat 471 { 472 bool useCeres; 473 Euler2RMat(bool useCeres0) : useCeres(useCeres0) {} 474 template <typename tp> bool operator()(const tp* const src, tp* dst) const 475 { 476 //RPY indicates: first Yaw aroundZ, second Pitch aroundY, third Roll aroundX 477 if (useCeres) { ceres::EulerAnglesToRotationMatrix(src, 0, dst); return true; } 478 tp sinR = sin(src[0] * deg2rad); 479 tp sinP = sin(src[1] * deg2rad); 480 tp sinY = sin(src[2] * deg2rad); 481 tp cosR = cos(src[0] * deg2rad); 482 tp cosP = cos(src[1] * deg2rad); 483 tp cosY = cos(src[2] * deg2rad); 484 485 dst[0] = cosY * cosP; dst[1] = cosY * sinP * sinR - sinY * cosR; dst[2] = cosY * sinP * cosR + sinY * sinR; 486 dst[3] = sinY * cosP; dst[4] = sinY * sinP * sinR + cosY * cosR; dst[5] = sinY * sinP * cosR - cosY * sinR; 487 dst[6] = -sinP; dst[7] = cosP * sinR; dst[8] = cosP * cosR; 488 return true; 489 } 490 static void TestMe(int argc = 0, char** argv = 0) 491 { 492 for (int k = 0; k < 999; ++k) 493 { 494 //0.GenerateData 495 Matx31d e = e.randu(-999, 999); 496 497 //1.CalcByCeres 498 Matx33d R1; 499 Mat_<double> dRde1(9, 3); 500 ceres::CostFunction* costFun1 = new ceres::AutoDiffCostFunction<Euler2RMat, 9, 3>(new Euler2RMat(true)); 501 costFun1->Evaluate(vector<double*>{e.val}.data(), R1.val, vector<double*>{dRde1.ptr<double>()}.data()); 502 503 //2.CalcByManu 504 Matx33d R2; 505 Mat_<double> dRde2(9, 3); 506 ceres::CostFunction* costFun2 = new ceres::AutoDiffCostFunction<Euler2RMat, 9, 3>(new Euler2RMat(false)); 507 costFun2->Evaluate(vector<double*>{e.val}.data(), R2.val, vector<double*>{dRde2.ptr<double>()}.data()); 508 509 //3.AnalyzeError 510 double infR1R2 = cv::norm(R1, R2, NORM_INF); 511 double infdRde1dRde2 = cv::norm(dRde1, dRde2, NORM_INF); 512 513 //4.PrintError 514 cout << endl << "LoopCount: " << k << endl; 515 if (infR1R2 > 1E-12 || infdRde1dRde2 > 1E-12) 516 { 517 cout << endl << "infR1R2: " << infR1R2 << endl; 518 cout << endl << "infdRde2dRde1: " << infdRde1dRde2 << endl; 519 if (0) 520 { 521 cout << endl << "R1: " << R1 << endl; 522 cout << endl << "R2: " << R2 << endl; 523 cout << endl << "dRde1: " << dRde1 << endl; 524 cout << endl << "dRde2: " << dRde2 << endl; 525 } 526 cout << endl << "Press any key to continue" << endl; getchar(); 527 } 528 } 529 } 530 }; 531 struct RMat2Euler 532 { 533 bool useCeres; 534 RMat2Euler(bool useCeres0) : useCeres(useCeres0) {} 535 template <typename tp> bool operator()(const tp* const src, tp* dst) const 536 { 537 //RPY indicates: first Yaw aroundZ, second Pitch aroundY, third Roll aroundX 538 if (useCeres) { ceres::RotationMatrixToEulerAngles(src, dst); return true; } 539 tp bound1 = abs(src[6] - 1.); 540 tp bound2 = abs(src[6] + 1.); 541 if (bound1 > ModelRotation::L1EPS && bound2 > ModelRotation::L1EPS) 542 { 543 dst[2] = atan2(src[3], src[0]); //Yaw aroundZ 544 dst[1] = asin(-src[6]);//Pitch aroundY 545 dst[0] = atan2(src[7], src[8]); //Roll aroundX 546 } 547 else if (bound2 <= ModelRotation::L1EPS) 548 { 549 dst[2] = tp(0.); //Yaw aroundZ 550 dst[1] = tp(3.14159265358979323846 * 0.5);//Pitch aroundY 551 dst[0] = dst[2] + atan2(src[1], src[2]); //Roll aroundX 552 } 553 else 554 { 555 dst[2] = tp(0.); //Yaw aroundZ 556 dst[1] = tp(-3.14159265358979323846 * 0.5);//Pitch aroundY 557 dst[0] = -dst[2] + atan2(-src[1], -src[2]); //Roll aroundX 558 } 559 dst[0] *= rad2deg; dst[1] *= rad2deg; dst[2] *= rad2deg; 560 return true; 561 } 562 static void TestMe(int argc, char** argv) 563 { 564 for (int k = 0; k < 999; ++k) 565 { 566 //0.GenerateData 567 Vec3d e0(Matx31d::randu(-180, 180).val); 568 Matx33d R; ceres::EulerAnglesToRotationMatrix(e0.val, 0, R.val); 569 570 //1.CalcByCeres 571 Vec3d e1; 572 Mat_<double> dedR1(3, 9); 573 ceres::CostFunction* costFun1 = new ceres::AutoDiffCostFunction<RMat2Euler, 3, 9>(new RMat2Euler(true)); 574 costFun1->Evaluate(vector<double*>{R.val}.data(), e1.val, vector<double*>{dedR1.ptr<double>()}.data()); 575 576 //2.CalcByManu 577 Vec3d e2; 578 Mat_<double> dedR2(3, 9); 579 ceres::CostFunction* costFun2 = new ceres::AutoDiffCostFunction<RMat2Euler, 3, 9>(new RMat2Euler(false)); 580 costFun2->Evaluate(vector<double*>{R.val}.data(), e2.val, vector<double*>{dedR2.ptr<double>()}.data()); 581 582 //3.CalcByOpenCV 583 Vec3d e3; 584 Mat_<double> dedR3(3, 9); 585 e3 = cv::RQDecomp3x3(R, Matx33d(), Matx33d()); 586 587 //4.AnalyzeError 588 double infe0e1 = min(cv::norm(e0, e1, NORM_INF), cv::norm(cv::abs(Mat_<double>(3, 1, e0.val)) + cv::abs(Mat_<double>(3, 1, e1.val)), Vec3d(180, 180, 180), NORM_INF)); 589 double infe0e2 = min(cv::norm(e0, e2, NORM_INF), cv::norm(cv::abs(Mat_<double>(3, 1, e0.val)) + cv::abs(Mat_<double>(3, 1, e2.val)), Vec3d(180, 180, 180), NORM_INF)); 590 double infe0e3 = min(cv::norm(e0, e3, NORM_INF), cv::norm(cv::abs(Mat_<double>(3, 1, e0.val)) + cv::abs(Mat_<double>(3, 1, e3.val)), Vec3d(180, 180, 180), NORM_INF)); 591 592 //5.PrintError 593 cout << endl << "LoopCount: " << k << endl; 594 if (infe0e1 > 1E-9 || infe0e2 > 1E-9 || infe0e3 > 1E-9) 595 { 596 cout << endl << "infe0e1: " << infe0e1 << endl; 597 cout << endl << "infe0e2: " << infe0e2 << endl; 598 cout << endl << "infe0e3: " << infe0e3 << endl; 599 if (0) 600 { 601 cout << endl << "e0: " << e0 << endl; 602 cout << endl << "e1: " << e1 << endl; 603 cout << endl << "e2: " << e2 << endl; 604 cout << endl << "e3: " << e3 << endl; 605 } 606 cout << endl << "Press any key to continue" << endl; getchar(); 607 } 608 } 609 } 610 }; 611 612 public: 613 struct SolidPose 614 { 615 double data[22] = { 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 1, 0, 0, 0, 1, 0, 0, 0, 1, 0, 0, 0 }; 616 double* vdata = data; //Mat_<double> rvec = Mat_<double>(3, 1, vdata); 617 double* tdata = vdata + 3; //Mat_<double> tvec = Mat_<double>(3, 1, tdata); 618 double* qdata = tdata + 3; //Mat_<double> quat = Mat_<double>(4, 1, qdata); 619 double* mdata = qdata + 4; //Mat_<double> rmat = Mat_<double>(3, 3, mdata); 620 double* edata = mdata + 9; //Mat_<double> euler = Mat_<double>(3, 1, edata); 621 SolidPose() {} 622 SolidPose(const SolidPose& other) { memcpy(data, other.data, sizeof(data)); } 623 SolidPose& operator=(const SolidPose& other) { memcpy(data, other.data, sizeof(data)); return *this; } 624 inline double* getRot(int n = 0/*0 3 4 9 for euler rvec quat rmat*/) { return n == 3 ? vdata : n == 4 ? qdata : n == 9 ? mdata : edata; } 625 inline void setPos(double* xyz) { memcpy(tdata, xyz, 3 * sizeof(double)); } 626 void setRot(double* rot, int n = 0/*0 3 4 9 for euler rvec quat rmat*/) 627 { 628 if (n == 0) 629 { 630 memcpy(edata, rot, 3 * sizeof(double)); 631 ceres::EulerAnglesToRotationMatrix(rot, 0, mdata); 632 ceres::RotationMatrixToQuaternion(ceres::RowMajorAdapter3x3((const double*)(mdata)), qdata); 633 ceres::QuaternionToAngleAxis(qdata, vdata);//rmat->quat->rvec in ceres 634 } 635 else if (n == 3) 636 { 637 memcpy(vdata, rot, 3 * sizeof(double)); 638 ceres::AngleAxisToQuaternion(vdata, qdata); 639 ceres::AngleAxisToRotationMatrix(vdata, ceres::RowMajorAdapter3x3(mdata)); 640 ceres::RotationMatrixToEulerAngles(mdata, edata); 641 } 642 else if (n == 4) 643 { 644 memcpy(qdata, rot, 4 * sizeof(double)); 645 ceres::QuaternionToAngleAxis(qdata, vdata); 646 ceres::QuaternionToRotation(qdata, mdata); 647 ceres::RotationMatrixToEulerAngles(mdata, edata); 648 } 649 else if (n == 9) 650 { 651 memcpy(mdata, rot, 9 * sizeof(double)); 652 ceres::RotationMatrixToQuaternion(ceres::RowMajorAdapter3x3((const double*)(mdata)), qdata); 653 ceres::QuaternionToAngleAxis(qdata, vdata);//rmat->quat->rvec in ceres 654 ceres::RotationMatrixToEulerAngles(mdata, edata); 655 } 656 } 657 SolidPose inv() 658 { 659 SolidPose dst; 660 dst.vdata[0] = -vdata[0]; dst.vdata[1] = -vdata[1]; dst.vdata[2] = -vdata[2]; 661 dst.qdata[0] = qdata[0]; dst.qdata[1] = -qdata[1]; dst.qdata[2] = -qdata[2]; dst.qdata[3] = -qdata[3]; 662 dst.mdata[0] = mdata[0]; dst.mdata[1] = mdata[3]; dst.mdata[2] = mdata[6]; 663 dst.mdata[3] = mdata[1]; dst.mdata[4] = mdata[4]; dst.mdata[5] = mdata[7]; 664 dst.mdata[6] = mdata[2]; dst.mdata[7] = mdata[5]; dst.mdata[8] = mdata[8]; 665 ceres::RotationMatrixToEulerAngles(dst.mdata, dst.edata);//no euler inversion operation 666 dst.tdata[0] = -(dst.mdata[0] * tdata[0] + dst.mdata[1] * tdata[1] + dst.mdata[2] * tdata[2]); 667 dst.tdata[1] = -(dst.mdata[3] * tdata[0] + dst.mdata[4] * tdata[1] + dst.mdata[5] * tdata[2]); 668 dst.tdata[2] = -(dst.mdata[6] * tdata[0] + dst.mdata[7] * tdata[1] + dst.mdata[8] * tdata[2]); 669 return dst; 670 } 671 SolidPose mul(SolidPose& other) 672 { 673 SolidPose dst; 674 ceres::QuaternionProduct(qdata, other.qdata, dst.qdata); 675 memcpy(dst.mdata, (Matx33d(mdata) * Matx33d(other.mdata)).val, 9 * sizeof(double)); 676 ceres::QuaternionToAngleAxis(dst.qdata, dst.vdata);//no rvec multiplication operation 677 ceres::RotationMatrixToEulerAngles(dst.mdata, dst.edata);//no euler multiplication operation 678 memcpy(dst.tdata, (Matx33d(mdata) * Matx31d(other.tdata) + Matx31d(tdata)).val, 3 * sizeof(double)); 679 return dst; 680 } 681 string print() 682 { 683 string str; 684 str += fmt::format("rvec: [ {}, {}, {} ]\n", vdata[0], vdata[1], vdata[2]); 685 str += fmt::format("tvec: [ {}, {}, {} ]\n", tdata[0], tdata[1], tdata[2]); 686 str += fmt::format("quat: [ {}, {}, {}, {} ]\n", qdata[0], qdata[1], qdata[2], qdata[3]); 687 str += fmt::format("rmat: [ {}, {}, {}, {}, {}, {}, {}, {}, {} ]\n", mdata[0], mdata[1], mdata[2], mdata[3], mdata[4], mdata[5], mdata[6], mdata[7], mdata[8]); 688 str += fmt::format("euler: [ {}, {}, {} ]\n", edata[0], edata[1], edata[2]); 689 return str; 690 } 691 }; 692 }; 693 694 #ifdef ModelRotationTest 695 int main(int argc, char** argv) 696 { 697 ModelRotation::RVec2Quat::TestMe(argc, argv); 698 ModelRotation::Quat2RVec::TestMe(argc, argv); 699 700 ModelRotation::Quat2RMat::TestMe(argc, argv); 701 ModelRotation::RMat2Quat::TestMe(argc, argv); 702 703 ModelRotation::RVec2RMat::TestMe(argc, argv); 704 ModelRotation::RMat2RVec::TestMe(argc, argv); 705 706 ModelRotation::Euler2RMat::TestMe(argc, argv); 707 ModelRotation::RMat2Euler::TestMe(argc, argv); 708 return 0; 709 } 710 int main1(int argc, char** argv) { ModelRotation::RVec2Quat::TestMe(argc, argv); return 0; } 711 int main2(int argc, char** argv) { ModelRotation::RVec2RMat::TestMe(argc, argv); return 0; } 712 int main3(int argc, char** argv) { ModelRotation::Quat2RMat::TestMe(argc, argv); return 0; } 713 int main7(int argc, char** argv) { ModelRotation::Euler2RMat::TestMe(argc, argv); return 0; } 714 int main8(int argc, char** argv) { ModelRotation::RMat2Euler::TestMe(argc, argv); return 0; } 715 #endif 716 717 #endif#ifndef __ModelRotation_h__ 718 #define __ModelRotation_h__ 719 720 #include <ceres/ceres.h> 721 #include <ceres/rotation.h> 722 #include <spdlog/spdlog.h> 723 #include <opencv2/opencv.hpp> 724 using namespace std; 725 using namespace cv; 726 727 constexpr static double pi = 3.1415926535897932384626433832795; 728 constexpr static double deg2rad = (pi * 0.0055555555555555556); 729 constexpr static double rad2deg = (180 * 0.3183098861837906715); 730 731 #ifndef tsns 732 #define tsns chrono::time_point_cast<chrono::nanoseconds>(chrono::system_clock::now()).time_since_epoch().count() 733 #define tsus chrono::time_point_cast<chrono::microseconds>(chrono::system_clock::now()).time_since_epoch().count() 734 #define tsms chrono::time_point_cast<chrono::milliseconds>(chrono::system_clock::now()).time_since_epoch().count() 735 #define tsse chrono::time_point_cast<chrono::seconds>(chrono::system_clock::now()).time_since_epoch().count() 736 #define tsmi chrono::time_point_cast<chrono::minutes>(chrono::system_clock::now()).time_since_epoch().count() 737 #define tsho chrono::time_point_cast<chrono::hours>(chrono::system_clock::now()).time_since_epoch().count() 738 #endif 739 740 class ModelRotation 741 { 742 public: 743 constexpr static double L1EPS = 1E-12; 744 constexpr static double L2EPS = 1E-18; 745 746 public://RVec2Quat 747 struct RVec2Quat 748 { 749 bool useCeres; 750 RVec2Quat(bool useCeres0) : useCeres(useCeres0) {} 751 template <typename tp> bool operator()(const tp* const src, tp* dst) const 752 { 753 if (useCeres) { ceres::AngleAxisToQuaternion(src, dst); return true; } 754 tp theta2 = src[0] * src[0] + src[1] * src[1] + src[2] * src[2]; 755 if (theta2 > ModelRotation::L2EPS) 756 { 757 tp theta = sqrt(theta2); 758 tp thetaa = theta * 0.5; 759 tp coeff = sin(thetaa) / theta; 760 dst[0] = cos(thetaa); 761 dst[1] = src[0] * coeff; 762 dst[2] = src[1] * coeff; 763 dst[3] = src[2] * coeff; 764 } 765 else //first order taylor 766 { 767 dst[0] = tp(1.); 768 dst[1] = src[0] * 0.5; 769 dst[2] = src[1] * 0.5; 770 dst[3] = src[2] * 0.5; 771 } 772 return true; 773 } 774 static void TestMe(int argc = 0, char** argv = 0) 775 { 776 for (int k = 0; k < 999; ++k) 777 { 778 //0.GenerateData 779 Matx31d r = r.randu(-999, 999); 780 781 //1.CalcByCeres 782 Vec4d q1; 783 Mat_<double> dqdr1(4, 3); 784 ceres::CostFunction* costFun1 = new ceres::AutoDiffCostFunction<RVec2Quat, 4, 3>(new RVec2Quat(true)); 785 costFun1->Evaluate(vector<double*>{r.val}.data(), q1.val, vector<double*>{dqdr1.ptr<double>()}.data()); 786 787 //2.CalcByManu 788 Vec4d q2; 789 Mat_<double> dqdr2(4, 3); 790 ceres::CostFunction* costFun2 = new ceres::AutoDiffCostFunction<RVec2Quat, 4, 3>(new RVec2Quat(false)); 791 costFun2->Evaluate(vector<double*>{r.val}.data(), q2.val, vector<double*>{dqdr2.ptr<double>()}.data()); 792 793 //3.AnalyzeError 794 double infq1q2 = cv::norm(q1, q2, NORM_INF); 795 double infdqdr1dqdr2 = cv::norm(dqdr1, dqdr2, NORM_INF); 796 797 //4.PrintError 798 cout << endl << "LoopCount: " << k << endl; 799 if (infq1q2 > 1E-12 || infdqdr1dqdr2 > 1E-12) 800 { 801 cout << endl << "infq1q2: " << infq1q2 << endl; 802 cout << endl << "infdqdr1dqdr2: " << infdqdr1dqdr2 << endl; 803 if (0) 804 { 805 cout << endl << "q1: " << q1 << endl; 806 cout << endl << "q2: " << q2 << endl; 807 cout << endl << "dqdr1: " << dqdr1 << endl; 808 cout << endl << "dqdr2: " << dqdr2 << endl; 809 } 810 cout << endl << "Press any key to continue" << endl; getchar(); 811 } 812 } 813 } 814 }; 815 struct Quat2RVec 816 { 817 bool useCeres; 818 Quat2RVec(bool useCeres0) : useCeres(useCeres0) {} 819 template <typename tp> bool operator()(const tp* const src, tp* dst) const 820 { 821 if (useCeres) { ceres::QuaternionToAngleAxis(src, dst); return true; } 822 tp thetaa2 = src[1] * src[1] + src[2] * src[2] + src[3] * src[3]; 823 if (thetaa2 > ModelRotation::L2EPS) 824 { 825 tp cs = src[0]; 826 tp sn = sqrt(thetaa2); 827 tp theta = 2.0 * (cs < 0.0 ? atan2(-sn, -cs) : atan2(sn, cs));//refer to ceres comments 828 tp coeff = theta / sn; 829 dst[0] = src[1] * coeff; 830 dst[1] = src[2] * coeff; 831 dst[2] = src[3] * coeff; 832 } 833 else //first order taylor 834 { 835 dst[0] = 2.0 * src[1]; 836 dst[1] = 2.0 * src[2]; 837 dst[2] = 2.0 * src[3]; 838 } 839 return true; 840 } 841 static void TestMe(int argc = 0, char** argv = 0) 842 { 843 for (int k = 0; k < 999; ++k) 844 { 845 //0.GenerateData 846 Matx41d q = q.randu(-999, 999); 847 848 //1.CalcByCeres 849 Matx33d r1; 850 Mat_<double> drdq1(3, 4); 851 ceres::CostFunction* costFun1 = new ceres::AutoDiffCostFunction<Quat2RVec, 3, 4>(new Quat2RVec(true)); 852 costFun1->Evaluate(vector<double*>{q.val}.data(), r1.val, vector<double*>{drdq1.ptr<double>()}.data()); 853 854 //2.CalcByManu 855 Matx33d r2; 856 Mat_<double> drdq2(3, 4); 857 ceres::CostFunction* costFun2 = new ceres::AutoDiffCostFunction<Quat2RVec, 3, 4>(new Quat2RVec(false)); 858 costFun2->Evaluate(vector<double*>{q.val}.data(), r2.val, vector<double*>{drdq2.ptr<double>()}.data()); 859 860 //3.AnalyzeError 861 double infr1r2 = cv::norm(r1, r2, NORM_INF); 862 double infdrdq1drdq2 = cv::norm(drdq1, drdq2, NORM_INF); 863 864 //4.PrintError 865 cout << endl << "LoopCount: " << k << endl; 866 if (infr1r2 > 1E-12 || infdrdq1drdq2 > 1E-12) 867 { 868 cout << endl << "infr1r2: " << infr1r2 << endl; 869 cout << endl << "infdrdq1drdq2: " << infdrdq1drdq2 << endl; 870 if (0) 871 { 872 cout << endl << "r1: " << r1 << endl; 873 cout << endl << "r2: " << r2 << endl; 874 cout << endl << "drdq1: " << drdq1 << endl; 875 cout << endl << "drdq2: " << drdq2 << endl; 876 } 877 cout << endl << "Press any key to continue" << endl; getchar(); 878 } 879 } 880 } 881 }; 882 883 public://Quat2RMat 884 struct Quat2RMat 885 { 886 bool useCeres; 887 Quat2RMat(bool useCeres0) : useCeres(useCeres0) {} 888 template <typename tp> bool operator()(const tp* const src, tp* dst) const 889 { 890 if (useCeres) { ceres::QuaternionToRotation(src, dst); return true; } 891 tp ww = src[0] * src[0]; 892 tp wx = src[0] * src[1]; 893 tp wy = src[0] * src[2]; 894 tp wz = src[0] * src[3]; 895 896 tp xx = src[1] * src[1]; 897 tp xy = src[1] * src[2]; 898 tp xz = src[1] * src[3]; 899 900 tp yy = src[2] * src[2]; 901 tp yz = src[2] * src[3]; 902 903 tp zz = src[3] * src[3]; 904 tp L2 = ww + xx + yy + zz; 905 if (L2 < ModelRotation::L2EPS) { dst[0] = dst[4] = dst[8] = tp(1.); dst[1] = dst[2] = dst[3] = dst[5] = dst[6] = dst[7] = tp(0.); return true; } 906 tp iL2 = 1. / L2; 907 908 dst[0] = ww + xx - yy - zz; dst[1] = 2.0 * (xy - wz); dst[2] = 2.0 * (wy + xz); 909 dst[3] = 2.0 * (wz + xy); dst[4] = ww - xx + yy - zz; dst[5] = 2.0 * (yz - wx); 910 dst[6] = 2.0 * (xz - wy); dst[7] = 2.0 * (wx + yz); dst[8] = ww - xx - yy + zz; 911 for (int k = 0; k < 9; ++k) dst[k] = dst[k] * iL2;//for not unit quaternion 912 913 return true; 914 } 915 static void TestMe(int argc = 0, char** argv = 0) 916 { 917 for (int k = 0; k < 999; ++k) 918 { 919 //0.GenerateData 920 Matx41d q = q.randu(-999, 999); 921 922 //1.CalcByCeres 923 Matx33d R1; 924 Mat_<double> dRdq1(9, 4); 925 ceres::CostFunction* costFun1 = new ceres::AutoDiffCostFunction<Quat2RMat, 9, 4>(new Quat2RMat(true)); 926 costFun1->Evaluate(vector<double*>{q.val}.data(), R1.val, vector<double*>{dRdq1.ptr<double>()}.data()); 927 928 //2.CalcByManu 929 Matx33d R2; 930 Mat_<double> dRdq2(9, 4); 931 ceres::CostFunction* costFun2 = new ceres::AutoDiffCostFunction<Quat2RMat, 9, 4>(new Quat2RMat(false)); 932 costFun2->Evaluate(vector<double*>{q.val}.data(), R2.val, vector<double*>{dRdq2.ptr<double>()}.data()); 933 934 //3.AnalyzeError 935 double infR1R2 = cv::norm(R1, R2, NORM_INF); 936 double infdRdq1dRdq2 = cv::norm(dRdq1, dRdq2, NORM_INF); 937 938 //4.PrintError 939 cout << endl << "LoopCount: " << k << endl; 940 if (infR1R2 > 1E-12 || infdRdq1dRdq2 > 1E-12) 941 { 942 cout << endl << "infR1R2: " << infR1R2 << endl; 943 cout << endl << "infdRdq1dRdq2: " << infdRdq1dRdq2 << endl; 944 if (0) 945 { 946 cout << endl << "R1: " << R1 << endl; 947 cout << endl << "R2: " << R2 << endl; 948 cout << endl << "dRdq1: " << dRdq1 << endl; 949 cout << endl << "dRdq2: " << dRdq2 << endl; 950 } 951 cout << endl << "Press any key to continue" << endl; getchar(); 952 } 953 } 954 } 955 }; 956 struct RMat2Quat 957 { 958 bool useCeres; 959 RMat2Quat(bool useCeres0) : useCeres(useCeres0) {} 960 template <typename tp> bool operator()(const tp* const src, tp* dst) const 961 { 962 if (useCeres) { ceres::RotationMatrixToQuaternion(ceres::RowMajorAdapter3x3(src), dst); return true; } 963 tp trace = src[0] + src[4] + src[8];//Strictly 1+trace>0 for rotation matrix. In ceres, This leads to different jacobians between ceres' rmat2rvec and cv::Rodrigues(rmat, rvec) 964 if (trace >= 0.0) 965 { 966 tp trace1 = sqrt(1. + trace); 967 dst[0] = 0.5 * trace1; 968 tp itrace1 = 0.5 / trace1; 969 dst[1] = (src[7] - src[5]) * itrace1; 970 dst[2] = (src[2] - src[6]) * itrace1; 971 dst[3] = (src[3] - src[1]) * itrace1; 972 } 973 else 974 { 975 int a = 0; 976 if (src[4] > src[0]) a = 1; 977 if (src[8] > src[a * 3 + a]) a = 2; 978 int b = (a + 1) % 3; 979 int c = (b + 1) % 3; 980 981 tp trace1 = sqrt(1.0 + src[a * 3 + a] - src[b * 3 + b] - src[c * 3 + c]); 982 dst[a + 1] = 0.5 * trace1; 983 tp itrace1 = 0.5 / trace1; 984 dst[0] = (src[c * 3 + b] - src[b * 3 + c]) * itrace1; 985 dst[b + 1] = (src[b * 3 + a] + src[a * 3 + b]) * itrace1; 986 dst[c + 1] = (src[c * 3 + a] + src[a * 3 + c]) * itrace1; 987 } 988 return true; 989 } 990 static void TestMe(int argc, char** argv) 991 { 992 for (int k = 0; k < 999; ++k) 993 { 994 //0.GenerateData 995 Matx33d R; ceres::EulerAnglesToRotationMatrix(Matx31d::randu(-180, 180).val, 0, R.val); 996 997 //1.CalcByCeres 998 Vec4d q1; 999 Mat_<double> dqdR1(4, 9); 1000 ceres::CostFunction* costFun1 = new ceres::AutoDiffCostFunction<RMat2Quat, 4, 9>(new RMat2Quat(true)); 1001 costFun1->Evaluate(vector<double*>{R.val}.data(), q1.val, vector<double*>{dqdR1.ptr<double>()}.data()); 1002 1003 //2.CalcByManu 1004 Vec4d q2; 1005 Mat_<double> dqdR2(4, 9); 1006 ceres::CostFunction* costFun2 = new ceres::AutoDiffCostFunction<RMat2Quat, 4, 9>(new RMat2Quat(false)); 1007 costFun2->Evaluate(vector<double*>{R.val}.data(), q2.val, vector<double*>{dqdR2.ptr<double>()}.data()); 1008 1009 //4.AnalyzeError 1010 double infq1q2 = cv::norm(q1, q2, NORM_INF); 1011 double infdqdR1dqdR2 = cv::norm(dqdR1, dqdR2, NORM_INF); 1012 1013 //5.PrintError 1014 cout << endl << "LoopCount: " << k << endl; 1015 if (infq1q2 > 1E-9 || infdqdR1dqdR2 > 1E-9) 1016 { 1017 cout << endl << "infq1q2: " << infq1q2 << endl; 1018 cout << endl << "infdqdR1dqdR2: " << infq1q2 << endl; 1019 if (0) 1020 { 1021 cout << endl << "q1:" << q1 << endl; 1022 cout << endl << "q2:" << q2 << endl; 1023 cout << endl << "dqdR1:" << dqdR1 << endl; 1024 cout << endl << "dqdR2:" << dqdR2 << endl; 1025 } 1026 cout << endl << "Press any key to continue" << endl; getchar(); 1027 } 1028 } 1029 } 1030 }; 1031 1032 public://RVec2RMat 1033 struct RVec2RMat 1034 { 1035 bool useCeres; 1036 RVec2RMat(bool useCeres0) : useCeres(useCeres0) {} 1037 template <typename tp> bool operator()(const tp* const src, tp* dst) const 1038 { 1039 if (useCeres) { ceres::AngleAxisToRotationMatrix(src, ceres::RowMajorAdapter3x3(dst)); return true; } 1040 tp theta2 = src[0] * src[0] + src[1] * src[1] + src[2] * src[2]; 1041 if (theta2 > ModelRotation::L2EPS) 1042 { 1043 tp theta = sqrt(theta2); 1044 tp itheta = 1. / theta; 1045 tp cs = cos(theta); 1046 tp sn = sin(theta); 1047 tp nx = src[0] * itheta; 1048 tp ny = src[1] * itheta; 1049 tp nz = src[2] * itheta; 1050 1051 dst[0] = nx * nx * (1. - cs) + cs; 1052 dst[3] = nx * ny * (1. - cs) + nz * sn; 1053 dst[6] = nx * nz * (1. - cs) - ny * sn; 1054 1055 dst[1] = nx * ny * (1. - cs) - nz * sn; 1056 dst[4] = ny * ny * (1. - cs) + cs; 1057 dst[7] = ny * nz * (1. - cs) + nx * sn; 1058 1059 dst[2] = nx * nz * (1. - cs) + ny * sn; 1060 dst[5] = ny * nz * (1. - cs) - nx * sn; 1061 dst[8] = nz * nz * (1. - cs) + cs; 1062 } 1063 else //first order taylor 1064 { 1065 dst[0] = tp(1.); 1066 dst[3] = src[2]; 1067 dst[6] = -src[1]; 1068 1069 dst[1] = -src[2]; 1070 dst[4] = tp(1.); 1071 dst[7] = src[0]; 1072 1073 dst[2] = src[1]; 1074 dst[5] = -src[0]; 1075 dst[8] = tp(1.); 1076 } 1077 return true; 1078 } 1079 static void TestMe(int argc = 0, char** argv = 0) 1080 { 1081 for (int k = 0; k < 999; ++k) 1082 { 1083 //0.GenerateData 1084 Matx31d r = r.randu(-999, 999); 1085 1086 //1.CalcByCeres 1087 Matx33d R1; 1088 Mat_<double> dRdr1(9, 3); 1089 ceres::CostFunction* costFun1 = new ceres::AutoDiffCostFunction<RVec2RMat, 9, 3>(new RVec2RMat(true)); 1090 costFun1->Evaluate(vector<double*>{r.val}.data(), R1.val, vector<double*>{dRdr1.ptr<double>()}.data()); 1091 1092 //2.CalcByManu 1093 Matx33d R2; 1094 Mat_<double> dRdr2(9, 3); 1095 ceres::CostFunction* costFun2 = new ceres::AutoDiffCostFunction<RVec2RMat, 9, 3>(new RVec2RMat(false)); 1096 costFun2->Evaluate(vector<double*>{r.val}.data(), R2.val, vector<double*>{dRdr2.ptr<double>()}.data()); 1097 1098 //3.CalcByOpenCV 1099 Matx33d R3; 1100 Mat_<double> dRdr3; 1101 cv::Rodrigues(r, R3, dRdr3); dRdr3 = dRdr3.t(); 1102 1103 //4.AnalyzeError 1104 double infR1R2 = cv::norm(R1, R2, NORM_INF); 1105 double infR1R3 = cv::norm(R1, R3, NORM_INF); 1106 double infdRdr1dRdr2 = cv::norm(dRdr1, dRdr2, NORM_INF); 1107 double infdRdr1dRdr3 = cv::norm(dRdr1, dRdr3, NORM_INF); 1108 1109 //5.PrintError 1110 cout << endl << "LoopCount: " << k << endl; 1111 if (infR1R2 > 1E-12 || infR1R3 > 1E-12 || infdRdr1dRdr2 > 1E-12 || infdRdr1dRdr3 > 1E-12) 1112 { 1113 cout << endl << "infR1R2: " << infR1R2 << endl; 1114 cout << endl << "infR1R3: " << infR1R3 << endl; 1115 cout << endl << "infdRdr3dRdr1: " << infdRdr1dRdr2 << endl; 1116 cout << endl << "infdRdr3dRdr2: " << infdRdr1dRdr3 << endl; 1117 if (0) 1118 { 1119 cout << endl << "R1: " << R1 << endl; 1120 cout << endl << "R2: " << R2 << endl; 1121 cout << endl << "R3: " << R3 << endl; 1122 cout << endl << "dRdr1: " << dRdr1 << endl; 1123 cout << endl << "dRdr2: " << dRdr2 << endl; 1124 cout << endl << "dRdr3: " << dRdr3 << endl; 1125 } 1126 cout << endl << "Press any key to continue" << endl; getchar(); 1127 } 1128 } 1129 } 1130 }; 1131 struct RMat2RVec 1132 { 1133 bool useCeres; 1134 RMat2RVec(bool useCeres0) : useCeres(useCeres0) {} 1135 template <typename tp> bool operator()(const tp* const src, tp* dst) const 1136 { 1137 if (useCeres) { ceres::RotationMatrixToAngleAxis(ceres::RowMajorAdapter3x3(src), dst); return true; } 1138 tp quat[4]; 1139 RMat2Quat(false)(src, quat); 1140 Quat2RVec(false)(quat, dst); 1141 return true; 1142 } 1143 static void TestMe(int argc, char** argv) 1144 { 1145 for (int k = 0; k < 999; ++k) 1146 { 1147 //0.GenerateData 1148 Matx33d R; ceres::EulerAnglesToRotationMatrix(Matx31d::randu(-180, 180).val, 0, R.val); 1149 1150 //1.CalcByCeres 1151 Vec3d r1; 1152 Mat_<double> drdR1(3, 9); 1153 ceres::CostFunction* costFun1 = new ceres::AutoDiffCostFunction<RMat2RVec, 3, 9>(new RMat2RVec(true)); 1154 costFun1->Evaluate(vector<double*>{R.val}.data(), r1.val, vector<double*>{drdR1.ptr<double>()}.data()); 1155 1156 //2.CalcByManu 1157 Vec3d r2; 1158 Mat_<double> drdR2(3, 9); 1159 ceres::CostFunction* costFun2 = new ceres::AutoDiffCostFunction<RMat2RVec, 3, 9>(new RMat2RVec(false)); 1160 costFun2->Evaluate(vector<double*>{R.val}.data(), r2.val, vector<double*>{drdR2.ptr<double>()}.data()); 1161 1162 //3.AnalyzeError 1163 double infr1r2 = cv::norm(r1, r2, NORM_INF); 1164 double infdrdR1drdR2 = cv::norm(drdR1, drdR2, NORM_INF); 1165 1166 //5.PrintError 1167 cout << endl << "LoopCount: " << k << endl; 1168 if (infr1r2 > 1E-9 || infdrdR1drdR2 > 1E-9) 1169 { 1170 cout << endl << "infr1r2: " << infr1r2 << endl; 1171 cout << endl << "infdrdR1drdR2: " << infdrdR1drdR2 << endl; 1172 if (0) 1173 { 1174 cout << endl << "r1:" << r1 << endl; 1175 cout << endl << "r2:" << r2 << endl; 1176 cout << endl << "drdR1:" << drdR1 << endl; 1177 cout << endl << "drdR2:" << drdR2 << endl; 1178 } 1179 cout << endl << "Press any key to continue" << endl; getchar(); 1180 } 1181 } 1182 } 1183 }; 1184 1185 public://Euler2RMat 1186 struct Euler2RMat 1187 { 1188 bool useCeres; 1189 Euler2RMat(bool useCeres0) : useCeres(useCeres0) {} 1190 template <typename tp> bool operator()(const tp* const src, tp* dst) const 1191 { 1192 //RPY indicates: first Yaw aroundZ, second Pitch aroundY, third Roll aroundX 1193 if (useCeres) { ceres::EulerAnglesToRotationMatrix(src, 0, dst); return true; } 1194 tp sinR = sin(src[0] * deg2rad); 1195 tp sinP = sin(src[1] * deg2rad); 1196 tp sinY = sin(src[2] * deg2rad); 1197 tp cosR = cos(src[0] * deg2rad); 1198 tp cosP = cos(src[1] * deg2rad); 1199 tp cosY = cos(src[2] * deg2rad); 1200 1201 dst[0] = cosY * cosP; dst[1] = cosY * sinP * sinR - sinY * cosR; dst[2] = cosY * sinP * cosR + sinY * sinR; 1202 dst[3] = sinY * cosP; dst[4] = sinY * sinP * sinR + cosY * cosR; dst[5] = sinY * sinP * cosR - cosY * sinR; 1203 dst[6] = -sinP; dst[7] = cosP * sinR; dst[8] = cosP * cosR; 1204 return true; 1205 } 1206 static void TestMe(int argc = 0, char** argv = 0) 1207 { 1208 for (int k = 0; k < 999; ++k) 1209 { 1210 //0.GenerateData 1211 Matx31d e = e.randu(-999, 999); 1212 1213 //1.CalcByCeres 1214 Matx33d R1; 1215 Mat_<double> dRde1(9, 3); 1216 ceres::CostFunction* costFun1 = new ceres::AutoDiffCostFunction<Euler2RMat, 9, 3>(new Euler2RMat(true)); 1217 costFun1->Evaluate(vector<double*>{e.val}.data(), R1.val, vector<double*>{dRde1.ptr<double>()}.data()); 1218 1219 //2.CalcByManu 1220 Matx33d R2; 1221 Mat_<double> dRde2(9, 3); 1222 ceres::CostFunction* costFun2 = new ceres::AutoDiffCostFunction<Euler2RMat, 9, 3>(new Euler2RMat(false)); 1223 costFun2->Evaluate(vector<double*>{e.val}.data(), R2.val, vector<double*>{dRde2.ptr<double>()}.data()); 1224 1225 //3.AnalyzeError 1226 double infR1R2 = cv::norm(R1, R2, NORM_INF); 1227 double infdRde1dRde2 = cv::norm(dRde1, dRde2, NORM_INF); 1228 1229 //4.PrintError 1230 cout << endl << "LoopCount: " << k << endl; 1231 if (infR1R2 > 1E-12 || infdRde1dRde2 > 1E-12) 1232 { 1233 cout << endl << "infR1R2: " << infR1R2 << endl; 1234 cout << endl << "infdRde2dRde1: " << infdRde1dRde2 << endl; 1235 if (0) 1236 { 1237 cout << endl << "R1: " << R1 << endl; 1238 cout << endl << "R2: " << R2 << endl; 1239 cout << endl << "dRde1: " << dRde1 << endl; 1240 cout << endl << "dRde2: " << dRde2 << endl; 1241 } 1242 cout << endl << "Press any key to continue" << endl; getchar(); 1243 } 1244 } 1245 } 1246 }; 1247 struct RMat2Euler 1248 { 1249 bool useCeres; 1250 RMat2Euler(bool useCeres0) : useCeres(useCeres0) {} 1251 template <typename tp> bool operator()(const tp* const src, tp* dst) const 1252 { 1253 //RPY indicates: first Yaw aroundZ, second Pitch aroundY, third Roll aroundX 1254 if (useCeres) { ceres::RotationMatrixToEulerAngles(src, dst); return true; } 1255 tp bound1 = abs(src[6] - 1.); 1256 tp bound2 = abs(src[6] + 1.); 1257 if (bound1 > ModelRotation::L1EPS && bound2 > ModelRotation::L1EPS) 1258 { 1259 dst[2] = atan2(src[3], src[0]); //Yaw aroundZ 1260 dst[1] = asin(-src[6]);//Pitch aroundY 1261 dst[0] = atan2(src[7], src[8]); //Roll aroundX 1262 } 1263 else if (bound2 <= ModelRotation::L1EPS) 1264 { 1265 dst[2] = tp(0.); //Yaw aroundZ 1266 dst[1] = tp(3.14159265358979323846 * 0.5);//Pitch aroundY 1267 dst[0] = dst[2] + atan2(src[1], src[2]); //Roll aroundX 1268 } 1269 else 1270 { 1271 dst[2] = tp(0.); //Yaw aroundZ 1272 dst[1] = tp(-3.14159265358979323846 * 0.5);//Pitch aroundY 1273 dst[0] = -dst[2] + atan2(-src[1], -src[2]); //Roll aroundX 1274 } 1275 dst[0] *= rad2deg; dst[1] *= rad2deg; dst[2] *= rad2deg; 1276 return true; 1277 } 1278 static void TestMe(int argc, char** argv) 1279 { 1280 for (int k = 0; k < 999; ++k) 1281 { 1282 //0.GenerateData 1283 Vec3d e0(Matx31d::randu(-180, 180).val); 1284 Matx33d R; ceres::EulerAnglesToRotationMatrix(e0.val, 0, R.val); 1285 1286 //1.CalcByCeres 1287 Vec3d e1; 1288 Mat_<double> dedR1(3, 9); 1289 ceres::CostFunction* costFun1 = new ceres::AutoDiffCostFunction<RMat2Euler, 3, 9>(new RMat2Euler(true)); 1290 costFun1->Evaluate(vector<double*>{R.val}.data(), e1.val, vector<double*>{dedR1.ptr<double>()}.data()); 1291 1292 //2.CalcByManu 1293 Vec3d e2; 1294 Mat_<double> dedR2(3, 9); 1295 ceres::CostFunction* costFun2 = new ceres::AutoDiffCostFunction<RMat2Euler, 3, 9>(new RMat2Euler(false)); 1296 costFun2->Evaluate(vector<double*>{R.val}.data(), e2.val, vector<double*>{dedR2.ptr<double>()}.data()); 1297 1298 //3.CalcByOpenCV 1299 Vec3d e3; 1300 Mat_<double> dedR3(3, 9); 1301 e3 = cv::RQDecomp3x3(R, Matx33d(), Matx33d()); 1302 1303 //4.AnalyzeError 1304 double infe0e1 = min(cv::norm(e0, e1, NORM_INF), cv::norm(cv::abs(Mat_<double>(3, 1, e0.val)) + cv::abs(Mat_<double>(3, 1, e1.val)), Vec3d(180, 180, 180), NORM_INF)); 1305 double infe0e2 = min(cv::norm(e0, e2, NORM_INF), cv::norm(cv::abs(Mat_<double>(3, 1, e0.val)) + cv::abs(Mat_<double>(3, 1, e2.val)), Vec3d(180, 180, 180), NORM_INF)); 1306 double infe0e3 = min(cv::norm(e0, e3, NORM_INF), cv::norm(cv::abs(Mat_<double>(3, 1, e0.val)) + cv::abs(Mat_<double>(3, 1, e3.val)), Vec3d(180, 180, 180), NORM_INF)); 1307 1308 //5.PrintError 1309 cout << endl << "LoopCount: " << k << endl; 1310 if (infe0e1 > 1E-9 || infe0e2 > 1E-9 || infe0e3 > 1E-9) 1311 { 1312 cout << endl << "infe0e1: " << infe0e1 << endl; 1313 cout << endl << "infe0e2: " << infe0e2 << endl; 1314 cout << endl << "infe0e3: " << infe0e3 << endl; 1315 if (0) 1316 { 1317 cout << endl << "e0: " << e0 << endl; 1318 cout << endl << "e1: " << e1 << endl; 1319 cout << endl << "e2: " << e2 << endl; 1320 cout << endl << "e3: " << e3 << endl; 1321 } 1322 cout << endl << "Press any key to continue" << endl; getchar(); 1323 } 1324 } 1325 } 1326 }; 1327 1328 public: 1329 struct SolidPose 1330 { 1331 double data[22] = { 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 1, 0, 0, 0, 1, 0, 0, 0, 1, 0, 0, 0 }; 1332 double* vdata = data; //Mat_<double> rvec = Mat_<double>(3, 1, vdata); 1333 double* tdata = vdata + 3; //Mat_<double> tvec = Mat_<double>(3, 1, tdata); 1334 double* qdata = tdata + 3; //Mat_<double> quat = Mat_<double>(4, 1, qdata); 1335 double* mdata = qdata + 4; //Mat_<double> rmat = Mat_<double>(3, 3, mdata); 1336 double* edata = mdata + 9; //Mat_<double> euler = Mat_<double>(3, 1, edata); 1337 SolidPose() {} 1338 SolidPose(const SolidPose& other) { memcpy(data, other.data, sizeof(data)); } 1339 SolidPose& operator=(const SolidPose& other) { memcpy(data, other.data, sizeof(data)); return *this; } 1340 inline double* getRot(int n = 0/*0 3 4 9 for euler rvec quat rmat*/) { return n == 3 ? vdata : n == 4 ? qdata : n == 9 ? mdata : edata; } 1341 inline void setPos(double* xyz) { memcpy(tdata, xyz, 3 * sizeof(double)); } 1342 void setRot(double* rot, int n = 0/*0 3 4 9 for euler rvec quat rmat*/) 1343 { 1344 if (n == 0) 1345 { 1346 memcpy(edata, rot, 3 * sizeof(double)); 1347 ceres::EulerAnglesToRotationMatrix(rot, 0, mdata); 1348 ceres::RotationMatrixToQuaternion(ceres::RowMajorAdapter3x3((const double*)(mdata)), qdata); 1349 ceres::QuaternionToAngleAxis(qdata, vdata);//rmat->quat->rvec in ceres 1350 } 1351 else if (n == 3) 1352 { 1353 memcpy(vdata, rot, 3 * sizeof(double)); 1354 ceres::AngleAxisToQuaternion(vdata, qdata); 1355 ceres::AngleAxisToRotationMatrix(vdata, ceres::RowMajorAdapter3x3(mdata)); 1356 ceres::RotationMatrixToEulerAngles(mdata, edata); 1357 } 1358 else if (n == 4) 1359 { 1360 memcpy(qdata, rot, 4 * sizeof(double)); 1361 ceres::QuaternionToAngleAxis(qdata, vdata); 1362 ceres::QuaternionToRotation(qdata, mdata); 1363 ceres::RotationMatrixToEulerAngles(mdata, edata); 1364 } 1365 else if (n == 9) 1366 { 1367 memcpy(mdata, rot, 9 * sizeof(double)); 1368 ceres::RotationMatrixToQuaternion(ceres::RowMajorAdapter3x3((const double*)(mdata)), qdata); 1369 ceres::QuaternionToAngleAxis(qdata, vdata);//rmat->quat->rvec in ceres 1370 ceres::RotationMatrixToEulerAngles(mdata, edata); 1371 } 1372 } 1373 SolidPose inv() 1374 { 1375 SolidPose dst; 1376 dst.vdata[0] = -vdata[0]; dst.vdata[1] = -vdata[1]; dst.vdata[2] = -vdata[2]; 1377 dst.qdata[0] = qdata[0]; dst.qdata[1] = -qdata[1]; dst.qdata[2] = -qdata[2]; dst.qdata[3] = -qdata[3]; 1378 dst.mdata[0] = mdata[0]; dst.mdata[1] = mdata[3]; dst.mdata[2] = mdata[6]; 1379 dst.mdata[3] = mdata[1]; dst.mdata[4] = mdata[4]; dst.mdata[5] = mdata[7]; 1380 dst.mdata[6] = mdata[2]; dst.mdata[7] = mdata[5]; dst.mdata[8] = mdata[8]; 1381 ceres::RotationMatrixToEulerAngles(dst.mdata, dst.edata);//no euler inversion operation 1382 dst.tdata[0] = -(dst.mdata[0] * tdata[0] + dst.mdata[1] * tdata[1] + dst.mdata[2] * tdata[2]); 1383 dst.tdata[1] = -(dst.mdata[3] * tdata[0] + dst.mdata[4] * tdata[1] + dst.mdata[5] * tdata[2]); 1384 dst.tdata[2] = -(dst.mdata[6] * tdata[0] + dst.mdata[7] * tdata[1] + dst.mdata[8] * tdata[2]); 1385 return dst; 1386 } 1387 SolidPose mul(SolidPose& other) 1388 { 1389 SolidPose dst; 1390 ceres::QuaternionProduct(qdata, other.qdata, dst.qdata); 1391 memcpy(dst.mdata, (Matx33d(mdata) * Matx33d(other.mdata)).val, 9 * sizeof(double)); 1392 ceres::QuaternionToAngleAxis(dst.qdata, dst.vdata);//no rvec multiplication operation 1393 ceres::RotationMatrixToEulerAngles(dst.mdata, dst.edata);//no euler multiplication operation 1394 memcpy(dst.tdata, (Matx33d(mdata) * Matx31d(other.tdata) + Matx31d(tdata)).val, 3 * sizeof(double)); 1395 return dst; 1396 } 1397 string print() 1398 { 1399 string str; 1400 str += fmt::format("rvec: [ {}, {}, {} ]\n", vdata[0], vdata[1], vdata[2]); 1401 str += fmt::format("tvec: [ {}, {}, {} ]\n", tdata[0], tdata[1], tdata[2]); 1402 str += fmt::format("quat: [ {}, {}, {}, {} ]\n", qdata[0], qdata[1], qdata[2], qdata[3]); 1403 str += fmt::format("rmat: [ {}, {}, {}, {}, {}, {}, {}, {}, {} ]\n", mdata[0], mdata[1], mdata[2], mdata[3], mdata[4], mdata[5], mdata[6], mdata[7], mdata[8]); 1404 str += fmt::format("euler: [ {}, {}, {} ]\n", edata[0], edata[1], edata[2]); 1405 return str; 1406 } 1407 }; 1408 }; 1409 1410 #ifdef ModelRotationTest 1411 int main(int argc, char** argv) 1412 { 1413 ModelRotation::RVec2Quat::TestMe(argc, argv); 1414 ModelRotation::Quat2RVec::TestMe(argc, argv); 1415 1416 ModelRotation::Quat2RMat::TestMe(argc, argv); 1417 ModelRotation::RMat2Quat::TestMe(argc, argv); 1418 1419 ModelRotation::RVec2RMat::TestMe(argc, argv); 1420 ModelRotation::RMat2RVec::TestMe(argc, argv); 1421 1422 ModelRotation::Euler2RMat::TestMe(argc, argv); 1423 ModelRotation::RMat2Euler::TestMe(argc, argv); 1424 return 0; 1425 } 1426 int main1(int argc, char** argv) { ModelRotation::RVec2Quat::TestMe(argc, argv); return 0; } 1427 int main2(int argc, char** argv) { ModelRotation::RVec2RMat::TestMe(argc, argv); return 0; } 1428 int main3(int argc, char** argv) { ModelRotation::Quat2RMat::TestMe(argc, argv); return 0; } 1429 int main7(int argc, char** argv) { ModelRotation::Euler2RMat::TestMe(argc, argv); return 0; } 1430 int main8(int argc, char** argv) { ModelRotation::RMat2Euler::TestMe(argc, argv); return 0; } 1431 #endif 1432 1433 #endif
