1.本文要点说明
介绍视觉定位导航中位姿变换的基础知识，包括：
(1)位姿变换模型
(2)左乘右乘法则
(3)模型几何解释
(4)基本运算公式
(5)旋转转换公式
2.实验测试代码
依赖于OpenCV、Ceres和Spdlog，封装在类ModelRotation：
(1)RVec2Quat、Quat2RMat、RVec2RMat、Euler2RMat
(2)Quat2RVec、RMat2Quat、RMat2RVec、RMat2Euler
(3)SolidPose：setPos、setRot、inv、mul、print
1 #include <ceres/ceres.h> 2 #include <ceres/rotation.h> 3 #include <spdlog/spdlog.h> 4 #include <opencv2/opencv.hpp> 5 using namespace std; 6 using namespace cv; 7 8 class ModelRotation 9 { 10 public: 11 static inline double L1EPS() { return 1E-12; } 12 static inline double L2EPS() { return 1E-18; } 13 14 public://RVec2Quat 15 struct RVec2Quat 16 { 17 bool useCeres; 18 RVec2Quat(bool useCeres0) : useCeres(useCeres0) {} 19 template <typename tp> bool operator()(const tp* const src, tp* dst) const 20 { 21 if (useCeres) { ceres::AngleAxisToQuaternion(src, dst); return true; } 22 tp theta2 = src[0] * src[0] + src[1] * src[1] + src[2] * src[2]; 23 if (theta2 > ModelRotation::L2EPS()) 24 { 25 tp theta = sqrt(theta2); 26 tp thetaa = theta * 0.5; 27 tp coeff = sin(thetaa) / theta; 28 dst[0] = cos(thetaa); 29 dst[1] = src[0] * coeff; 30 dst[2] = src[1] * coeff; 31 dst[3] = src[2] * coeff; 32 } 33 else //first order taylor 34 { 35 dst[0] = tp(1.); 36 dst[1] = src[0] * 0.5; 37 dst[2] = src[1] * 0.5; 38 dst[3] = src[2] * 0.5; 39 } 40 return true; 41 } 42 static void TestMe(int argc = 0, char** argv = 0) 43 { 44 for (int k = 0; k < 999; ++k) 45 { 46 //0.GenerateData 47 Matx31d r = r.randu(-999, 999); 48 49 //1.CalcByCeres 50 Vec4d q1; 51 Mat_<double> dqdr1(4, 3); 52 ceres::CostFunction* costFun1 = new ceres::AutoDiffCostFunction<RVec2Quat, 4, 3>(new RVec2Quat(true)); 53 costFun1->Evaluate(vector<double*>{r.val}.data(), q1.val, vector<double*>{dqdr1.ptr<double>()}.data()); 54 55 //2.CalcByManu 56 Vec4d q2; 57 Mat_<double> dqdr2(4, 3); 58 ceres::CostFunction* costFun2 = new ceres::AutoDiffCostFunction<RVec2Quat, 4, 3>(new RVec2Quat(false)); 59 costFun2->Evaluate(vector<double*>{r.val}.data(), q2.val, vector<double*>{dqdr2.ptr<double>()}.data()); 60 61 //3.AnalyzeError 62 double infq1q2 = cv::norm(q1, q2, NORM_INF); 63 double infdqdr1dqdr2 = cv::norm(dqdr1, dqdr2, NORM_INF); 64 65 //4.PrintError 66 cout << endl << "LoopCount: " << k << endl; 67 if (infq1q2 > 1E-12 || infdqdr1dqdr2 > 1E-12) 68 { 69 cout << endl << "infq1q2: " << infq1q2 << endl; 70 cout << endl << "infdqdr1dqdr2: " << infdqdr1dqdr2 << endl; 71 if (0) 72 { 73 cout << endl << "q1: " << q1 << endl; 74 cout << endl << "q2: " << q2 << endl; 75 cout << endl << "dqdr1: " << dqdr1 << endl; 76 cout << endl << "dqdr2: " << dqdr2 << endl; 77 } 78 cout << endl << "Press any key to continue" << endl; getchar(); 79 } 80 } 81 } 82 }; 83 struct Quat2RVec 84 { 85 bool useCeres; 86 Quat2RVec(bool useCeres0) : useCeres(useCeres0) {} 87 template <typename tp> bool operator()(const tp* const src, tp* dst) const 88 { 89 if (useCeres) { ceres::QuaternionToAngleAxis(src, dst); return true; } 90 tp thetaa2 = src[1] * src[1] + src[2] * src[2] + src[3] * src[3]; 91 if (thetaa2 > ModelRotation::L2EPS()) 92 { 93 tp cs = src[0]; 94 tp sn = sqrt(thetaa2); 95 tp theta = 2.0 * (cs < 0.0 ? atan2(-sn, -cs) : atan2(sn, cs));//refer to ceres comments 96 tp coeff = theta / sn; 97 dst[0] = src[1] * coeff; 98 dst[1] = src[2] * coeff; 99 dst[2] = src[3] * coeff; 100 } 101 else //first order taylor 102 { 103 dst[0] = 2.0 * src[1]; 104 dst[1] = 2.0 * src[2]; 105 dst[2] = 2.0 * src[3]; 106 } 107 return true; 108 } 109 static void TestMe(int argc = 0, char** argv = 0) 110 { 111 for (int k = 0; k < 999; ++k) 112 { 113 //0.GenerateData 114 Matx41d q = q.randu(-999, 999); 115 116 //1.CalcByCeres 117 Matx33d r1; 118 Mat_<double> drdq1(3, 4); 119 ceres::CostFunction* costFun1 = new ceres::AutoDiffCostFunction<Quat2RVec, 3, 4>(new Quat2RVec(true)); 120 costFun1->Evaluate(vector<double*>{q.val}.data(), r1.val, vector<double*>{drdq1.ptr<double>()}.data()); 121 122 //2.CalcByManu 123 Matx33d r2; 124 Mat_<double> drdq2(3, 4); 125 ceres::CostFunction* costFun2 = new ceres::AutoDiffCostFunction<Quat2RVec, 3, 4>(new Quat2RVec(false)); 126 costFun2->Evaluate(vector<double*>{q.val}.data(), r2.val, vector<double*>{drdq2.ptr<double>()}.data()); 127 128 //3.AnalyzeError 129 double infr1r2 = cv::norm(r1, r2, NORM_INF); 130 double infdrdq1drdq2 = cv::norm(drdq1, drdq2, NORM_INF); 131 132 //4.PrintError 133 cout << endl << "LoopCount: " << k << endl; 134 if (infr1r2 > 1E-12 || infdrdq1drdq2 > 1E-12) 135 { 136 cout << endl << "infr1r2: " << infr1r2 << endl; 137 cout << endl << "infdrdq1drdq2: " << infdrdq1drdq2 << endl; 138 if (0) 139 { 140 cout << endl << "r1: " << r1 << endl; 141 cout << endl << "r2: " << r2 << endl; 142 cout << endl << "drdq1: " << drdq1 << endl; 143 cout << endl << "drdq2: " << drdq2 << endl; 144 } 145 cout << endl << "Press any key to continue" << endl; getchar(); 146 } 147 } 148 } 149 }; 150 151 public://Quat2RMat 152 struct Quat2RMat 153 { 154 bool useCeres; 155 Quat2RMat(bool useCeres0) : useCeres(useCeres0) {} 156 template <typename tp> bool operator()(const tp* const src, tp* dst) const 157 { 158 if (useCeres) { ceres::QuaternionToRotation(src, dst); return true; } 159 tp ww = src[0] * src[0]; 160 tp wx = src[0] * src[1]; 161 tp wy = src[0] * src[2]; 162 tp wz = src[0] * src[3]; 163 164 tp xx = src[1] * src[1]; 165 tp xy = src[1] * src[2]; 166 tp xz = src[1] * src[3]; 167 168 tp yy = src[2] * src[2]; 169 tp yz = src[2] * src[3]; 170 171 tp zz = src[3] * src[3]; 172 tp L2 = ww + xx + yy + zz; 173 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; } 174 tp iL2 = 1. / L2; 175 176 dst[0] = ww + xx - yy - zz; dst[1] = 2.0 * (xy - wz); dst[2] = 2.0 * (wy + xz); 177 dst[3] = 2.0 * (wz + xy); dst[4] = ww - xx + yy - zz; dst[5] = 2.0 * (yz - wx); 178 dst[6] = 2.0 * (xz - wy); dst[7] = 2.0 * (wx + yz); dst[8] = ww - xx - yy + zz; 179 for (int k = 0; k < 9; ++k) dst[k] = dst[k] * iL2;//for not unit quaternion 180 181 return true; 182 } 183 static void TestMe(int argc = 0, char** argv = 0) 184 { 185 for (int k = 0; k < 999; ++k) 186 { 187 //0.GenerateData 188 Matx41d q = q.randu(-999, 999); 189 190 //1.CalcByCeres 191 Matx33d R1; 192 Mat_<double> dRdq1(9, 4); 193 ceres::CostFunction* costFun1 = new ceres::AutoDiffCostFunction<Quat2RMat, 9, 4>(new Quat2RMat(true)); 194 costFun1->Evaluate(vector<double*>{q.val}.data(), R1.val, vector<double*>{dRdq1.ptr<double>()}.data()); 195 196 //2.CalcByManu 197 Matx33d R2; 198 Mat_<double> dRdq2(9, 4); 199 ceres::CostFunction* costFun2 = new ceres::AutoDiffCostFunction<Quat2RMat, 9, 4>(new Quat2RMat(false)); 200 costFun2->Evaluate(vector<double*>{q.val}.data(), R2.val, vector<double*>{dRdq2.ptr<double>()}.data()); 201 202 //3.AnalyzeError 203 double infR1R2 = cv::norm(R1, R2, NORM_INF); 204 double infdRdq1dRdq2 = cv::norm(dRdq1, dRdq2, NORM_INF); 205 206 //4.PrintError 207 cout << endl << "LoopCount: " << k << endl; 208 if (infR1R2 > 1E-12 || infdRdq1dRdq2 > 1E-12) 209 { 210 cout << endl << "infR1R2: " << infR1R2 << endl; 211 cout << endl << "infdRdq1dRdq2: " << infdRdq1dRdq2 << endl; 212 if (0) 213 { 214 cout << endl << "R1: " << R1 << endl; 215 cout << endl << "R2: " << R2 << endl; 216 cout << endl << "dRdq1: " << dRdq1 << endl; 217 cout << endl << "dRdq2: " << dRdq2 << endl; 218 } 219 cout << endl << "Press any key to continue" << endl; getchar(); 220 } 221 } 222 } 223 }; 224 struct RMat2Quat 225 { 226 bool useCeres; 227 RMat2Quat(bool useCeres0) : useCeres(useCeres0) {} 228 template <typename tp> bool operator()(const tp* const src, tp* dst) const 229 { 230 if (useCeres) { ceres::RotationMatrixToQuaternion(ceres::RowMajorAdapter3x3(src), dst); return true; } 231 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) 232 if (trace >= 0.0) 233 { 234 tp trace1 = sqrt(1. + trace); 235 dst[0] = 0.5 * trace1; 236 tp itrace1 = 0.5 / trace1; 237 dst[1] = (src[7] - src[5]) * itrace1; 238 dst[2] = (src[2] - src[6]) * itrace1; 239 dst[3] = (src[3] - src[1]) * itrace1; 240 } 241 else 242 { 243 int a = 0; 244 if (src[4] > src[0]) a = 1; 245 if (src[8] > src[a * 3 + a]) a = 2; 246 int b = (a + 1) % 3; 247 int c = (b + 1) % 3; 248 249 tp trace1 = sqrt(1.0 + src[a * 3 + a] - src[b * 3 + b] - src[c * 3 + c]); 250 dst[a + 1] = 0.5 * trace1; 251 tp itrace1 = 0.5 / trace1; 252 dst[0] = (src[c * 3 + b] - src[b * 3 + c]) * itrace1; 253 dst[b + 1] = (src[b * 3 + a] + src[a * 3 + b]) * itrace1; 254 dst[c + 1] = (src[c * 3 + a] + src[a * 3 + c]) * itrace1; 255 } 256 return true; 257 } 258 static void TestMe(int argc, char** argv) 259 { 260 for (int k = 0; k < 999; ++k) 261 { 262 //0.GenerateData 263 Matx33d R; ceres::EulerAnglesToRotationMatrix(Matx31d::randu(-180, 180).val, 0, R.val); 264 265 //1.CalcByCeres 266 Vec4d q1; 267 Mat_<double> dqdR1(4, 9); 268 ceres::CostFunction* costFun1 = new ceres::AutoDiffCostFunction<RMat2Quat, 4, 9>(new RMat2Quat(true)); 269 costFun1->Evaluate(vector<double*>{R.val}.data(), q1.val, vector<double*>{dqdR1.ptr<double>()}.data()); 270 271 //2.CalcByManu 272 Vec4d q2; 273 Mat_<double> dqdR2(4, 9); 274 ceres::CostFunction* costFun2 = new ceres::AutoDiffCostFunction<RMat2Quat, 4, 9>(new RMat2Quat(false)); 275 costFun2->Evaluate(vector<double*>{R.val}.data(), q2.val, vector<double*>{dqdR2.ptr<double>()}.data()); 276 277 //4.AnalyzeError 278 double infq1q2 = cv::norm(q1, q2, NORM_INF); 279 double infdqdR1dqdR2 = cv::norm(dqdR1, dqdR2, NORM_INF); 280 281 //5.PrintError 282 cout << endl << "LoopCount: " << k << endl; 283 if (infq1q2 > 1E-9 || infdqdR1dqdR2 > 1E-9) 284 { 285 cout << endl << "infq1q2: " << infq1q2 << endl; 286 cout << endl << "infdqdR1dqdR2: " << infq1q2 << endl; 287 if (0) 288 { 289 cout << endl << "q1:" << q1 << endl; 290 cout << endl << "q2:" << q2 << endl; 291 cout << endl << "dqdR1:" << dqdR1 << endl; 292 cout << endl << "dqdR2:" << dqdR2 << endl; 293 } 294 cout << endl << "Press any key to continue" << endl; getchar(); 295 } 296 } 297 } 298 }; 299 300 public://RVec2RMat 301 struct RVec2RMat 302 { 303 bool useCeres; 304 RVec2RMat(bool useCeres0) : useCeres(useCeres0) {} 305 template <typename tp> bool operator()(const tp* const src, tp* dst) const 306 { 307 if (useCeres) { ceres::AngleAxisToRotationMatrix(src, ceres::RowMajorAdapter3x3(dst)); return true; } 308 tp theta2 = src[0] * src[0] + src[1] * src[1] + src[2] * src[2]; 309 if (theta2 > ModelRotation::L2EPS()) 310 { 311 tp theta = sqrt(theta2); 312 tp itheta = 1. / theta; 313 tp cs = cos(theta); 314 tp sn = sin(theta); 315 tp nx = src[0] * itheta; 316 tp ny = src[1] * itheta; 317 tp nz = src[2] * itheta; 318 319 dst[0] = nx * nx * (1. - cs) + cs; 320 dst[3] = nx * ny * (1. - cs) + nz * sn; 321 dst[6] = nx * nz * (1. - cs) - ny * sn; 322 323 dst[1] = nx * ny * (1. - cs) - nz * sn; 324 dst[4] = ny * ny * (1. - cs) + cs; 325 dst[7] = ny * nz * (1. - cs) + nx * sn; 326 327 dst[2] = nx * nz * (1. - cs) + ny * sn; 328 dst[5] = ny * nz * (1. - cs) - nx * sn; 329 dst[8] = nz * nz * (1. - cs) + cs; 330 } 331 else //first order taylor 332 { 333 dst[0] = tp(1.); 334 dst[3] = src[2]; 335 dst[6] = -src[1]; 336 337 dst[1] = -src[2]; 338 dst[4] = tp(1.); 339 dst[7] = src[0]; 340 341 dst[2] = src[1]; 342 dst[5] = -src[0]; 343 dst[8] = tp(1.); 344 } 345 return true; 346 } 347 static void TestMe(int argc = 0, char** argv = 0) 348 { 349 for (int k = 0; k < 999; ++k) 350 { 351 //0.GenerateData 352 Matx31d r = r.randu(-999, 999); 353 354 //1.CalcByCeres 355 Matx33d R1; 356 Mat_<double> dRdr1(9, 3); 357 ceres::CostFunction* costFun1 = new ceres::AutoDiffCostFunction<RVec2RMat, 9, 3>(new RVec2RMat(true)); 358 costFun1->Evaluate(vector<double*>{r.val}.data(), R1.val, vector<double*>{dRdr1.ptr<double>()}.data()); 359 360 //2.CalcByManu 361 Matx33d R2; 362 Mat_<double> dRdr2(9, 3); 363 ceres::CostFunction* costFun2 = new ceres::AutoDiffCostFunction<RVec2RMat, 9, 3>(new RVec2RMat(false)); 364 costFun2->Evaluate(vector<double*>{r.val}.data(), R2.val, vector<double*>{dRdr2.ptr<double>()}.data()); 365 366 //3.CalcByOpenCV 367 Matx33d R3; 368 Mat_<double> dRdr3; 369 cv::Rodrigues(r, R3, dRdr3); dRdr3 = dRdr3.t(); 370 371 //4.AnalyzeError 372 double infR1R2 = cv::norm(R1, R2, NORM_INF); 373 double infR1R3 = cv::norm(R1, R3, NORM_INF); 374 double infdRdr1dRdr2 = cv::norm(dRdr1, dRdr2, NORM_INF); 375 double infdRdr1dRdr3 = cv::norm(dRdr1, dRdr3, NORM_INF); 376 377 //5.PrintError 378 cout << endl << "LoopCount: " << k << endl; 379 if (infR1R2 > 1E-12 || infR1R3 > 1E-12 || infdRdr1dRdr2 > 1E-12 || infdRdr1dRdr3 > 1E-12) 380 { 381 cout << endl << "infR1R2: " << infR1R2 << endl; 382 cout << endl << "infR1R3: " << infR1R3 << endl; 383 cout << endl << "infdRdr3dRdr1: " << infdRdr1dRdr2 << endl; 384 cout << endl << "infdRdr3dRdr2: " << infdRdr1dRdr3 << endl; 385 if (0) 386 { 387 cout << endl << "R1: " << R1 << endl; 388 cout << endl << "R2: " << R2 << endl; 389 cout << endl << "R3: " << R3 << endl; 390 cout << endl << "dRdr1: " << dRdr1 << endl; 391 cout << endl << "dRdr2: " << dRdr2 << endl; 392 cout << endl << "dRdr3: " << dRdr3 << endl; 393 } 394 cout << endl << "Press any key to continue" << endl; getchar(); 395 } 396 } 397 } 398 }; 399 struct RMat2RVec 400 { 401 bool useCeres; 402 RMat2RVec(bool useCeres0) : useCeres(useCeres0) {} 403 template <typename tp> bool operator()(const tp* const src, tp* dst) const 404 { 405 if (useCeres) { ceres::RotationMatrixToAngleAxis(ceres::RowMajorAdapter3x3(src), dst); return true; } 406 tp quat[4]; 407 RMat2Quat(false)(src, quat); 408 Quat2RVec(false)(quat, dst); 409 return true; 410 } 411 static void TestMe(int argc, char** argv) 412 { 413 for (int k = 0; k < 999; ++k) 414 { 415 //0.GenerateData 416 Matx33d R; ceres::EulerAnglesToRotationMatrix(Matx31d::randu(-180, 180).val, 0, R.val); 417 418 //1.CalcByCeres 419 Vec3d r1; 420 Mat_<double> drdR1(3, 9); 421 ceres::CostFunction* costFun1 = new ceres::AutoDiffCostFunction<RMat2RVec, 3, 9>(new RMat2RVec(true)); 422 costFun1->Evaluate(vector<double*>{R.val}.data(), r1.val, vector<double*>{drdR1.ptr<double>()}.data()); 423 424 //2.CalcByManu 425 Vec3d r2; 426 Mat_<double> drdR2(3, 9); 427 ceres::CostFunction* costFun2 = new ceres::AutoDiffCostFunction<RMat2RVec, 3, 9>(new RMat2RVec(false)); 428 costFun2->Evaluate(vector<double*>{R.val}.data(), r2.val, vector<double*>{drdR2.ptr<double>()}.data()); 429 430 //3.AnalyzeError 431 double infr1r2 = cv::norm(r1, r2, NORM_INF); 432 double infdrdR1drdR2 = cv::norm(drdR1, drdR2, NORM_INF); 433 434 //5.PrintError 435 cout << endl << "LoopCount: " << k << endl; 436 if (infr1r2 > 1E-9 || infdrdR1drdR2 > 1E-9) 437 { 438 cout << endl << "infr1r2: " << infr1r2 << endl; 439 cout << endl << "infdrdR1drdR2: " << infdrdR1drdR2 << endl; 440 if (0) 441 { 442 cout << endl << "r1:" << r1 << endl; 443 cout << endl << "r2:" << r2 << endl; 444 cout << endl << "drdR1:" << drdR1 << endl; 445 cout << endl << "drdR2:" << drdR2 << endl; 446 } 447 cout << endl << "Press any key to continue" << endl; getchar(); 448 } 449 } 450 } 451 }; 452 453 public://Euler2RMat 454 struct Euler2RMat 455 { 456 bool useCeres; 457 Euler2RMat(bool useCeres0) : useCeres(useCeres0) {} 458 template <typename tp> bool operator()(const tp* const src, tp* dst) const 459 { 460 //RPY indicates: first Yaw aroundZ, second Pitch aroundY, third Roll aroundX 461 if (useCeres) { ceres::EulerAnglesToRotationMatrix(src, 0, dst); return true; } 462 const double deg2rad = 3.14159265358979323846 * 0.0055555555555555556; 463 tp sinR = sin(src[0] * deg2rad); 464 tp sinP = sin(src[1] * deg2rad); 465 tp sinY = sin(src[2] * deg2rad); 466 tp cosR = cos(src[0] * deg2rad); 467 tp cosP = cos(src[1] * deg2rad); 468 tp cosY = cos(src[2] * deg2rad); 469 470 dst[0] = cosY * cosP; dst[1] = cosY * sinP * sinR - sinY * cosR; dst[2] = cosY * sinP * cosR + sinY * sinR; 471 dst[3] = sinY * cosP; dst[4] = sinY * sinP * sinR + cosY * cosR; dst[5] = sinY * sinP * cosR - cosY * sinR; 472 dst[6] = -sinP; dst[7] = cosP * sinR; dst[8] = cosP * cosR; 473 return true; 474 } 475 static void TestMe(int argc = 0, char** argv = 0) 476 { 477 for (int k = 0; k < 999; ++k) 478 { 479 //0.GenerateData 480 Matx31d e = e.randu(-999, 999); 481 482 //1.CalcByCeres 483 Matx33d R1; 484 Mat_<double> dRde1(9, 3); 485 ceres::CostFunction* costFun1 = new ceres::AutoDiffCostFunction<Euler2RMat, 9, 3>(new Euler2RMat(true)); 486 costFun1->Evaluate(vector<double*>{e.val}.data(), R1.val, vector<double*>{dRde1.ptr<double>()}.data()); 487 488 //2.CalcByManu 489 Matx33d R2; 490 Mat_<double> dRde2(9, 3); 491 ceres::CostFunction* costFun2 = new ceres::AutoDiffCostFunction<Euler2RMat, 9, 3>(new Euler2RMat(false)); 492 costFun2->Evaluate(vector<double*>{e.val}.data(), R2.val, vector<double*>{dRde2.ptr<double>()}.data()); 493 494 //3.AnalyzeError 495 double infR1R2 = cv::norm(R1, R2, NORM_INF); 496 double infdRde1dRde2 = cv::norm(dRde1, dRde2, NORM_INF); 497 498 //4.PrintError 499 cout << endl << "LoopCount: " << k << endl; 500 if (infR1R2 > 1E-12 || infdRde1dRde2 > 1E-12) 501 { 502 cout << endl << "infR1R2: " << infR1R2 << endl; 503 cout << endl << "infdRde2dRde1: " << infdRde1dRde2 << endl; 504 if (0) 505 { 506 cout << endl << "R1: " << R1 << endl; 507 cout << endl << "R2: " << R2 << endl; 508 cout << endl << "dRde1: " << dRde1 << endl; 509 cout << endl << "dRde2: " << dRde2 << endl; 510 } 511 cout << endl << "Press any key to continue" << endl; getchar(); 512 } 513 } 514 } 515 }; 516 struct RMat2Euler 517 { 518 bool useCeres; 519 RMat2Euler(bool useCeres0) : useCeres(useCeres0) {} 520 template <typename tp> bool operator()(const tp* const src, tp* dst) const 521 { 522 //RPY indicates: first Yaw aroundZ, second Pitch aroundY, third Roll aroundX 523 if (useCeres) { ceres::RotationMatrixToEulerAngles(src, dst); return true; } 524 const double rad2deg = 180 * 0.3183098861837906715; 525 tp bound1 = abs(src[6] - 1.); 526 tp bound2 = abs(src[6] + 1.); 527 if (bound1 > ModelRotation::L1EPS() && bound2 > ModelRotation::L1EPS()) 528 { 529 dst[2] = atan2(src[3], src[0]); //Yaw aroundZ 530 dst[1] = asin(-src[6]);//Pitch aroundY 531 dst[0] = atan2(src[7], src[8]); //Roll aroundX 532 } 533 else if (bound2 <= ModelRotation::L1EPS()) 534 { 535 dst[2] = tp(0.); //Yaw aroundZ 536 dst[1] = tp(3.14159265358979323846 * 0.5);//Pitch aroundY 537 dst[0] = dst[2] + atan2(src[1], src[2]); //Roll aroundX 538 } 539 else 540 { 541 dst[2] = tp(0.); //Yaw aroundZ 542 dst[1] = tp(-3.14159265358979323846 * 0.5);//Pitch aroundY 543 dst[0] = -dst[2] + atan2(-src[1], -src[2]); //Roll aroundX 544 } 545 dst[0] *= rad2deg; dst[1] *= rad2deg; dst[2] *= rad2deg; 546 return true; 547 } 548 static void TestMe(int argc, char** argv) 549 { 550 for (int k = 0; k < 999; ++k) 551 { 552 //0.GenerateData 553 Vec3d e0(Matx31d::randu(-180, 180).val); 554 Matx33d R; ceres::EulerAnglesToRotationMatrix(e0.val, 0, R.val); 555 556 //1.CalcByCeres 557 Vec3d e1; 558 Mat_<double> dedR1(3, 9); 559 ceres::CostFunction* costFun1 = new ceres::AutoDiffCostFunction<RMat2Euler, 3, 9>(new RMat2Euler(true)); 560 costFun1->Evaluate(vector<double*>{R.val}.data(), e1.val, vector<double*>{dedR1.ptr<double>()}.data()); 561 562 //2.CalcByManu 563 Vec3d e2; 564 Mat_<double> dedR2(3, 9); 565 ceres::CostFunction* costFun2 = new ceres::AutoDiffCostFunction<RMat2Euler, 3, 9>(new RMat2Euler(false)); 566 costFun2->Evaluate(vector<double*>{R.val}.data(), e2.val, vector<double*>{dedR2.ptr<double>()}.data()); 567 568 //3.CalcByOpenCV 569 Vec3d e3; 570 Mat_<double> dedR3(3, 9); 571 e3 = cv::RQDecomp3x3(R, Matx33d(), Matx33d()); 572 573 //4.AnalyzeError 574 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)); 575 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)); 576 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)); 577 578 //5.PrintError 579 cout << endl << "LoopCount: " << k << endl; 580 if (infe0e1 > 1E-9 || infe0e2 > 1E-9 || infe0e3 > 1E-9) 581 { 582 cout << endl << "infe0e1: " << infe0e1 << endl; 583 cout << endl << "infe0e2: " << infe0e2 << endl; 584 cout << endl << "infe0e3: " << infe0e3 << endl; 585 if (0) 586 { 587 cout << endl << "e0: " << e0 << endl; 588 cout << endl << "e1: " << e1 << endl; 589 cout << endl << "e2: " << e2 << endl; 590 cout << endl << "e3: " << e3 << endl; 591 } 592 cout << endl << "Press any key to continue" << endl; getchar(); 593 } 594 } 595 } 596 }; 597 598 public: 599 struct SolidPose 600 { 601 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 }; 602 double* vdata = data; //Mat_<double> rvec = Mat_<double>(3, 1, vdata); 603 double* tdata = vdata + 3; //Mat_<double> tvec = Mat_<double>(3, 1, tdata); 604 double* qdata = tdata + 3; //Mat_<double> quat = Mat_<double>(4, 1, qdata); 605 double* mdata = qdata + 4; //Mat_<double> rmat = Mat_<double>(3, 3, mdata); 606 double* edata = mdata + 9; //Mat_<double> euler = Mat_<double>(3, 1, edata); 607 SolidPose() {} 608 SolidPose(const SolidPose& other) { memcpy(data, other.data, sizeof(data)); } 609 void setPos(double* xyz) { memcpy(tdata, xyz, 3 * sizeof(double)); } 610 void setRot(double* rot, int n = 0/*0 3 4 9 for euler rvec quat rmat*/) 611 { 612 if (n == 0) 613 { 614 memcpy(edata, rot, 3 * sizeof(double)); 615 ceres::EulerAnglesToRotationMatrix(rot, 0, mdata); 616 ceres::RotationMatrixToQuaternion(ceres::RowMajorAdapter3x3((const double*)(mdata)), qdata); 617 ceres::QuaternionToAngleAxis(qdata, vdata);//rmat->quat->rvec in ceres 618 } 619 else if (n == 3) 620 { 621 memcpy(vdata, rot, 3 * sizeof(double)); 622 ceres::AngleAxisToQuaternion(vdata, qdata); 623 ceres::AngleAxisToRotationMatrix(vdata, ceres::RowMajorAdapter3x3(mdata)); 624 ceres::RotationMatrixToEulerAngles(mdata, edata); 625 } 626 else if (n == 4) 627 { 628 memcpy(qdata, rot, 4 * sizeof(double)); 629 ceres::QuaternionToAngleAxis(qdata, vdata); 630 ceres::QuaternionToRotation(qdata, mdata); 631 ceres::RotationMatrixToEulerAngles(mdata, edata); 632 } 633 else if (n == 9) 634 { 635 memcpy(mdata, rot, 9 * sizeof(double)); 636 ceres::RotationMatrixToQuaternion(ceres::RowMajorAdapter3x3((const double*)(mdata)), qdata); 637 ceres::QuaternionToAngleAxis(qdata, vdata);//rmat->quat->rvec in ceres 638 ceres::RotationMatrixToEulerAngles(mdata, edata); 639 } 640 } 641 SolidPose inv() 642 { 643 SolidPose dst; 644 dst.vdata[0] = -vdata[0]; dst.vdata[1] = -vdata[1]; dst.vdata[2] = -vdata[2]; 645 dst.qdata[0] = qdata[0]; dst.qdata[1] = -qdata[1]; dst.qdata[2] = -qdata[2]; dst.qdata[3] = -qdata[3]; 646 dst.mdata[0] = mdata[0]; dst.mdata[1] = mdata[3]; dst.mdata[2] = mdata[6]; 647 dst.mdata[3] = mdata[1]; dst.mdata[4] = mdata[4]; dst.mdata[5] = mdata[7]; 648 dst.mdata[6] = mdata[2]; dst.mdata[7] = mdata[5]; dst.mdata[8] = mdata[8]; 649 ceres::RotationMatrixToEulerAngles(dst.mdata, dst.edata);//no euler inversion operation 650 dst.tdata[0] = -(dst.mdata[0] * tdata[0] + dst.mdata[1] * tdata[1] + dst.mdata[2] * tdata[2]); 651 dst.tdata[1] = -(dst.mdata[3] * tdata[0] + dst.mdata[4] * tdata[1] + dst.mdata[5] * tdata[2]); 652 dst.tdata[2] = -(dst.mdata[6] * tdata[0] + dst.mdata[7] * tdata[1] + dst.mdata[8] * tdata[2]); 653 return dst; 654 } 655 SolidPose mul(SolidPose& other) 656 { 657 SolidPose dst; 658 ceres::QuaternionProduct(qdata, other.qdata, dst.qdata); 659 memcpy(dst.mdata, (Matx33d(mdata) * Matx33d(other.mdata)).val, 9 * sizeof(double)); 660 ceres::QuaternionToAngleAxis(dst.qdata, dst.vdata);//no rvec multiplication operation 661 ceres::RotationMatrixToEulerAngles(dst.mdata, dst.edata);//no euler multiplication operation 662 memcpy(dst.tdata, (Matx33d(mdata) * Matx31d(other.tdata) + Matx31d(tdata)).val, 3 * sizeof(double)); 663 return dst; 664 } 665 string print() 666 { 667 string str; 668 str += fmt::format("rvec: [ {}, {}, {} ] ", vdata[0], vdata[1], vdata[2]); 669 str += fmt::format("tvec: [ {}, {}, {} ] ", tdata[0], tdata[1], tdata[2]); 670 str += fmt::format("quat: [ {}, {}, {}, {} ] ", qdata[0], qdata[1], qdata[2], qdata[3]); 671 str += fmt::format("rmat: [ {}, {}, {}, {}, {}, {}, {}, {}, {} ] ", mdata[0], mdata[1], mdata[2], mdata[3], mdata[4], mdata[5], mdata[6], mdata[7], mdata[8]); 672 str += fmt::format("euler: [ {}, {}, {} ] ", edata[0], edata[1], edata[2]); 673 return str; 674 } 675 }; 676 }; 677 678 #if 0 679 int main(int argc, char** argv) 680 { 681 ModelRotation::RVec2Quat::TestMe(argc, argv); 682 ModelRotation::Quat2RVec::TestMe(argc, argv); 683 684 ModelRotation::Quat2RMat::TestMe(argc, argv); 685 ModelRotation::RMat2Quat::TestMe(argc, argv); 686 687 ModelRotation::RVec2RMat::TestMe(argc, argv); 688 ModelRotation::RMat2RVec::TestMe(argc, argv); 689 690 ModelRotation::Euler2RMat::TestMe(argc, argv); 691 ModelRotation::RMat2Euler::TestMe(argc, argv); 692 return 0; 693 } 694 int main1(int argc, char** argv) { ModelRotation::RVec2Quat::TestMe(argc, argv); return 0; } 695 int main2(int argc, char** argv) { ModelRotation::RVec2RMat::TestMe(argc, argv); return 0; } 696 int main3(int argc, char** argv) { ModelRotation::Quat2RMat::TestMe(argc, argv); return 0; } 697 int main7(int argc, char** argv) { ModelRotation::Euler2RMat::TestMe(argc, argv); return 0; } 698 int main8(int argc, char** argv) { ModelRotation::RMat2Euler::TestMe(argc, argv); return 0; } 699 #endif