Eigen::Quaterniond q0(0.8026, 0.1804, -0.5130, -0.2452); // 按照0度图片构造
Eigen::Quaterniond q90(0.1136, -0.1687, -0.4764, -0.8554); // 按照90度图片构造
Eigen::Matrix3d rotationMatrix0 = q0.toRotationMatrix(); // 4元数转旋转矩阵
Eigen::Matrix3d rotationMatrix90 = q90.toRotationMatrix(); // 4元数转旋转矩阵
Eigen::MatrixXd R_B1A(3, 4), R_B2A(3, 4);
// 添加tx,ty,tz
Eigen::Vector3d txyz0(-205.18, 170.00, -1555.46);
Eigen::Vector3d txyz90(-132.46, 336.67, -1479.16);
R_B1A << rotationMatrix0, txyz0;
R_B2A << rotationMatrix90, txyz90;
// 添加0行
Eigen::Vector4d emptyRow(0, 0, 0, 1);
R_B1A.conservativeResize(4, 4);
R_B1A.row(3) = emptyRow;
R_B2A.conservativeResize(4, 4);
R_B2A.row(3) = emptyRow;
/*
cout << rotationMatrix0 << "\n" << endl;
cout << rotationMatrix90 << "\n" << endl;
cout << R_B1A << "\n" << endl;
cout << R_B2A << "\n" << endl;
*/
q0和q90都是(w,x,y,z)的形式,假设四元数表示的是C在A下的坐标,那么生成的矩阵 R C A R_{CA} RCA的用法是 P c = R C A × P a P_c=R_{CA}\times P_a Pc=RCA×Pa。点p在C坐标系下的坐标等于旋转矩阵乘以在A下的坐标。