add pose mat optim, debug z solver mat.

alter voxelgrid order to improve perf.

tests/vlp16_driver_test.cpp

@@ -2,7 +2,7 @@
  * @Description: vlp16驱动测试
  * @Author: yct
  * @Date: 2021-07-26 14:11:26
- * @LastEditTime: 2021-11-12 12:54:33
+ * @LastEditTime: 2021-11-17 15:35:02
  * @LastEditors: yct
  */
 
@@ -12,6 +12,30 @@
 // velodyne driver seperation test
 #include "../velodyne_lidar/velodyne_driver/input.h"
 #include "../velodyne_lidar/velodyne_driver/rawdata.h"
+
+GOOGLE_GLOG_DLL_DECL void shut_down_logging(const char* data, int size)
+{
+	time_t tt;
+	time( &tt );
+	tt = tt + 8*3600;  // transform the time zone
+	tm* t= gmtime( &tt );
+	char buf[255]={0};
+	sprintf(buf,"./%d%02d%02d-%02d%02d%02d-dump.txt",
+			t->tm_year + 1900,
+			t->tm_mon + 1,
+			t->tm_mday,
+			t->tm_hour,
+			t->tm_min,
+			t->tm_sec);
+
+	FILE* tp_file=fopen(buf,"w");
+	fprintf(tp_file,data,strlen(data));
+	fclose(tp_file);
+
+}
+
+
+
 bool velo_driver_test()
 {
     velodyne_driver::InputSocket sock;
@@ -117,7 +141,7 @@ void device_test()
 
 void region_detect()
 {
-    std::string exce = "../tests/region_4_26x.txt", envi = "../tests/region_4_env.txt";
+    std::string exce = "../tests/region_4_26x_g.txt", envi = "../tests/region_4_env_g.txt";
     pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_exce(new pcl::PointCloud<pcl::PointXYZ>);
     pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_envi(new pcl::PointCloud<pcl::PointXYZ>);
     bool res = read_pointcloud(exce, cloud_exce);
@@ -157,11 +181,11 @@ void region_detect()
             detect_wheel_ceres3d::Detect_result t_result;
             Error_manager ec = t_region.detect(cloud_exce, t_result);
             std::cout << ec.to_string() << std::endl;
-            usleep(1000 * 100);
+            usleep(1);
 
             ec = t_region.detect(cloud_envi, t_result);
             std::cout << ec.to_string() << std::endl;
-            usleep(1000 * 100);
+            usleep(1);
         }
 
         // 检测自动识别功能
@@ -304,6 +328,19 @@ void message_test()
 
 int main()
 {
+    	const char* logPath = "./log/";
+	google::InitGoogleLogging("LidarMeasurement");
+	google::SetStderrLogging(google::INFO);
+	google::SetLogDestination(0, logPath);
+	google::SetLogFilenameExtension("zxlog");
+	google::InstallFailureSignalHandler();
+	google::InstallFailureWriter(&shut_down_logging);
+	FLAGS_colorlogtostderr = true;        // Set log color
+	FLAGS_logbufsecs = 0;                // Set log output speed(s)
+	FLAGS_max_log_size = 1024;            // Set max log file size(GB)
+	FLAGS_stop_logging_if_full_disk = true;
+
+
     // velo_driver_test();
     // device_test();
     region_detect();

velodyne_lidar/car_pose_detector.cpp

@@ -2,7 +2,7 @@
  * @Description: 
  * @Author: yct
  * @Date: 2021-09-16 15:18:34
- * @LastEditTime: 2021-10-12 17:38:39
+ * @LastEditTime: 2021-11-17 15:37:25
  * @LastEditors: yct
  */
 
@@ -50,6 +50,309 @@ void Car_pose_detector::create_curve_cloud(pcl::PointCloud<pcl::PointXYZ>::Ptr &
     }
 }
 
+// 代价函数的计算模型
+struct Car_pose_cost
+{
+    Car_pose_cost(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_ptr) : m_cloud_ptr(cloud_ptr) {}
+
+    // 残差的计算
+    template <typename T>
+    bool operator()(
+        const T *const vars, // 模型参数，x y theta w
+        T *residual) const
+    {
+        if (m_cloud_ptr == nullptr || m_cloud_ptr->size() <= 0)
+        {
+            std::cout << "error occured" << std::endl;
+            return false;
+        }
+        const T norm_scale = T(0.002);
+        //residual[0] = T(0);
+        // residual[1] = T(0);
+        // residual[m_cloud_ptr->size()] = T(0.0);
+        // 点云loss
+        const T width = T(2.0); //vars[3];
+        
+        char buf[40960]={0};
+        // sprintf(buf, "----");
+        for (int i = 0; i < m_cloud_ptr->size(); i++)
+        {
+            Eigen::Matrix<T, 2, 1> t_point(T(m_cloud_ptr->points[i].x) - vars[0], T(m_cloud_ptr->points[i].y) - vars[1]);
+            Eigen::Rotation2D<T> rotation(vars[2]);
+            Eigen::Matrix<T, 2, 2> rotation_matrix = rotation.toRotationMatrix();
+            Eigen::Matrix<T, 2, 1> t_trans_point = rotation_matrix * t_point;
+
+            T left_loss = T(1.0) / (T(1.0) + ceres::exp(T(30.0) * (t_trans_point.x() + width / T(2.0))));
+            T right_loss = T(1.0) / (T(1.0) + ceres::exp(T(30.0) * (-t_trans_point.x() + width / T(2.0))));
+            T front_loss = T(1.0) / (T(1.0) + ceres::exp(T(15.0) * (t_trans_point.y() + T(2.2))));
+            T back_loss = T(1.0) / (T(1.0) + ceres::exp(T(15.0) * (-t_trans_point.y() + T(2.2))));
+            residual[i] = left_loss + right_loss + front_loss + back_loss; // + norm_scale * ((left_loss - T(0.5)) + (right_loss - T(0.5)));
+            // residual[m_cloud_ptr->size()] += (left_loss - T(0.5)) + (right_loss - T(0.5));
+            // if(left_loss > T(0.01))
+            //   std::cout << "index l r: " << i << ", " << left_loss << ", " << right_loss << ", " << trans_point.x() << std::endl;
+            // sprintf(buf + strlen(buf), "%.4f ", residual[i]);
+            // if(i%20==8)
+            // {
+            //     sprintf(buf + strlen(buf), "\n");
+            // }
+        }
+        // printf(buf);
+
+        // // 参数L2正则化loss
+        // residual[m_cloud_ptr->size()] = T(m_cloud_ptr->size()) * width * norm_scale;
+        // residual[1] += ceres::pow(vars[1],2) * norm_scale;
+        // residual[1] += ceres::pow(vars[2],2) * norm_scale;
+        // residual[1] += ceres::pow((vars[3]-T(1.8)),2) * norm_scale;
+        // ((i != 3) ? norm_scale : norm_scale * T(m_cloud_ptr->size()));
+        return true;
+    }
+
+    pcl::PointCloud<pcl::PointXYZ>::Ptr m_cloud_ptr; // x,y数据
+};
+
+// 公式转图片优化
+#include <ceres/cubic_interpolation.h>
+
+constexpr float kMinProbability = 0.01f;
+constexpr float kMaxProbability = 1.f - kMinProbability;
+constexpr float kMaxCorrespondenceCost = 1.f - kMinProbability;
+constexpr int kPadding = INT_MAX / 4;
+constexpr float resolutionx = 0.01f;
+constexpr float resolutiony = 0.01f;
+constexpr float min_x = -2.0f;
+constexpr float min_y = -3.0f;
+constexpr float max_x = 2.0f;
+constexpr float max_y = 3.0f;
+
+cv::Mat Car_pose_detector::m_model = Car_pose_detector::create_mat(min_x, max_x, min_y, max_y, resolutionx, resolutiony);
+
+class GridArrayAdapter
+{
+public:
+    enum { DATA_DIMENSION = 1 };
+
+    explicit GridArrayAdapter(const cv::Mat& grid) : grid_(grid) {}
+
+    void GetValue(const int row, const int column, double* const value) const {
+        if (row < kPadding || column < kPadding || row >= NumRows() - kPadding ||
+            column >= NumCols() - kPadding) {
+            *value = kMaxCorrespondenceCost;
+        } else {
+            *value = static_cast<double>(grid_.at<float>(row - kPadding, column - kPadding));
+            // if (row - kPadding > 100 && row - kPadding < 300 && column - kPadding > 50 && column - kPadding < 150)
+            //     printf("row:%d, col:%d, val%.3f\n", row-kPadding, column-kPadding, grid_.at<float>(row - kPadding, column - kPadding));
+        }
+    }
+
+    int NumRows() const {
+        return grid_.rows + 2 * kPadding;
+    }
+
+    int NumCols() const {
+        return grid_.cols + 2 * kPadding;
+    }
+private:
+    const cv::Mat& grid_;
+};
+
+// 代价函数的计算模型
+struct Trans_mat_cost
+{
+    Trans_mat_cost(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_ptr, cv::Mat &mat) : m_cloud_ptr(cloud_ptr), m_mat(mat) {
+        // cv::namedWindow("img", CV_WINDOW_FREERATIO);
+        // cv::imshow("img", m_mat);
+        // cv::waitKey();
+        // debug_img(cloud_ptr, mat);
+    }
+
+    void debug_img(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_ptr, cv::Mat &mat)
+    {
+        if (m_cloud_ptr == nullptr || m_cloud_ptr->size() <= 0)
+        {
+            std::cout << "error occured" << std::endl;
+            return;
+        }
+        cv::Mat img_show = mat.clone();//cv::Mat::zeros(mat.rows * 1, mat.cols * 1, CV_32FC1);
+        const GridArrayAdapter adapter(mat);
+        ceres::BiCubicInterpolator<GridArrayAdapter> interpolator(adapter);
+        
+        // 点云loss
+        // char buf[150960]={0};
+        // sprintf(buf, "----");
+        for (int i = 0; i < m_cloud_ptr->size(); i++)
+        {
+            Eigen::Matrix<double, 2, 1> t_point(double(m_cloud_ptr->points[i].x), double(m_cloud_ptr->points[i].y));
+            Eigen::Rotation2D<double> rotation(0);
+            Eigen::Matrix<double, 2, 2> rotation_matrix = rotation.toRotationMatrix();
+            Eigen::Matrix<double, 2, 1> t_trans_point = rotation_matrix * t_point;
+
+            int col_index = int((t_trans_point.x() - min_x) / resolutionx);
+            int row_index = int((t_trans_point.y() - min_y) / resolutiony);
+            double val=0.0;
+            interpolator.Evaluate(row_index +0.5+ kPadding, col_index +0.5+ kPadding, &val);
+            cv::circle(img_show, cv::Point(col_index, row_index), 1, cv::Scalar(0.25), 1);
+            // sprintf(buf + strlen(buf), "(%.2f,%.2f)---%.3f ", t_trans_point.x(), t_trans_point.y(), val);
+            // if(i%15==8)
+            // {
+            //     sprintf(buf + strlen(buf), "\n");
+            // }
+        }
+        // printf(buf);
+        // for (size_t i = -mat.rows; i < mat.rows*2; i++)
+        // {
+        //     for (size_t j = -mat.cols; j < mat.cols*2; j++)
+        //     {
+        //         double val;
+        //         // adapter.GetValue(i + kPadding, j + kPadding, &val);
+        //         interpolator.Evaluate(i+kPadding, j+kPadding, &val);
+        //         img_show.at<float>(i, j) = val;
+        //     }
+        // }
+        // printf("r:%d c:%d\n", img_show.rows, img_show.cols);
+        cv::namedWindow("img", CV_WINDOW_FREERATIO);
+        // cv::imshow("origin", mat);
+        cv::imshow("img", img_show);
+        cv::waitKey();
+    }
+
+    // 残差的计算
+    template <typename T>
+    bool operator()(
+        const T *const vars, // 模型参数，x y theta w
+        T *residual) const
+    {
+        if (m_cloud_ptr == nullptr || m_cloud_ptr->size() <= 0)
+        {
+            std::cout << "error occured" << std::endl;
+            return false;
+        }
+        const GridArrayAdapter adapter(m_mat);
+        ceres::BiCubicInterpolator<GridArrayAdapter> interpolator(adapter);
+
+        // 点云loss
+        Eigen::Rotation2D<T> rotation(vars[2]);
+        Eigen::Matrix<T, 2, 2> rotation_matrix = rotation.toRotationMatrix();
+        for (int i = 0; i < m_cloud_ptr->size(); i++)
+        {
+            Eigen::Matrix<T, 2, 1> t_point(T(m_cloud_ptr->points[i].x) - vars[0], T(m_cloud_ptr->points[i].y) - vars[1]);
+            Eigen::Matrix<T, 2, 1> t_trans_point = rotation_matrix * t_point;
+
+            T col_index = (t_trans_point.x() - T(min_x)) / T(resolutionx) + T(0.5+kPadding);
+            T row_index = (t_trans_point.y() - T(min_y)) / T(resolutiony) + T(0.5+kPadding);
+            interpolator.Evaluate(row_index, col_index, &residual[i]);
+        }
+
+        // // 参数L2正则化loss
+        // residual[m_cloud_ptr->size()] = T(m_cloud_ptr->size()) * width * norm_scale;
+        // residual[1] += ceres::pow(vars[1],2) * norm_scale;
+        // residual[1] += ceres::pow(vars[2],2) * norm_scale;
+        // residual[1] += ceres::pow((vars[3]-T(1.8)),2) * norm_scale;
+        // ((i != 3) ? norm_scale : norm_scale * T(m_cloud_ptr->size()));
+        return true;
+    }
+
+    pcl::PointCloud<pcl::PointXYZ>::Ptr m_cloud_ptr; // x,y数据
+    cv::Mat m_mat;
+};
+
+// 生成优化图像，降低优化耗时
+cv::Mat Car_pose_detector::create_mat(double min_x, double max_x, double min_y, double max_y, double resolution_x, double resolution_y)
+{
+    if (max_x < min_x || max_y < min_y || resolution_x <= 0 || resolution_y <= 0)
+    {
+        return cv::Mat();
+    }
+    int cols = (max_x - min_x) / resolution_x + 1;
+    int rows = (max_y - min_y) / resolution_y + 1;
+    if(rows <=1 || cols <=1)
+    {
+        return cv::Mat();
+    }
+    cv::Mat t_mat(rows, cols, CV_32FC1);
+
+    for (size_t i = 0; i < rows; i++)
+    {
+        for (size_t j = 0; j < cols; j++)
+        {
+            double x = j * resolution_x + min_x;
+            double y = i * resolution_y + min_y;
+            double left_value = 1.0 / (1.0 + exp(30 * (x + 1.0)));
+            double right_value = 1.0 / (1.0 + exp(30 * (-x + 1.0)));
+            double front_value = 1.0 / (1.0 + exp(15 * (y + 2.2)));
+            double back_value = 1.0 / (1.0 + exp(15 * (-y + 2.2)));
+            t_mat.at<float>(i, j) = float((left_value + right_value + front_value + back_value) / 1.0f);
+        }
+    }
+    // std::cout << "r,c " << t_mat.rows << ", " << t_mat.cols << std::endl;
+    // cv::imshow("img", t_mat);
+    // cv::waitKey();
+
+    return t_mat;
+}
+
+// 检测底盘z方向值，去中心，使用mat加速
+bool Car_pose_detector::detect_pose_mat(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_ptr, pcl::PointCloud<pcl::PointXYZ>::Ptr out_cloud_ptr, double &x, double &y, double &theta, bool debug_cloud)
+{
+    pcl::PointCloud<pcl::PointXYZ>::Ptr t_cloud = pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>);
+    t_cloud->operator+=(*cloud_ptr);
+    double t_vars[4] = {0, 0, 0, 1.9};
+    // 去中心化
+    pcl::PointXYZ t_center(0, 0, 0);
+    for (size_t i = 0; i < t_cloud->size(); i++)
+    {
+        t_center.x += t_cloud->points[i].x;
+        t_center.y += t_cloud->points[i].y;
+    }
+    t_center.x /= t_cloud->size();
+    t_center.y /= t_cloud->size();
+    for (size_t i = 0; i < t_cloud->size(); i++)
+    {
+        t_cloud->points[i].x -= t_center.x;
+        t_cloud->points[i].y -= t_center.y;
+    }
+    // // write_pointcloud(t_cloud, std::to_string(count)+"_ori_");
+
+    // 构建最小二乘问题
+    ceres::Problem problem;
+    Trans_mat_cost *tp_trans_mat_cost = new Trans_mat_cost(t_cloud, m_model);
+    // tp_trans_mat_cost->debug_img(t_cloud, m_model);
+    problem.AddResidualBlock( // 向问题中添加误差项
+        // 使用自动求导，模板参数：误差类型，输出维度，输入维度，维数要与前面struct中一致
+        new ceres::AutoDiffCostFunction<Trans_mat_cost, ceres::DYNAMIC, 3>(
+            tp_trans_mat_cost, t_cloud->size()),
+        new ceres::HuberLoss(1.0), // 核函数，这里不使用，为空
+        t_vars   // 待估计参数
+    );
+
+    // 配置求解器
+    ceres::Solver::Options options;                            // 这里有很多配置项可以填
+    options.linear_solver_type = ceres::DENSE_QR; // 增量方程如何求解
+    options.minimizer_progress_to_stdout = false;              // 输出到cout
+    options.max_num_iterations = 100;
+
+    ceres::Solver::Summary summary; // 优化信息
+    std::chrono::steady_clock::time_point t1 = std::chrono::steady_clock::now();
+    ceres::Solve(options, &problem, &summary); // 开始优化
+    std::chrono::steady_clock::time_point t2 = std::chrono::steady_clock::now();
+    std::chrono::duration<double> time_used = std::chrono::duration_cast<std::chrono::duration<double>>(t2 - t1);
+    // std::cout << "solve time cost = " << time_used.count() << " seconds. " << std::endl;
+
+    // 输出结果
+    // std::cout << summary.BriefReport() << std::endl;
+    // t_vars[3] -= 0.4;
+    // 保存结果，将去除的中心补上
+    x = t_vars[0] + t_center.x;
+    y = t_vars[1] + t_center.y;
+    theta = t_vars[2];
+    // printf("x:%.3f y:%.3f th:%.3f\n", x, y, theta);
+    if (fabs(theta) > 10 * M_PI / 180.0)
+    {
+        //std::cout << "wrong angle, detect failed" << theta*180.0/M_PI << std::endl;
+        return false;
+    }
+    return true;
+}
+
 // 检测底盘z方向值，原始点云去中心，并切除底盘z以上部分
 bool Car_pose_detector::detect_pose(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_ptr, pcl::PointCloud<pcl::PointXYZ>::Ptr out_cloud_ptr, double &x, double &y, double &theta, double &width, double &z_value, bool debug_cloud)
 {
@@ -84,7 +387,7 @@ bool Car_pose_detector::detect_pose(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_pt
         // 使用自动求导，模板参数：误差类型，输出维度，输入维度，维数要与前面struct中一致
         new ceres::AutoDiffCostFunction<Car_pose_cost, ceres::DYNAMIC, 3>(
             new Car_pose_cost(t_cloud), t_cloud->size()),
-        nullptr, // 核函数，这里不使用，为空
+        new ceres::HuberLoss(1.0), // 核函数，这里不使用，为空
         t_vars   // 待估计参数
     );
 
@@ -99,15 +402,17 @@ bool Car_pose_detector::detect_pose(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_pt
     ceres::Solve(options, &problem, &summary); // 开始优化
     std::chrono::steady_clock::time_point t2 = std::chrono::steady_clock::now();
     std::chrono::duration<double> time_used = std::chrono::duration_cast<std::chrono::duration<double>>(t2 - t1);
-    // std::cout << "solve time cost = " << time_used.count() << " seconds. " << std::endl;
+    std::cout << "solve time cost = " << time_used.count() << " seconds. " << std::endl;
 
     // 输出结果
     // std::cout << summary.BriefReport() << std::endl;
     // t_vars[3] -= 0.4;
-    x = t_vars[0];
-    y = t_vars[1];
+    // 保存结果，将去除的中心补上
+    x = t_vars[0] + t_center.x;
+    y = t_vars[1] + t_center.y;
     theta = t_vars[2];
-    if(fabs(theta)>30*M_PI/180.0)
+    // printf("x:%.3f y:%.3f th:%.3f\n", x, y, theta);
+    if (fabs(theta) > 10 * M_PI / 180.0)
     {
         //std::cout << "wrong angle, detect failed" << theta*180.0/M_PI << std::endl;
         return false;
@@ -181,8 +486,6 @@ bool Car_pose_detector::detect_pose(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_pt
             out_cloud_ptr->operator+=(*t_cloud);
         }
     }
-    x += t_center.x;
-    y += t_center.y;
     // std::cout << "estimated x,y,theta = " << x << ", " << y << ", " << theta*180.0/M_PI << std::endl;
     // std::cout << "----------------------------------" << std::endl;
     return true;

velodyne_lidar/car_pose_detector.h

@@ -2,7 +2,7 @@
  * @Description: 底盘z高度检测类,准确识别出平移,旋转,车宽,底盘高度，供3D车轮算法优化出车辆准确信息
  * @Author: yct
  * @Date: 2021-09-16 15:02:49
- * @LastEditTime: 2021-09-22 15:01:45
+ * @LastEditTime: 2021-11-16 15:45:02
  * @LastEditors: yct
  */
 
@@ -18,111 +18,9 @@
 #include <pcl/filters/passthrough.h>
 #include <pcl/filters/statistical_outlier_removal.h>
 
-#include "../tool/singleton.h"
-
-// 代价函数的计算模型
-struct Car_pose_cost
-{
-    Car_pose_cost(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_ptr) : m_cloud_ptr(cloud_ptr) {}
-
-    // 残差的计算
-    template <typename T>
-    bool operator()(
-        const T *const vars, // 模型参数，x y theta w
-        T *residual) const
-    {
-        if (m_cloud_ptr == nullptr || m_cloud_ptr->size() <= 0)
-        {
-            std::cout << "error occured" << std::endl;
-            return false;
-        }
-        const T norm_scale = T(0.002);
-        //residual[0] = T(0);
-        // residual[1] = T(0);
-        // residual[m_cloud_ptr->size()] = T(0.0);
-        // 点云loss
-        const T width = T(2.0); //vars[3];
-        for (int i = 0; i < m_cloud_ptr->size(); i++)
-        {
-            Eigen::Matrix<T, 2, 1> t_point(T(m_cloud_ptr->points[i].x) - vars[0], T(m_cloud_ptr->points[i].y) - vars[1]);
-            Eigen::Rotation2D<T> rotation(vars[2]);
-            Eigen::Matrix<T, 2, 2> rotation_matrix = rotation.toRotationMatrix();
-            Eigen::Matrix<T, 2, 1> t_trans_point = rotation_matrix * t_point;
-
-            T left_loss = T(1.0) / (T(1.0) + ceres::exp(T(30.0) * (t_trans_point.x() + width / T(2.0))));
-            T right_loss = T(1.0) / (T(1.0) + ceres::exp(T(30.0) * (-t_trans_point.x() + width / T(2.0))));
-            T front_loss = T(1.0) / (T(1.0) + ceres::exp(T(15.0) * (t_trans_point.y() + T(2.2))));
-            T back_loss = T(1.0) / (T(1.0) + ceres::exp(T(15.0) * (-t_trans_point.y() + T(2.2))));
-            residual[i] = left_loss + right_loss + front_loss + back_loss; // + norm_scale * ((left_loss - T(0.5)) + (right_loss - T(0.5)));
-            // residual[m_cloud_ptr->size()] += (left_loss - T(0.5)) + (right_loss - T(0.5));
-            // if(left_loss > T(0.01))
-            //   std::cout << "index l r: " << i << ", " << left_loss << ", " << right_loss << ", " << trans_point.x() << std::endl;
-        }
-
-        // // 参数L2正则化loss
-        // residual[m_cloud_ptr->size()] = T(m_cloud_ptr->size()) * width * norm_scale;
-        // residual[1] += ceres::pow(vars[1],2) * norm_scale;
-        // residual[1] += ceres::pow(vars[2],2) * norm_scale;
-        // residual[1] += ceres::pow((vars[3]-T(1.8)),2) * norm_scale;
-        // ((i != 3) ? norm_scale : norm_scale * T(m_cloud_ptr->size()));
-        return true;
-    }
-
-    pcl::PointCloud<pcl::PointXYZ>::Ptr m_cloud_ptr; // x,y数据
-};
-
 #include <opencv2/opencv.hpp>
-// 代价函数的计算模型
-struct Trans_mat_cost
-{
-    Trans_mat_cost(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_ptr, cv::Mat mat) : m_cloud_ptr(cloud_ptr), m_mat(mat) {}
-
-    // 残差的计算
-    template <typename T>
-    bool operator()(
-        const T *const vars, // 模型参数，x y theta w
-        T *residual) const
-    {
-        if (m_cloud_ptr == nullptr || m_cloud_ptr->size() <= 0)
-        {
-            std::cout << "error occured" << std::endl;
-            return false;
-        }
-        const T norm_scale = T(0.002);
-        //residual[0] = T(0);
-        // residual[1] = T(0);
-        // residual[m_cloud_ptr->size()] = T(0.0);
-        // 点云loss
-        const T width = T(2.0); //vars[3];
-        for (int i = 0; i < m_cloud_ptr->size(); i++)
-        {
-            Eigen::Matrix<T, 2, 1> t_point(T(m_cloud_ptr->points[i].x) - vars[0], T(m_cloud_ptr->points[i].y) - vars[1]);
-            Eigen::Rotation2D<T> rotation(vars[2]);
-            Eigen::Matrix<T, 2, 2> rotation_matrix = rotation.toRotationMatrix();
-            Eigen::Matrix<T, 2, 1> t_trans_point = rotation_matrix * t_point;
-
-            T left_loss = T(1.0) / (T(1.0) + ceres::exp(T(30.0) * (t_trans_point.x() + width / T(2.0))));
-            T right_loss = T(1.0) / (T(1.0) + ceres::exp(T(30.0) * (-t_trans_point.x() + width / T(2.0))));
-            T front_loss = T(1.0) / (T(1.0) + ceres::exp(T(15.0) * (t_trans_point.y() + T(2.4))));
-            T back_loss = T(1.0) / (T(1.0) + ceres::exp(T(15.0) * (-t_trans_point.y() + T(2.4))));
-            residual[i] = left_loss + right_loss + front_loss + back_loss; // + norm_scale * ((left_loss - T(0.5)) + (right_loss - T(0.5)));
-            // residual[m_cloud_ptr->size()] += (left_loss - T(0.5)) + (right_loss - T(0.5));
-            // if(left_loss > T(0.01))
-            //   std::cout << "index l r: " << i << ", " << left_loss << ", " << right_loss << ", " << trans_point.x() << std::endl;
-        }
 
-        // // 参数L2正则化loss
-        // residual[m_cloud_ptr->size()] = T(m_cloud_ptr->size()) * width * norm_scale;
-        // residual[1] += ceres::pow(vars[1],2) * norm_scale;
-        // residual[1] += ceres::pow(vars[2],2) * norm_scale;
-        // residual[1] += ceres::pow((vars[3]-T(1.8)),2) * norm_scale;
-        // ((i != 3) ? norm_scale : norm_scale * T(m_cloud_ptr->size()));
-        return true;
-    }
-
-    pcl::PointCloud<pcl::PointXYZ>::Ptr m_cloud_ptr; // x,y数据
-    cv::Mat m_mat;
-};
+#include "../tool/singleton.h"
 
 class Car_pose_detector : public Singleton<Car_pose_detector>
 {
@@ -144,8 +42,16 @@ public:
     // 检测底盘z方向值，去中心，
     bool detect_pose(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_ptr, pcl::PointCloud<pcl::PointXYZ>::Ptr out_cloud_ptr, double &x, double &y, double &theta, double &width, double &z_value, bool debug_cloud=false);
 
+    // 检测底盘z方向值，去中心，使用mat加速
+    bool detect_pose_mat(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_ptr, pcl::PointCloud<pcl::PointXYZ>::Ptr out_cloud_ptr, double &x, double &y, double &theta, bool debug_cloud=false);
+
+    // 生成优化图像，降低优化耗时
+    static cv::Mat create_mat(double min_x, double max_x, double min_y, double max_y, double resolution_x, double resolution_y);
+
 private:
     // 父类的构造函数必须保护，子类的构造函数必须私有。
     Car_pose_detector() = default;
+
+    static cv::Mat m_model;
 };
 #endif // !Z_DETECTOR_HH

velodyne_lidar/chassis_ceres_solver.cpp

@@ -52,6 +52,19 @@ class chassis_ceres_cost
 // 公式转图片优化
 #include <ceres/cubic_interpolation.h>
 
+constexpr float kMinProbability = 0.01f;
+constexpr float kMaxProbability = 1.f - kMinProbability;
+constexpr float kMaxCorrespondenceCost = 1.f - kMinProbability;
+constexpr int kPadding = INT_MAX / 4;
+constexpr float resolutionx = 0.01;
+constexpr float resolutiony = 0.005;
+constexpr float inner_width = 1.2;
+constexpr float chassis_height = 0.14;
+constexpr float startx = -2.0;
+constexpr float starty = -0.1;
+constexpr float endx = 2.0;
+constexpr float endy = 0.3;
+
 class GridArrayAdapter
 {
 public:
@@ -64,7 +77,7 @@ public:
             column >= NumCols() - kPadding) {
             *value = kMaxCorrespondenceCost;
         } else {
-            *value = static_cast<double>(grid_.at<double >(row - kPadding, column - kPadding));
+            *value = static_cast<double>(grid_.at<float>(row - kPadding, column - kPadding));
         }
     }
 
@@ -107,8 +120,8 @@ 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 ky=(point(1,0)*h_ratio-T(starty)) / T(resolutiony);
+            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);
             interpolator.Evaluate(kx, ky, &residual[i]);
 
             // residual[i] = fx * fy; //+(50./m_cloud->size())/(w*h+1e-16);
