calib no.4 vlp16, add some print.

3d wheel detect need debug.

setting/communication.prototxt

@@ -10,7 +10,7 @@ communication_parameters
   # connect_string_vector:"tcp://192.168.2.125:9876"
   # connect_string_vector:"tcp://192.168.2.166:1234"
 
- #  bind_string:"tcp://192.168.2.192:30001"
+  # bind_string:"tcp://192.168.2.108:30010"
  #  bind_string:"tcp://192.168.2.167:30001"
 
 

setting/velodyne_manager.prototxt

@@ -4,7 +4,7 @@ left_model_path:"/home/youchen/extra_space/chutian/measure/puai_wj_2021/setting/
 right_model_path:"/home/youchen/extra_space/chutian/measure/puai_wj_2021/setting/right_model.txt"
 distribution_mode:false
 
-# 1 lidar
+# 6 lidar 192.168.1.201
 velodyne_lidars
 {
     ip:""
@@ -30,7 +30,7 @@ velodyne_lidars
     }
 }
 
-# 2 lidar
+# 5 lidar 192.168.1.202
 velodyne_lidars
 {
     ip:""
@@ -56,24 +56,27 @@ velodyne_lidars
     }
 }
 
-# # 3 lidar
-# velodyne_lidars
-# {
-#     ip:""
-#     port:2370
-#     model:"VLP16"
-#     calibrationFile:"../setting/VLP16db.yaml"
-#     lidar_id:2
-#     max_range:5.0
-#     min_range:0.1
-#     min_angle:0
-#     max_angle:360
-#     rpm:600
-#     calib
-#     {
-#         y:1.57
-#     }
-# }
+# 3 lidar 192.168.1.203
+velodyne_lidars
+{
+    ip:""
+    port:2370
+    model:"VLP16"
+    calibrationFile:"../setting/VLP16db.yaml"
+    lidar_id:4
+    max_range:8.0
+    min_range:0.1
+    min_angle:0
+    max_angle:360
+    rpm:600
+    calib
+    {
+        r:-0.038115
+        p:-0.577136
+        y:91.2659
+        cz:0.06
+    }
+}
 
 # 1 region
 region
@@ -113,32 +116,39 @@ region
     }
 }
 
-# # 2 region
-# region
-# {
-#     minx:-3.1
-# 	maxx:0
-# 	miny:-2.75
-# 	maxy:3.08
-# 	minz:0.01
-# 	maxz:0.2
-#     region_id:1
-#     lidar_exts
-#     {
-#         lidar_id:1
-#         calib
-#         {
-#             cx:0.1
-#             cy:0.3
-#         }
-#     }
-#     lidar_exts
-#     {
-#         lidar_id:2
-#         calib
-#         {
-#             cx:0.1
-#             cy:0.3
-#         }
-#     }
-# }
+# 2 region
+region
+{
+    minx:-1.5
+	maxx:1.5
+	miny:-2.6
+	maxy:2.0
+	minz:0.03
+	maxz:1.0
+    region_id:4
+    turnplate_cx:0.0
+    turnplate_cy:0.0
+    border_minx:-1.3
+    border_maxx:1.3
+    plc_offsetx:0.0
+    plc_offsety:0.0
+    # 4-->5: -3.81895 0.038776
+    lidar_exts
+    {
+        lidar_id:5
+        calib
+        {
+            cx:1.9095
+            cy:-0.019388
+        }
+    }
+    lidar_exts
+    {
+        lidar_id:4
+        calib
+        {
+            cx:-1.9095
+            cy:0.019388
+        }
+    }
+}

tests/lidar_calib_tools.cpp

@@ -2,7 +2,7 @@
  * @Description: lidar calib tools
  * @Author: yct
  * @Date: 2021-09-02 11:02:00
- * @LastEditTime: 2021-09-07 17:53:53
+ * @LastEditTime: 2021-10-12 14:01:29
  * @LastEditors: yct
  */
 
