点击(此处)折叠或打开
-
/// 得到的位姿估计是:从Marker坐标系到相机坐标系的
-
cv::Mat R;
-
cv::Rodrigues(rvecs[i],R);
-
cout << "ID :" << ids[i] << endl;
-
cout << "R_{camera<---marker} :" << R << endl;
-
cout << "t_{camera<---marker} :" << tvecs[i] << endl;
-
cout << endl;
-
cv::aruco::drawAxis(imageCopy, cameraMatrix, distCoeffs, rvecs[i], tvecs[i], 0.1);
-
-
Eigen::Matrix3d R_eigen;
-
cv::cv2eigen(R,R_eigen);
-
Eigen::Vector3d zyx_Euler_fromR=R_eigen.eulerAngles(0,1,2);//Eigen中使用右乘的顺序,因此ZYX对应的是012,实际上这个编号跟乘法的顺序一致就可以了(从左向又右看的顺序)
- cout<< "zyx euler from Rotation \n[输出顺序为:x,y,z]:\n"<<(180)/(M_PI)*zyx_Euler_fromR.transpose()<<endl
eigen 中四元数、欧拉角、旋转矩阵、旋转向量
点击(此处)折叠或打开
-
一、旋转向量
-
-
1.0 初始化旋转向量:旋转角为alpha,旋转轴为(x,y,z)
-
-
Eigen::AngleAxisd rotation_vector(alpha,Vector3d(x,y,z))
-
-
1.1 旋转向量转旋转矩阵
-
-
Eigen::Matrix3d rotation_matrix;rotation_matrix=rotation_vector.matrix();
-
-
Eigen::Matrix3d rotation_matrix;rotation_matrix=rotation_vector.toRotationMatrix();
-
-
1.2 旋转向量转欧拉角(Z-Y-X,即RPY)
-
-
Eigen::Vector3d eulerAngle=rotation_vector.matrix().eulerAngles(2,1,0);
-
-
1.3 旋转向量转四元数
-
-
Eigen::Quaterniond quaternion(rotation_vector);
-
-
Eigen::Quaterniond quaternion;Quaterniond quaternion;
-
-
Eigen::Quaterniond quaternion;quaternion=rotation_vector;
-
-
二、旋转矩阵
-
-
2.0 初始化旋转矩阵
-
-
Eigen::Matrix3d rotation_matrix;rotation_matrix<<x_00,x_01,x_02,x_10,x_11,x_12,x_20,x_21,x_22;
-
-
2.1 旋转矩阵转旋转向量
-
-
Eigen::AngleAxisd rotation_vector(rotation_matrix);
-
-
-
Eigen::AngleAxisd rotation_vector;rotation_vector=rotation_matrix;
-
-
Eigen::AngleAxisd rotation_vector;rotation_vector.fromRotationMatrix(rotation_matrix);
-
-
2.2 旋转矩阵转欧拉角(Z-Y-X,即RPY)
-
-
Eigen::Vector3d eulerAngle=rotation_matrix.eulerAngles(2,1,0);
-
-
2.3 旋转矩阵转四元数
-
-
Eigen::Quaterniond quaternion(rotation_matrix);
-
-
Eigen::Quaterniond quaternion;quaternion=rotation_matrix;
-
-
三、欧拉角
-
-
3.0 初始化欧拉角(Z-Y-X,即RPY)
-
-
Eigen::Vector3d eulerAngle(yaw,pitch,roll);
-
-
3.1 欧拉角转旋转向量
-
-
Eigen::AngleAxisd rollAngle(AngleAxisd(eulerAngle(2),Vector3d::UnitX()));Eigen::AngleAxisd pitchAngle(AngleAxisd(eulerAngle(1),Vector3d::UnitY()));Eigen::AngleAxisd yawAngle(AngleAxisd(eulerAngle(0),Vector3d::UnitZ())); Eigen::AngleAxisd rotation_vector;rotation_vector=yawAngle*pitchAngle*rollAngle;
-
-
3.2 欧拉角转旋转矩阵
-
-
Eigen::AngleAxisd rollAngle(AngleAxisd(eulerAngle(2),Vector3d::UnitX()));Eigen::AngleAxisd pitchAngle(AngleAxisd(eulerAngle(1),Vector3d::UnitY()));Eigen::AngleAxisd yawAngle(AngleAxisd(eulerAngle(0),Vector3d::UnitZ())); Eigen::Matrix3d rotation_matrix;rotation_matrix=yawAngle*pitchAngle*rollAngle;
-
-
3.3 欧拉角转四元数
-
-
Eigen::AngleAxisd rollAngle(AngleAxisd(eulerAngle(2),Vector3d::UnitX()));Eigen::AngleAxisd pitchAngle(AngleAxisd(eulerAngle(1),Vector3d::UnitY()));Eigen::AngleAxisd yawAngle(AngleAxisd(eulerAngle(0),Vector3d::UnitZ())); Eigen::Quaterniond quaternion;quaternion=yawAngle*pitchAngle*rollAngle;
-
-
四、四元数
-
-
4.0 初始化四元数
-
-
Eigen::Quaterniond quaternion(w,x,y,z);
-
-
4.1 四元数转旋转向量
-
-
Eigen::AngleAxisd rotation_vector(quaternion);
-
-
Eigen::AngleAxisd rotation_vector;rotation_vector=quaternion;
-
-
4.2 四元数转旋转矩阵
-
-
Eigen::Matrix3d rotation_matrix;rotation_matrix=quaternion.matrix();
-
-
Eigen::Matrix3d rotation_matrix;rotation_matrix=quaternion.toRotationMatrix();
-
-
4.4 四元数转欧拉角(Z-Y-X,即RPY)
-
- Eigen::Vector3d eulerAngle=quaternion.matrix().eulerAngles(2,1,0)