@@ -158,7 +171,7 @@ Error_manager chassis_ceres_solver::solve(pcl::PointCloud<pcl::PointXYZ>::Ptr cl
     ceres::CostFunction* cost_function =new
             ceres::AutoDiffCostFunction<chassis_ceres_cost, ceres::DYNAMIC, 4>(
             cost_func,cloud->size()+1);
-    problem.AddResidualBlock(cost_function, NULL, variable); //向问题中添加误差项，本问题比较简单，添加一个就行。
+    problem.AddResidualBlock(cost_function, new ceres::HuberLoss(1.0), variable); //向问题中添加误差项，本问题比较简单，添加一个就行。
 
 
 
@@ -209,7 +222,7 @@ Error_manager chassis_ceres_solver::solve_mat(pcl::PointCloud<pcl::PointXYZ>::Pt
     ceres::CostFunction* cost_function =new
             ceres::AutoDiffCostFunction<chassis_mat_cost, ceres::DYNAMIC, 4>(
             cost_func, cloud->size()+2);
-    problem.AddResidualBlock(cost_function, NULL, variable); //向问题中添加误差项，本问题比较简单，添加一个就行。
+    problem.AddResidualBlock(cost_function, new ceres::HuberLoss(1.0), variable); //向问题中添加误差项，本问题比较简单，添加一个就行。
 
     //第三部分： 配置并运行求解器
     ceres::Solver::Options options;
@@ -222,6 +235,8 @@ 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;
+
     double loss=summary.final_cost/(cloud->size());
 
     x = variable[0];
@@ -283,3 +298,21 @@ cv::Mat chassis_ceres_solver::create_mat()
 
     return t_mat;
 }
+
+
+// 返回投影地图转换的点云, 便于投影到pcl视野中
+pcl::PointCloud<pcl::PointXYZ>::Ptr chassis_ceres_solver::get_projected_cloud()
+{
+    pcl::PointCloud<pcl::PointXYZ>::Ptr t_cloud = pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>);
+    if (m_projected_mat.empty())
+        return t_cloud;
+    for (size_t i = 0; i < m_projected_mat.rows; i++)
+    {
+        for (size_t j = 0; j < m_projected_mat.cols; j++)
+        {
+            double x0 = (i * resolutionx + startx);
+            t_cloud->push_back(pcl::PointXYZ());
+        }
+    }
+    return t_cloud;
+}

