【机器人坐标系第一讲】

// 求解的是世界坐标系到相机坐标系的变换矩阵
cv::Mat Tcw = mpTracker->GrabImageRGBD(im,depthmap,timestamp);

// 保存轨迹是相机坐标系到世界坐标系的变换矩阵
// 将相机坐标系下面的点装换到世界坐标系下面
mRcw = mTcw.rowRange(0,3).colRange(0,3);
mRwc = mRcw.t();
mRwc * x3Dc + -mRwc*mTcw.rowRange(0,3).col(3);

角度要是负数，右手法则

机器人坐标系 符合右手坐标系 红前x   绿左y  蓝上z
x+ to forward, y+ to left, z+ to up
q.setRPY(0, M_PI/2, -M_PI/2); // -0 90 -90 先绕z轴负90度 再绕y轴正90度 最后绕x轴转0度

// 旋转矩阵
Eigen::Matrix3d rotation_matrix3d;						
// 绕 Z 轴旋转 45 度的旋转矩阵 sin45 = cos45 = 0.707107
rotation_matrix3d << cos45, -sin45, 0,
                   sin45,  cos45, 0,
                   0,         0,        1;
rotation_matrix3d << 0.707107, -0.707107, 0,
                   0.707107,  0.707107, 0,
                   0,         0,        1;
std::cout << "rotation_matrix =\n" << rotation_matrix << std::endl;
/* * rotation_matrix = * 0.707107 -0.707107 0 * 0.707107 0.707107 0 * 0 0 1 */
 

// 2. 旋转矩阵转换为欧拉角,"2" represents the z axis , "0" x axis, "1" y axis
Eigen::Vector3d euler_angle = rotation_matrix3d.eulerAngles(2, 1, 0);// z y x

std::cout << "绕z轴旋转的角度是 " << euler_angle.z() * 180 / M_PI << std::endl;
std::cout << "绕y轴旋转的角度是 " << euler_angle.y() * 180 / M_PI << std::endl;
std::cout << "绕x轴旋转的角度是 " << euler_angle.x() * 180 / M_PI << std::endl;

tf_camera_rotation.getEulerYPR(z,y,x);
std::cout << "zb " << z << " yb " << y << " xb " << x << std::endl; // rotation_matrix3d.x() -1.57062 rotation_matrix3d.y() -0.000116255 rotation_matrix3d.z() -1.57072