chassis mat optim cx cy only,

fill filtered ground cloud for chassis detect,
add tostring for wheel detect

velodyne_lidar/chassis_ceres_solver.cpp

@@ -109,8 +109,8 @@ public:
     {
         T cx = variable[0];
         T cy = variable[1];
-        T w_ratio = variable[2]/T(inner_width);
-        T h_ratio = variable[3]/T(chassis_height);
+        // T w_ratio = variable[2]/T(inner_width);
+        // T h_ratio = variable[3]/T(chassis_height);
 
         const GridArrayAdapter adapter(m_mat);
         ceres::BiCubicInterpolator<GridArrayAdapter> interpolator(adapter);
@@ -120,14 +120,14 @@ public:
             //先平移后旋转
             const Eigen::Matrix<T, 2, 1> point((T(m_cloud->points[i].x)-cx),
                                                (T(m_cloud->points[i].z)-cy));
-            T kx=(point(0,0)/w_ratio-T(startx)) / T(resolutionx) + T(0.5+kPadding);
-            T ky=(point(1,0)/h_ratio-T(starty)) / T(resolutiony) + T(0.5+kPadding);
+            T kx=(point(0,0)-T(startx)) / T(resolutionx) + T(0.5+kPadding);
+            T ky=(point(1,0)-T(starty)) / T(resolutiony) + T(0.5+kPadding);
             interpolator.Evaluate(kx, ky, &residual[i]);
 
             // residual[i] = fx * fy; //+(50./m_cloud->size())/(w*h+1e-16);
         }
-        residual[m_cloud->size()] = (w_ratio - T(1.0));
-        residual[m_cloud->size()+1] = (h_ratio - T(1.0));
+        // residual[m_cloud->size()] = (w_ratio - T(1.0));
+        // residual[m_cloud->size()+1] = (h_ratio - T(1.0));
         return true;
     }
 
@@ -135,13 +135,13 @@ public:
     void generate_mat(cv::Mat &mat, pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, double *variables)
     {
         mat = m_mat.clone();
-        double w_ratio = variables[2] / inner_width;
-        double h_ratio = variables[3] / chassis_height;
+        // double w_ratio = variables[2] / inner_width;
+        // double h_ratio = variables[3] / chassis_height;
         for (size_t i = 0; i < cloud->size(); i++)
         {
             const Eigen::Vector2d point(
-                ((cloud->points[i].x - variables[0]) * w_ratio - startx) / resolutionx,
-                ((cloud->points[i].z - variables[1]) * h_ratio - starty) / resolutiony);
+                ((cloud->points[i].x - variables[0]) - startx) / resolutionx,
+                ((cloud->points[i].z - variables[1]) - starty) / resolutiony);
             cv::circle(mat, cv::Point2d(point.y(), point.x()), 2, cv::Scalar(0.4), 1);
             // std::cout << point.y() << ", " << point.x() << std::endl;
         }