velodyne_lidar/chassis_ceres_solver.h

@@ -16,19 +16,6 @@
 
 #include "../error_code/error_code.h"
 
-constexpr float kMinProbability = 0.01f;
-constexpr float kMaxProbability = 1.f - kMinProbability;
-constexpr float kMaxCorrespondenceCost = 1.f - kMinProbability;
-constexpr int kPadding = INT_MAX / 4;
-constexpr float resolutionx = 0.01;
-constexpr float resolutiony = 0.005;
-constexpr float inner_width = 1.2;
-constexpr float chassis_height = 0.14;
-constexpr float startx = -2.0;
-constexpr float starty = -0.1;
-constexpr float endx = 2.0;
-constexpr float endy = 0.3;
-
 class chassis_ceres_solver
 {
  public:
@@ -45,21 +32,7 @@ class chassis_ceres_solver
     // 返回更新投影地图
     cv::Mat get_projected_mat() { return m_projected_mat; }
     // 返回投影地图转换的点云, 便于投影到pcl视野中
-    pcl::PointCloud<pcl::PointXYZ>::Ptr get_projected_cloud()
-    {
-       pcl::PointCloud<pcl::PointXYZ>::Ptr t_cloud = pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>);
-       if (m_projected_mat.empty())
-          return t_cloud;
-       for (size_t i = 0; i < m_projected_mat.rows; i++)
-       {
-          for (size_t j = 0; j < m_projected_mat.cols; j++)
-          {
-             double x0 = (i * resolutionx + startx);
-             t_cloud->push_back(pcl::PointXYZ());
-          }
-       }
-       return t_cloud;
-    }
+    pcl::PointCloud<pcl::PointXYZ>::Ptr get_projected_cloud();
 
  private:
     static cv::Mat m_model;

