calib matrix convert

tests/lidar_calib_tools.cpp

@@ -2,7 +2,7 @@
  * @Description: lidar calib tools
  * @Author: yct
  * @Date: 2021-09-02 11:02:00
- * @LastEditTime: 2021-12-01 11:02:07
+ * @LastEditTime: 2021-12-14 15:31:13
  * @LastEditors: yct
  */
 
@@ -175,8 +175,61 @@ void find_ground(pcl::PointCloud<pcl::PointXYZ>::Ptr ground_cloud, Eigen::Vector
     params << coefficients->values[0], coefficients->values[1], coefficients->values[2], coefficients->values[3];
 }
 
+void matrix_trans()
+{
+    // 921 A left
+    Eigen::Matrix3d rot_matrix0;
+    rot_matrix0 << 0.3967, -0.9023, -0.1686,
+        -0.8619, -0.4293, 0.2698,
+        -0.3159, 0.03827, -0.948;
+    Eigen::Matrix3d rot_matrix1;
+    rot_matrix1 << 0.9993, 0.03618, 0.0,
+        -0.03618, 0.9993, 0.0,
+        0.0, 0.0, 1.0;
+
+    Eigen::Vector3d euler = (rot_matrix1 * rot_matrix0).eulerAngles(2, 1, 0);
+    std::cout << "mat to euler left (z y x): " << euler[0]*180.0/M_PI << ", " << euler[1]*180.0/M_PI << ", " << euler[2]*180.0/M_PI << std::endl;
+
+    // 541 A right
+    Eigen::Matrix3d rot_matrix2;
+    rot_matrix2 << 0.0812, 0.997, 0.0185,
+        0.8787, -0.0628, -0.4731,
+        -0.4703, 0.0547, -0.8808;
+    Eigen::Matrix3d rot_matrix3;
+    rot_matrix3 << 0.97486, 0.2228, 0.0,
+        -0.2228, 0.97486, 0.0,
+        0.0, 0.0, 1.0;
+
+    euler = (rot_matrix3 * rot_matrix2).eulerAngles(2, 1, 0);
+    std::cout << "mat to euler right (z y x): " << euler[0]*180.0/M_PI << ", " << euler[1]*180.0/M_PI << ", " << euler[2]*180.0/M_PI << std::endl;
+
+    // 921----- -2.31 161.58 -64 2.512
+    // 541----- 176.45 28.05 71.85
+    euler << -2.31, 161.58, -64;
+    Eigen::Matrix3d euler2mat = Eigen::AngleAxisd(euler[2]*M_PI/180.0, Eigen::Vector3d::UnitZ()).toRotationMatrix() * 
+                                        Eigen::AngleAxisd(euler[1]*M_PI/180.0, Eigen::Vector3d::UnitY()).toRotationMatrix() *
+                                        Eigen::AngleAxisd(euler[0]*M_PI/180.0, Eigen::Vector3d::UnitX()).toRotationMatrix();
+    std::cout << "921 euler mat: \n" << euler2mat << std::endl;
+    // -0.415912  0.892481  0.174631
+    // 0.852745  0.449462 -0.266101
+    // -0.31598 0.0382411 -0.947995
+
+    euler << 176.45, 28.05, 71.85;
+    euler2mat = Eigen::AngleAxisd(euler[2]*M_PI/180.0, Eigen::Vector3d::UnitZ()).toRotationMatrix() * 
+                                        Eigen::AngleAxisd(euler[1]*M_PI/180.0, Eigen::Vector3d::UnitY()).toRotationMatrix() *
+                                        Eigen::AngleAxisd(euler[0]*M_PI/180.0, Eigen::Vector3d::UnitX()).toRotationMatrix();
+
+    std::cout << "541 euler mat: \n" << euler2mat << std::endl;
+    //   0.274916   0.957491 -0.0873633
+    // 0.838626   -0.28324  -0.465276
+    // -0.470242  0.0546463  -0.880844
+}
+
 int main()
 {
+    matrix_trans();
+    return 0;
+
     // calib process:
     // 1. cc cut and fit ground, get normal and z
     // 2. get front and back pole txt
@@ -185,22 +238,21 @@ int main()
     // lidar 0 normal [09:22:54] 	- normal: (-0.000392,-0.011935,0.999929) z -0.07697
     // 90.6538 0.68354 0.0302616
 
-    // lidar 1 normal [09:52:53] 	- normal: (0.000297,-0.001893,0.999998) z -0.0686
+    // lidar 1 normal 0.000297,-0.001893,0.999998 z -0.0686 
     // 91.3879 0.108841 -0.0143847
 
-    // lidar 2 normal [16:45:14] 	- normal: (0.000484,-0.022007,0.999758) z -0.0461051
+    // lidar 2 normal [10:43:07] 	- normal: (0.001131,-0.024040,0.999710) z -0.0461051
     // 86.3796  1.37069 -0.15165
-    // 86.3802   1.25674 -0.107284
 
     pcl::PointCloud<pcl::PointXYZ>::Ptr t_front, t_back;
     t_front = pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>);
     t_back = pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>);
-    if (!read_pointcloud("/home/zx/yct/chutian_measure_2021/setting/calib/2front_pole.txt", t_front))
+    if (!read_pointcloud("/home/youchen/extra_space/chutian/measure/puai_wj_2021/setting/calib/2front_pole.txt", t_front))
     {
         std::cout << "read front failed." << std::endl;
         return -1;
     }
-    if(!read_pointcloud("/home/zx/yct/chutian_measure_2021/setting/calib/2back_pole.txt", t_back))
+    if(!read_pointcloud("/home/youchen/extra_space/chutian/measure/puai_wj_2021/setting/calib/2back_pole.txt", t_back))
     {
         std::cout << "read back failed." << std::endl;
         return -1;
@@ -211,7 +263,7 @@ int main()
     // 经过测试，地面法向量不适宜使用杆子获取，还是使用地面环点云拟合获得
     // find_intrinsic((front_direction + back_direction) / 2.0, y_direction);
     // find_intrinsic(Eigen::Vector3d(-0.000392,-0.011935,0.999929), y_direction);
-    find_intrinsic(Eigen::Vector3d(0.000484,-0.022007,0.999758), y_direction);
+    find_intrinsic(Eigen::Vector3d(0.001131,-0.024040,0.999710), y_direction);
 
     return 0;
 
@@ -233,4 +285,4 @@ int main()
     find_intrinsic(Eigen::Vector3d(-0.00566995,-0.0233422,0.999712), y_direction);
 
     return 0;
-}
+}