@@ -133,8 +133,7 @@ void find_rotation_from_points(pcl::PointCloud<pcl::PointXYZ>::Ptr front_barrier
 
 int main()
 {
-    std::cout << "------------------------ start calib lidar 0 ----------------------" << std::endl;
-    // cloud 0 calib
+    std::cout << "------------------------ start calib lidar 4 ----------------------" << std::endl;
     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>);
@@ -154,11 +153,13 @@ int main()
 
     // front direction: 0.0191082  0.002411  0.999815
     // back direction: 0.0190939 0.0143106  0.999715
-    // 00.0103153 -0.0110542 000.999886 000.101527
+    // -0.000887596 00-0.0100556 00000.999949 0000.0650278
     find_rotation_from_points(t_front, t_back, front_direction, back_direction, y_direction);
     // 经过测试，地面法向量不适宜使用杆子获取，还是使用地面环点云拟合获得
     // find_intrinsic((front_direction + back_direction) / 2.0, y_direction);
-    find_intrinsic(Eigen::Vector3d(0.0103153,-0.0110542,0.999886), y_direction);
+    find_intrinsic(Eigen::Vector3d(-0.000887596, 0.0100556, 0.999949), y_direction);
+
+    return 0;
 
     std::cout << "------------------------ start calib lidar 1 ----------------------" << std::endl;
     // cloud 1

velodyne_lidar/car_pose_detector.cpp

@@ -2,7 +2,7 @@
  * @Description: 
  * @Author: yct
  * @Date: 2021-09-16 15:18:34
- * @LastEditTime: 2021-09-24 17:55:03
+ * @LastEditTime: 2021-10-12 17:38:39
  * @LastEditors: yct
  */
 
@@ -90,9 +90,9 @@ bool Car_pose_detector::detect_pose(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_pt
 
     // 配置求解器
     ceres::Solver::Options options;                            // 这里有很多配置项可以填
-    options.linear_solver_type = ceres::DENSE_NORMAL_CHOLESKY; // 增量方程如何求解
+    options.linear_solver_type = ceres::DENSE_QR; // 增量方程如何求解
     options.minimizer_progress_to_stdout = false;              // 输出到cout
-    // options.max_num_iterations = 100;
+    options.max_num_iterations = 100;
 
     ceres::Solver::Summary summary; // 优化信息
     std::chrono::steady_clock::time_point t1 = std::chrono::steady_clock::now();
@@ -109,7 +109,7 @@ bool Car_pose_detector::detect_pose(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_pt
     theta = t_vars[2];
     if(fabs(theta)>30*M_PI/180.0)
     {
-        std::cout << "wrong angle, detect failed" << std::endl;
+        std::cout << "wrong angle, detect failed" << theta*180.0/M_PI << std::endl;
         return false;
     }
 

velodyne_lidar/chassis_ceres_solver.cpp

@@ -270,6 +270,8 @@ cv::Mat chassis_ceres_solver::create_mat()
             t_mat.at<float>(i, j) = fx * fy;
             // std::cout << "x,y: " << x << ", " << y << "-----value: " << 1.0 / (1.0 + std::exp(-CERES_PA * (x + default_width / 2.0)))<<", "<<
             //   1.0 / (1.0 + std::exp(-CERES_PA * (x - default_width / 2.0))) << std::endl;
+            // if(fx*fy>0)
+            //     std::cout << "x,y: " << x << "," << y << ", fx,fy: " << fx << ", " << fy << ", " << fx * fy << std::endl;
         }
     }
     cv::normalize(t_mat, t_mat, 1.0, 0.1, cv::NORM_MINMAX);

velodyne_lidar/ground_region.cpp

@@ -221,7 +221,7 @@ bool Ground_region::classify_ceres_detect(pcl::PointCloud<pcl::PointXYZ>::Ptr cl
     //下采样
     pcl::VoxelGrid<pcl::PointXYZ> vox;   //创建滤波对象
     vox.setInputCloud(cloud_filtered);   //设置需要过滤的点云给滤波对象
-    vox.setLeafSize(0.02f, 0.02f, 0.02f); //设置滤波时创建的体素体积为1cm的立方体
+    vox.setLeafSize(0.01f, 0.01f, 0.01f); //设置滤波时创建的体素体积为1cm的立方体
     vox.filter(*cloud_filtered);         //执行滤波处理，存储输出
     if (cloud_filtered->size() == 0)
     {
@@ -264,7 +264,7 @@ bool Ground_region::classify_ceres_detect(pcl::PointCloud<pcl::PointXYZ>::Ptr cl
         }
         else
         {
-            // LOG(WARNING) << error_str;
+            LOG(WARNING) << error_str;
             return false;
         }
     }
@@ -337,11 +337,11 @@ bool computer_var(std::vector<double> data, double &var)
 
 Error_manager Ground_region::detect(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, detect_wheel_ceres3d::Detect_result &last_result)
 {
-    // 1.*********点云合法性检验
+    // 1.*********点云合法性检验*********
     if (cloud->size() == 0)
         return Error_manager(VELODYNE_REGION_EMPTY_CLOUD, NORMAL, "no point");
 
-    // 2.*********点云预处理
+    // 2.*********点云预处理*********
     std::chrono::steady_clock::time_point t1 = std::chrono::steady_clock::now();
     pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered(new pcl::PointCloud<pcl::PointXYZ>);
     for (int i = 0; i < cloud->size(); ++i)
@@ -357,7 +357,7 @@ Error_manager Ground_region::detect(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, d
     pcl::StatisticalOutlierRemoval<pcl::PointXYZ> sor;
     sor.setInputCloud(cloud_filtered);
     sor.setMeanK(15);            //K近邻搜索点个数
-    sor.setStddevMulThresh(3.0); //标准差倍数
+    sor.setStddevMulThresh(1.0); //标准差倍数
     sor.setNegative(false);      //保留未滤波点（内点）
     sor.filter(*cloud_filtered); //保存滤波结果到cloud_filter
 
@@ -379,19 +379,20 @@ Error_manager Ground_region::detect(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, d
     mp_cloud_filtered->operator+=(*cloud_filtered);
     m_filtered_cloud_mutex.unlock();
 
-    // 3.*********位姿优化，获取中心xy与角度
+    // 3.*********位姿优化，获取中心xy与角度*********
     pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_detect_z(new pcl::PointCloud<pcl::PointXYZ>);
     double x, y, theta, width, z_value=0.2;
     if(!Car_pose_detector::get_instance_references().detect_pose(cloud_filtered, cloud_detect_z, x, y, theta, width, z_value, false))
     {
-        return Error_manager(VELODYNE_REGION_CERES_SOLVE_ERROR, NEGLIGIBLE_ERROR, "find chassis z value failed.");
+        return Error_manager(VELODYNE_REGION_CERES_SOLVE_ERROR, NEGLIGIBLE_ERROR, "find general car pose value failed.");
     }
-    std::cout << "cx: " << x+m_region.plc_offsetx() << ", cy: " << y+m_region.plc_offsety() << ", theta: " << theta*180.0/M_PI << std::endl;
+    LOG(INFO) << "car pose :::: cx: " << x+m_region.plc_offsetx() << ", cy: " << y+m_region.plc_offsety() << ", theta: " << theta*180.0/M_PI << std::endl;
 
     std::chrono::steady_clock::time_point t2 = std::chrono::steady_clock::now();
     std::chrono::duration<double> time_used_bowl = std::chrono::duration_cast<std::chrono::duration<double>>(t2 - t1);
 
-    // 4.*********xoz优化获取底盘高度
+    // 4.*********xoz优化获取底盘高度*********
+    // 重新取包含地面点的点云,用于底盘优化
     chassis_ceres_solver t_solver;
     pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_z_solver(new pcl::PointCloud<pcl::PointXYZ>);
     for (int i = 0; i < cloud->size(); ++i)
@@ -402,15 +403,19 @@ Error_manager Ground_region::detect(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, d
             cloud_z_solver->push_back(pt);
         }
     }
-    double mid_z = 0.06, height = 0.08;
+    //离群点过滤
+    sor.setInputCloud(cloud_z_solver);
+    sor.setMeanK(15);            //K近邻搜索点个数
+    sor.setStddevMulThresh(1.0); //标准差倍数
+    sor.setNegative(false);      //保留未滤波点（内点）
+    sor.filter(*cloud_z_solver); //保存滤波结果到cloud_filter
+
+    double mid_z = 0.05, height = 0.08;
+    // 去中心，角度调正
     Car_pose_detector::get_instance_references().inv_trans_cloud(cloud_z_solver, x, y, theta);
-    // //下采样
-    // vox.setInputCloud(cloud_z_solver);   //设置需要过滤的点云给滤波对象
-    // vox.setLeafSize(0.02f, 0.02f, 0.02f); //设置滤波时创建的体素体积为1cm的立方体
-    // vox.filter(*cloud_z_solver);         //执行滤波处理，存储输出
 
-    if (cloud_filtered->size() == 0)
-        return Error_manager(VELODYNE_REGION_EMPTY_CLOUD, NORMAL, "filtered no point");
+    if (cloud_z_solver->size() == 0)
+        return Error_manager(VELODYNE_REGION_EMPTY_CLOUD, NORMAL, "z solver no point");
 
     x = 0.0;
     width = 1.0;
@@ -431,6 +436,9 @@ Error_manager Ground_region::detect(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, d
     //     cloud_z_solver->push_back(pcl::PointXYZ(width/2.0, i, 0));
     // }
     // std::cout << "\n------------------------------------ chassis z1: " << mid_z + height / 2.0 << std::endl;
+
+    if(ec != SUCCESS)
+        return Error_manager(VELODYNE_REGION_CERES_SOLVE_ERROR, NORMAL, "failed to find chassis z value.");
     std::chrono::steady_clock::time_point t3 = std::chrono::steady_clock::now();
     std::chrono::duration<double> time_used_block = std::chrono::duration_cast<std::chrono::duration<double>>(t3 - t2);
 
@@ -440,19 +448,21 @@ Error_manager Ground_region::detect(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, d
     mp_cloud_detect_z->operator+=(*cloud_z_solver);
     m_detect_z_cloud_mutex.unlock();
 
-    // 二分法存在错误，直接使用底盘z值
+    // 二分法存在错误弃用！！！直接使用底盘z值
     std::vector<detect_wheel_ceres3d::Detect_result> results;
     detect_wheel_ceres3d::Detect_result result;
     double chassis_z = mid_z + height / 2.0; // + 0.02;
     if(chassis_z > m_region.maxz() || chassis_z < m_region.minz())
     {
-        return Error_manager(VELODYNE_REGION_CERES_SOLVE_ERROR, NEGLIGIBLE_ERROR, (std::string("failed to find chassis z value: ")+std::to_string(chassis_z)).c_str());
+        return Error_manager(VELODYNE_REGION_CERES_SOLVE_ERROR, NEGLIGIBLE_ERROR, (std::string("optimized chassis z value out of range: ")+std::to_string(chassis_z)).c_str());
     }
     bool ret = false;
-    while(chassis_z > (mid_z-0.02))
+    
+    while (chassis_z > (mid_z))
     {
+        LOG_IF(WARNING, m_region.region_id() == 4) << "chassis z: " << chassis_z;
         ret = classify_ceres_detect(cloud_filtered, chassis_z, result);
-        // changed by yct, 暂时直接识别，调试用，之后将条件恢复
+        // changed by yct, 根据底盘z识别，失败则向下移动直至成功或超过底盘内空中点
         if(ret)
         {
             results.push_back(result);
@@ -461,33 +471,6 @@ Error_manager Ground_region::detect(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, d
             chassis_z -= 0.01;
         }
     }
-    // float start_z = m_region.minz();
-    // float max_z = z_value;//0.2;
-    // float center_z = (start_z + max_z) / 2.0;
-    // float last_center_z = start_z;
-    // float last_succ_z = -1.0;
-    // int count = 0;
-    // //二分法 找识别成功的 最高的z
-    // std::vector<detect_wheel_ceres3d::Detect_result> results;
-    // do
-    // {
-    //     detect_wheel_ceres3d::Detect_result result;
-    //     bool ret = classify_ceres_detect(cloud_filtered, center_z, result);
-    //     // std::cout << "z: " << center_z <<", "<<start_z<<"," << max_z <<(ret?"clustered":"clustering failed")<< std::endl;
-    //     if (ret)
-    //     {
-    //         results.push_back(result);
-    //         last_succ_z = center_z;
-    //         start_z = center_z;
-    //         last_center_z = center_z;
-    //     }
-    //     else
-    //     {
-    //         max_z = center_z;
-    //         last_center_z = center_z;
-    //     }
-    //     center_z = (start_z + max_z) / 2.0;
-    //     count++;
 
     // } while (fabs(center_z - last_center_z) > 0.01);
     std::chrono::steady_clock::time_point t4 = std::chrono::steady_clock::now();
@@ -497,18 +480,18 @@ Error_manager Ground_region::detect(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, d
     if (results.size() == 0)
     {
         // 20201010, may lead to problem in chutian, uncomment in debug only
-        // // changed by yct, save 3d wheel detect result. 
-        // static int save_debug = 0;
-        // if (save_debug++ == 2)
-        //     m_detector->save_debug_data("/home/youchen/extra_space/chutian/measure/chutian_velo_ws/log/debug");
+        // changed by yct, save 3d wheel detect result. 
+        static int save_debug = 0;
+        if (save_debug++ == 5)
+            m_detector->save_debug_data("/home/youchen/extra_space/chutian/measure/chutian_velo_ws/log/debug");
 
         // std::cout << "\n-------- no result: " << std::endl;
-        LOG(INFO) << "failed with midz, currz: " << mid_z << ", " << chassis_z;
-        return Error_manager(FAILED, NORMAL, "no car detected");
+        LOG_IF(INFO, m_region.region_id() == 4) << "failed with midz, currz: " << mid_z << ", " << chassis_z;
+        return Error_manager(VELODYNE_REGION_CERES_SOLVE_ERROR, NEGLIGIBLE_ERROR, "3d wheel detect failed.");
     }
     else
     {
-        LOG(INFO) << "detected with suitable z value: " << chassis_z;
+        LOG_IF(INFO, m_region.region_id() == 4) << "detected with suitable z value: " << chassis_z;
     }
     ///  to be
     float min_mean_loss = 1.0;
@@ -533,8 +516,8 @@ Error_manager Ground_region::detect(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, d
     //        center_z, last_result.theta, last_result.front_theta, last_result.wheel_base, last_result.width,
     //        min_mean_loss);
     // std::cout << "\n-------- final z: " << chassis_z << std::endl;
-    std::cout << "cx: " << last_result.cx << ", cy: " << last_result.cy << ", theta: " << last_result.theta
-              << ", front: " << last_result.front_theta << ", wheelbase: " << last_result.wheel_base << ", width: " << last_result.width << std::endl << std::endl;
+    // std::cout << "cx: " << last_result.cx << ", cy: " << last_result.cy << ", theta: " << last_result.theta
+    //           << ", front: " << last_result.front_theta << ", wheelbase: " << last_result.wheel_base << ", width: " << last_result.width << std::endl << std::endl;
 
     // last_result.cx -= x;
     // last_result.cy -= y;

velodyne_lidar/match3d/detect_wheel_ceres3d.cpp

@@ -343,7 +343,7 @@ bool detect_wheel_ceres3d::detect(std::vector<pcl::PointCloud<pcl::PointXYZ>::Pt
         vec.y = vertice[1].y - vertice[2].y;
     }
     float angle_x = 180.0 / M_PI * acos(vec.x / sqrt(vec.x * vec.x + vec.y * vec.y));
-    // printf("rect theta: %.3f\n",angle_x);
+    printf("rect theta: %.3f\n",angle_x);
     //第二步, 将每份点云旋转回去,计算点云重心所在象限
     for (int i = 0; i < cloud_vec.size(); ++i)
     {
@@ -564,8 +564,8 @@ bool detect_wheel_ceres3d::Solve(Detect_result &detect_result, Loss_result &loss
     double init_theta_front=0*M_PI/180.0;
 
     double variable[] = {cx, cy, init_theta, init_wheel_base, init_width, init_theta_front};
-    /*printf("init solve x:%.3f  y: %.3f  wheel:%.3f  width:%.3f    theta : %.3f   front theta: %.3f\n",
-            variable[0], variable[1], variable[3], variable[4], variable[2], variable[5]);*/
+    printf("init solve x:%.3f  y: %.3f  wheel:%.3f  width:%.3f    theta : %.3f   front theta: %.3f\n",
+            variable[0], variable[1], variable[3], variable[4], variable[2], variable[5]);
 
     // 第二部分：构建寻优问题
     ceres::Problem problem;
@@ -586,11 +586,14 @@ bool detect_wheel_ceres3d::Solve(Detect_result &detect_result, Loss_result &loss
     options.linear_solver_type = ceres::DENSE_QR; //配置增量方程的解法
     //options.logging_type = ceres::LoggingType::SILENT;
     options.max_num_iterations=500;
-    options.num_threads=2;
+    options.num_threads=1;
     options.minimizer_progress_to_stdout = false;//输出到cout
     ceres::Solver::Summary summary;//优化信息
     ceres::Solve(options, &problem, &summary);//求解!!!
 
+    printf("after solve x:%.3f  y: %.3f  wheel:%.3f  width:%.3f    theta : %.3f   front theta: %.3f\n",
+            variable[0], variable[1], variable[3], variable[4], variable[2], variable[5]);
+
     double loss=summary.final_cost/(m_left_front_cloud.size()+m_right_front_cloud.size()+m_left_rear_cloud.size()+m_right_rear_cloud.size());
 
     detect_result.cx=variable[0];
@@ -605,10 +608,14 @@ bool detect_wheel_ceres3d::Solve(Detect_result &detect_result, Loss_result &loss
     if(detect_result.theta<0)
         detect_result.theta+=180.0;
 
-    if(summary.iterations.size()<=1)
+    if(summary.iterations.size()<=1){
+        error_info.append(std::string("仅迭代一轮，优化异常"));
         return false;
-    if(summary.iterations[summary.iterations.size()-1].iteration<=1)
+    }
+    if(summary.iterations[summary.iterations.size()-1].iteration<=1){
+        error_info.append(std::string("最终仅迭代一轮，优化异常"));
         return false;
+    }
     /*printf("it : %d ,summary loss: %.5f \n",
            summary.iterations.size(),loss);*/
 
@@ -646,6 +653,8 @@ bool detect_wheel_ceres3d::Solve(Detect_result &detect_result, Loss_result &loss
         if(loss_result.lf_loss>0.4 || loss_result.rf_loss>0.4
         ||loss_result.lb_loss>0.4||loss_result.rb_loss>0.4)
         {
+            error_info.append("loss过大 lf").append(std::to_string(loss_result.lf_loss)).append("，rf").append(std::to_string(loss_result.rf_loss)).append(", lr").
+            append(std::to_string(loss_result.lb_loss)).append("，rb").append(std::to_string(loss_result.rb_loss));
             return false;
         }