velodyne_lidar/ground_region.cpp

@@ -359,25 +359,25 @@ Error_manager Ground_region::detect(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, d
         return Error_manager(VELODYNE_REGION_EMPTY_CLOUD, NEGLIGIBLE_ERROR, "cut no point");
     }
 
+    //下采样
+    pcl::VoxelGrid<pcl::PointXYZ> vox;   //创建滤波对象
+    vox.setInputCloud(cloud_cut);   //设置需要过滤的点云给滤波对象
+    vox.setLeafSize(0.02f, 0.02f, 0.02f); //设置滤波时创建的体素体积为1cm的立方体
+    vox.filter(*cloud_clean);         //执行滤波处理，存储输出
+    // cloud_filtered = cloud_clean;
+    if (cloud_clean->size() == 0)
+        return Error_manager(VELODYNE_REGION_EMPTY_CLOUD, NEGLIGIBLE_ERROR, "ApproximateVoxelGrid no point");
+
     //离群点过滤
     pcl::StatisticalOutlierRemoval<pcl::PointXYZ> sor;
-    sor.setInputCloud(cloud_cut);
-    sor.setMeanK(15);            //K近邻搜索点个数
+    sor.setInputCloud(cloud_clean);
+    sor.setMeanK(5);            //K近邻搜索点个数
     sor.setStddevMulThresh(3.0); //标准差倍数
     sor.setNegative(false);      //保留未滤波点（内点）