@@ -190,6 +190,8 @@ Error_manager chassis_ceres_solver::solve(pcl::PointCloud<pcl::PointXYZ>::Ptr cl
 
     x=variable[0];
     y=variable[1];
+    w = variable[2];
+    h = variable[3];
 //     printf(" loss : %f    x:%.5f   y:%.5f  w:%.5f  h:%.5f\n",
 //             loss,variable[0],variable[1],variable[2],variable[3]);
 
@@ -220,8 +222,8 @@ Error_manager chassis_ceres_solver::solve_mat(pcl::PointCloud<pcl::PointXYZ>::Pt
     chassis_mat_cost *cost_func = new chassis_mat_cost(cloud, m_model);
     //使用自动求导，将之前的代价函数结构体传入，第一个1是输出维度，即残差的维度，第二个1是输入维度，即待寻优参数x的维度。
     ceres::CostFunction* cost_function =new
-            ceres::AutoDiffCostFunction<chassis_mat_cost, ceres::DYNAMIC, 4>(
-            cost_func, cloud->size()+2);
+            ceres::AutoDiffCostFunction<chassis_mat_cost, ceres::DYNAMIC, 2>(
+            cost_func, cloud->size());
     problem.AddResidualBlock(cost_function, new ceres::HuberLoss(1.0), variable); //向问题中添加误差项，本问题比较简单，添加一个就行。
 
     //第三部分： 配置并运行求解器
@@ -235,14 +237,14 @@ Error_manager chassis_ceres_solver::solve_mat(pcl::PointCloud<pcl::PointXYZ>::Pt
     ceres::Solver::Summary summary;//优化信息
     ceres::Solve(options, &problem, &summary);//求解!!!
 
-    std::cout << summary.BriefReport() << std::endl;
+    // std::cout << summary.BriefReport() << std::endl;
 
     double loss=summary.final_cost/(cloud->size());
 
     x = variable[0];
     y = variable[1];
-    w = variable[2];
-    h = variable[3];
+    // w = variable[2];
+    // h = variable[3];
     // printf(" loss : %f    x:%.5f   y:%.5f  w:%.5f  h:%.5f\n",
     //         loss,variable[0],variable[1],variable[2],variable[3]);
 
@@ -251,10 +253,11 @@ Error_manager chassis_ceres_solver::solve_mat(pcl::PointCloud<pcl::PointXYZ>::Pt
 
     if(update_debug_img)
     {
+        // printf("midx:%.3f, midz:%.3f\n", x, y);
         cost_func->generate_mat(m_projected_mat, cloud, variable);
         cv::namedWindow("win", cv::WINDOW_FREERATIO);
         cv::imshow("win", m_projected_mat);
-        cv::waitKey(30);
+        cv::waitKey();
     }
 
 

velodyne_lidar/ground_region.cpp

@@ -405,11 +405,12 @@ Error_manager Ground_region::detect(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, d
     double z_solver_theta = car_pose_theta;
     double z_solver_width = 1.0;
     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)
     {
         pcl::PointXYZ pt = cloud->points[i];
-        if (pt.x > m_region.minx() && pt.x < m_region.maxx() && pt.y > m_region.miny() && pt.y < m_region.maxy())
+        if (pt.x > m_region.minx() && pt.x < m_region.maxx() && pt.y > m_region.miny() && pt.y < m_region.maxy() && pt.z <= m_region.minz())
         {
             cloud_z_solver->push_back(pt);
         }
@@ -420,6 +421,8 @@ Error_manager Ground_region::detect(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, d
     sor.setStddevMulThresh(3.0); //标准差倍数
     sor.setNegative(false);      //保留未滤波点（内点）
     sor.filter(*cloud_z_solver); //保存滤波结果到cloud_filter
+    // 补上车身与车轮点
+    cloud_z_solver->operator+=(*mp_cloud_filtered);
     // 去中心，角度调正
     Car_pose_detector::get_instance_references().inv_trans_cloud(cloud_z_solver, z_solver_x, z_solver_y, z_solver_theta);
 
@@ -429,8 +432,8 @@ Error_manager Ground_region::detect(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, d
     // 给予底盘z中心与高度初值
     double mid_z = 0.05, height = 0.08;
     z_solver_x = 0.0;
-    Error_manager ec = t_solver.solve(cloud_z_solver, z_solver_x, mid_z, z_solver_width, height);
-    // Error_manager ec = t_solver.solve_mat(cloud_z_solver, z_solver_x, mid_z, z_solver_width, height, false);
+    // Error_manager ec = t_solver.solve(cloud_z_solver, z_solver_x, mid_z, z_solver_width, height);
+    Error_manager ec = t_solver.solve_mat(cloud_z_solver, z_solver_x, mid_z, z_solver_width, height, false);
     // 切除大于height高度以外点，并显示width直线
     // 根据z值切原始点云
     pcl::PassThrough<pcl::PointXYZ> pass;
@@ -466,11 +469,11 @@ Error_manager Ground_region::detect(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, d
     {
         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))
+    bool ret = false;
+
+    while (chassis_z > (mid_z - height / 2.0))
     {
-        // LOG_IF(WARNING, m_region.region_id() == 4) << "chassis z: " << chassis_z;
+        // LOG_IF(WARNING, m_region.region_id() == 4) << "chassis z: " << chassis_z << ", midz: " << mid_z - height / 2.0;
         ret = classify_ceres_detect(cloud_filtered, chassis_z, result);
         // changed by yct, 根据底盘z识别，失败则向下移动直至成功或超过底盘内空中点
         if(ret)
@@ -485,7 +488,7 @@ Error_manager Ground_region::detect(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, d
     // } while (fabs(center_z - last_center_z) > 0.01);
     std::chrono::steady_clock::time_point t4 = std::chrono::steady_clock::now();
     std::chrono::duration<double> time_used_div = std::chrono::duration_cast<std::chrono::duration<double>>(t4 - t3);
-    std::cout << "\n------------------------------------------------------------ " << std::to_string(time_used_bowl.count()) << " " << std::to_string(time_used_block.count()) << " " << std::to_string(time_used_div.count()) << std::endl;
+    // std::cout << "\n------------------------------------------------------------ " << std::to_string(time_used_bowl.count()) << " " << std::to_string(time_used_block.count()) << " " << std::to_string(time_used_div.count())<<" || "<< std::to_string(time_used_bowl.count()+time_used_block.count()+time_used_div.count())<< std::endl;
 
     if (results.size() == 0)
     {
@@ -542,6 +545,8 @@ Error_manager Ground_region::detect(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, d
         return Error_manager(VELODYNE_REGION_CERES_SOLVE_ERROR, NEGLIGIBLE_ERROR, valid_info);
     }
 
+    // LOG_IF(INFO, m_region.region_id() == 4) << last_result.to_string();
+
     return SUCCESS;
 }
 

velodyne_lidar/match3d/detect_wheel_ceres3d.cpp

@@ -439,6 +439,7 @@ bool detect_wheel_ceres3d::detect(std::vector<pcl::PointCloud<pcl::PointXYZ>::Pt
             solve_result = current_result;
             loss_result = t_loss_result;
     }
+    detect_result.success = solve_result;
 
     return solve_result;
 

velodyne_lidar/match3d/detect_wheel_ceres3d.h

@@ -62,6 +62,12 @@ public:
             this->loss=detect_result.loss;
             return *this;
         }
+        std::string to_string()
+        {
+            char buf[512];
+            sprintf(buf, "cx:%.3f cy:%.3f th:%.3f wb:%.3f wd:%.3f fth:%.3f suc:%s\n", cx, cy, theta, wheel_base, width, front_theta, success ? "true" : "false");
+            return std::string(buf);
+        }
     };
     detect_wheel_ceres3d(pcl::PointCloud<pcl::PointXYZ>::Ptr left_grid_cloud,
                          pcl::PointCloud<pcl::PointXYZ>::Ptr right_grid_cloud);