[first lecture on robot coordinate system]

//  The solution is the transformation matrix from the world coordinate system to the camera coordinate system 
cv::Mat Tcw = mpTracker->GrabImageRGBD(im,depthmap,timestamp);

//  The saved track is the transformation matrix from the camera coordinate system to the world coordinate system 
//  Change the point installation under the camera coordinate system to the world coordinate system 
mRcw = mTcw.rowRange(0,3).colRange(0,3);
mRwc = mRcw.t();
mRwc * x3Dc + -mRwc*mTcw.rowRange(0,3).col(3);

If the angle is negative , Right hand rule

 Robot coordinate system   Right handed coordinate system   Red front x    Green left y   On the blue z
x+ to forward, y+ to left, z+ to up
q.setRPY(0, M_PI/2, -M_PI/2); // -0 90 -90  Go around first z Axial negative 90 degree   Rewind y Axial alignment 90 degree   Finally around x pivot 0 degree 

//  Rotation matrix 
Eigen::Matrix3d rotation_matrix3d;						
//  Around the  Z  Shaft rotation  45  The rotation matrix of degrees  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.  The rotation matrix is converted to Euler angle ,"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 << " Around the z The rotation angle of the axis is  " << euler_angle.z() * 180 / M_PI << std::endl;
std::cout << " Around the y The rotation angle of the axis is  " << euler_angle.y() * 180 / M_PI << std::endl;
std::cout << " Around the x The rotation angle of the axis is  " << 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