-    sor.filter(*cloud_clean); //保存滤波结果到cloud_filter
+    sor.filter(*cloud_filtered); //保存滤波结果到cloud_filter
 
-    if (cloud_clean->size() == 0)
-        return Error_manager(VELODYNE_REGION_EMPTY_CLOUD, NEGLIGIBLE_ERROR, "StatisticalOutlierRemoval no point");
-
-    //下采样
-    pcl::VoxelGrid<pcl::PointXYZ> vox;   //创建滤波对象
-    vox.setInputCloud(cloud_clean);   //设置需要过滤的点云给滤波对象
-    vox.setLeafSize(0.02f, 0.02f, 0.02f); //设置滤波时创建的体素体积为1cm的立方体
-    vox.filter(*cloud_filtered);         //执行滤波处理，存储输出
-    // cloud_filtered = cloud_clean;
     if (cloud_filtered->size() == 0)
-        return Error_manager(VELODYNE_REGION_EMPTY_CLOUD, NEGLIGIBLE_ERROR, "ApproximateVoxelGrid no point");
+        return Error_manager(VELODYNE_REGION_EMPTY_CLOUD, NEGLIGIBLE_ERROR, "StatisticalOutlierRemoval no point");
 
     // 更新过滤点
     m_filtered_cloud_mutex.lock();
@@ -388,7 +388,8 @@ Error_manager Ground_region::detect(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, d
     // 3.*********位姿优化，获取中心xy与角度*********
     pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_detect_z(new pcl::PointCloud<pcl::PointXYZ>);
     double car_pose_x, car_pose_y, car_pose_theta, car_pose_width, z_value=0.2;
-    if(!Car_pose_detector::get_instance_references().detect_pose(cloud_filtered, cloud_detect_z, car_pose_x, car_pose_y, car_pose_theta, car_pose_width, z_value, false))
+    // if(!Car_pose_detector::get_instance_references().detect_pose(cloud_filtered, cloud_detect_z, car_pose_x, car_pose_y, car_pose_theta, car_pose_width, z_value, false))
+    if(!Car_pose_detector::get_instance_references().detect_pose_mat(cloud_filtered, cloud_detect_z, car_pose_x, car_pose_y, car_pose_theta, false))
     {
         return Error_manager(VELODYNE_REGION_CERES_SOLVE_ERROR, NEGLIGIBLE_ERROR, "find general car pose value failed.");
     }
@@ -415,7 +416,7 @@ Error_manager Ground_region::detect(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, d
     }
     //离群点过滤
     sor.setInputCloud(cloud_z_solver);
-    sor.setMeanK(15);            //K近邻搜索点个数
+    sor.setMeanK(5);            //K近邻搜索点个数
     sor.setStddevMulThresh(3.0); //标准差倍数
     sor.setNegative(false);      //保留未滤波点（内点）
     sor.filter(*cloud_z_solver); //保存滤波结果到cloud_filter
@@ -428,8 +429,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;
@@ -444,7 +445,7 @@ Error_manager Ground_region::detect(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, d
     //     cloud_z_solver->push_back(pcl::PointXYZ(-width/2.0, i, 0));
     //     cloud_z_solver->push_back(pcl::PointXYZ(width/2.0, i, 0));
     // }
-    // std::cout << "\n------------------------------------ chassis z1: " << mid_z + height / 2.0 << std::endl;
+    // std::cout << "\n------------------------------------ chassis z1: " << mid_z + height / 2.0 << ", mid z: "<< mid_z << std::endl;
 
     if(ec != SUCCESS)
         return Error_manager(VELODYNE_REGION_CERES_SOLVE_ERROR, NORMAL, "failed to find chassis z value.");
@@ -484,7 +485,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::endl;
 
     if (results.size() == 0)
     {