增加wj雷达ceres模型优化,修改大疆与wj雷达测量角度校验差<2°

CMakeLists.txt

@@ -10,6 +10,7 @@ FIND_PACKAGE(OpenCV REQUIRED)
 FIND_PACKAGE(PCL REQUIRED)
 FIND_PACKAGE(Protobuf REQUIRED)
 FIND_PACKAGE(Glog REQUIRED)
+FIND_PACKAGE(Ceres REQUIRED)
 set(CMAKE_MODULE_PATH "/usr/local/share/")
 set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wl,--no-as-needed")
 set(CMAKE_CXX_FLAGS "-std=c++11 -msse -msse2 -msse3 -msse4 -msse4.1 -msse4.2")
@@ -28,6 +29,7 @@ include_directories(
         /usr/local/include/snap7
   ${PCL_INCLUDE_DIRS}
   ${OpenCV_INCLUDE_DIRS}
+        ${CERES_INCLUDE_DIRS}
 )
 link_directories("/usr/local/lib")
 
@@ -58,7 +60,7 @@ target_link_libraries(fence_debug ${GLOG_LIBRARIES} ${PROTOBUF_LIBRARIES} /usr/l
 
 add_executable(locate  main.cpp ${LOCATE_SRC} ${PLC_SRC} ${TERMINOR_SRC} ${TASK_MANAGER_SRC} ${LASER_SRC} ${ERROR_SRC}
         ${TOOL_SRC} ${SYS_SRC} ${VERIFY_SRC} ${WJLIDAR_SRC})
-target_link_libraries(locate ${OpenCV_LIBS}
+target_link_libraries(locate ${OpenCV_LIBS} ${CERES_LIBRARIES}
         ${GLOG_LIBRARIES} ${PCL_LIBRARIES} ${PROTOBUF_LIBRARIES} ipopt libtensorflow_cc.so
         tf_3dcnn_api.so pointSIFT_API.so dark.so /usr/local/lib/libmodbus.so /usr/local/lib/libglog.a
         /usr/local/lib/libgflags.a /usr/local/lib/liblivox_sdk_static.a /usr/local/apr/lib/libapr-1.a snap7 nnxx nanomsg)

error_code/error_code.h

@@ -199,6 +199,7 @@ enum Error_code
     WJ_REGION_RECTANGLE_SIZE_ERROR,
     WJ_REGION_RECTANGLE_SYMMETRY_ERROR,
     WJ_REGION_CLUSTER_SIZE_ERROR,
+    WJ_REGION_CERES_SOLVE_ERROR,
 
     //wj manager error from 0x06040000-0x0604FFFF
     WJ_MANAGER_UNINITIALIZED=0x06040000,

terminor/terminal_command_executor.cpp

@@ -482,8 +482,8 @@ Error_manager Terminor_Command_Executor::scanning_measuring() {
     float offset_width = fabs(dj_locate_information.locate_width - wj_locate_information.locate_width);
     float offset_wheel_base = fabs(
             dj_locate_information.locate_wheel_base - wj_locate_information.locate_wheel_base);
-    if (offset_x > 100 || offset_y > 100 || offset_angle > 4 || offset_wheel_base > 200 || offset_width > 200) {
-        LOG(WARNING) << "chekc failed. (offset x>100, y>100, angle>4, wheel_base>200, width>200): " << offset_x
+    if (offset_x > 100 || offset_y > 100 || offset_angle > 2 || offset_wheel_base > 200 || offset_width > 200) {
+        LOG(WARNING) << "chekc failed. (offset x>100, y>100, angle>2, wheel_base>200, width>200): " << offset_x
                      << " " << offset_y << " " << offset_angle << " " << offset_wheel_base << " " << offset_width;
         return Error_manager(TERMINOR_CHECK_RESULTS_ERROR, NORMAL, "check results failed ");
     }

wj_lidar/detect_wheel_ceres.cpp

@@ -0,0 +1,352 @@
+//
+// Created by zx on 2020/7/1.
+//
+
+#include "detect_wheel_ceres.h"
+#include <ceres/cubic_interpolation.h>
+#include <pcl/common/transforms.h>
+
+
+constexpr float kMinProbability = 0.0f;
+constexpr float kMaxProbability = 1.f - kMinProbability;
+constexpr float kMaxCorrespondenceCost = 1.f - kMinProbability;
+constexpr int kPadding = INT_MAX / 4;
+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<double >(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_;
+};
+
+class CostFunctor {
+private:
+    cv::Mat  m_map;
+    double m_scale;
+    pcl::PointCloud<pcl::PointXYZ>          m_left_front_cloud;     //左前点云
+    pcl::PointCloud<pcl::PointXYZ>          m_right_front_cloud;    //右前点云
+    pcl::PointCloud<pcl::PointXYZ>          m_left_rear_cloud;      //左后点云
+    pcl::PointCloud<pcl::PointXYZ>          m_right_rear_cloud;     //右后点云
+
+public:
+    CostFunctor(pcl::PointCloud<pcl::PointXYZ> left_front,pcl::PointCloud<pcl::PointXYZ> right_front,
+                pcl::PointCloud<pcl::PointXYZ> left_rear,pcl::PointCloud<pcl::PointXYZ> right_rear,
+                cv::Mat& map,double scale)
+    {
+        m_map=map;
+        m_scale=scale;
+        m_left_front_cloud=left_front;
+        m_right_front_cloud=right_front;
+        m_left_rear_cloud=left_rear;
+        m_right_rear_cloud=right_rear;
+    }
+    template <typename T>
+    bool operator()(const T* const variable, T* residual) const {
+        T cx = variable[0];
+        T cy = variable[1];
+        T theta = variable[2];
+        T length = variable[3];
+        T width = variable[4];
+        T theta_front = variable[5];
+
+        //整车旋转
+        Eigen::Rotation2D<T> rotation(theta);
+        Eigen::Matrix<T, 2, 2> rotation_matrix = rotation.toRotationMatrix();
+
+        //左前偏移
+        Eigen::Matrix<T, 2, 1> wheel_center_normal_left_front(length / 2.0, width / 2.0);
+        //右前偏移
+        Eigen::Matrix<T, 2, 1> wheel_center_normal_right_front(length / 2.0, -width / 2.0);
+        //左后偏移
+        Eigen::Matrix<T, 2, 1> wheel_center_normal_left_rear(-length / 2.0, width / 2.0);
+        //右后偏移
+        Eigen::Matrix<T, 2, 1> wheel_center_normal_right_rear(-length / 2.0, -width / 2.0);
+
+        //前轮旋转
+        Eigen::Rotation2D<T> rotation_front(theta_front);
+        Eigen::Matrix<T, 2, 2> rotation_matrix_front = rotation_front.toRotationMatrix();
+
+
+        const GridArrayAdapter adapter(m_map);
+        ceres::BiCubicInterpolator<GridArrayAdapter> interpolator(adapter);
+
+        double rows = m_map.rows;
+        double cols = m_map.cols;
+        //左前轮
+        int left_front_num = m_left_front_cloud.size();
+        for (int i = 0; i < m_left_front_cloud.size(); ++i) {
+            const Eigen::Matrix<T, 2, 1> point((T(m_left_front_cloud[i].x) - cx),
+                                               (T(m_left_front_cloud[i].y) - cy));
+            //减去经过车辆旋转计算的左前中心
+            const Eigen::Matrix<T, 2, 1> tanslate_point = rotation_matrix * point - wheel_center_normal_left_front;
+            //旋转
+            const Eigen::Matrix<T, 2, 1> point_rotation = rotation_matrix_front * tanslate_point;
+            interpolator.Evaluate(point_rotation(1, 0) * m_scale + rows / 2.0 + 0.5 + T(kPadding),
+                                  point_rotation(0, 0) * m_scale + cols / 2.0 + 0.5 + T(kPadding),
+                                  &residual[i]);
+
+        }
+        //右前轮
+        int right_front_num = m_right_front_cloud.size();
+        for (int i = 0; i < m_right_front_cloud.size(); ++i) {
+            const Eigen::Matrix<T, 2, 1> point((T(m_right_front_cloud[i].x) - cx),
+                                               (T(m_right_front_cloud[i].y) - cy));
+            //减去经过车辆旋转计算的左前中心
+            const Eigen::Matrix<T, 2, 1> tanslate_point = rotation_matrix * point - wheel_center_normal_right_front;
+            //旋转
+            const Eigen::Matrix<T, 2, 1> point_rotation = rotation_matrix_front * tanslate_point;
+            interpolator.Evaluate(point_rotation(1, 0) * m_scale + rows / 2.0 + 0.5 + T(kPadding),
+                                  point_rotation(0, 0) * m_scale + cols / 2.0 + 0.5 + T(kPadding),
+                                  &residual[left_front_num + i]);
+
+        }
+        //左后轮
+        int left_rear_num = m_left_rear_cloud.size();
+
+        for (int i = 0; i < m_left_rear_cloud.size(); ++i) {
+            const Eigen::Matrix<T, 2, 1> point((T(m_left_rear_cloud[i].x) - cx),
+                                               (T(m_left_rear_cloud[i].y) - cy));
+            //减去经过车辆旋转计算的左前中心
+            const Eigen::Matrix<T, 2, 1> tanslate_point = rotation_matrix * point - wheel_center_normal_left_rear;
+
+            interpolator.Evaluate(tanslate_point(1, 0) * m_scale + rows / 2.0 + 0.5 + T(kPadding),
+                                  tanslate_point(0, 0) * m_scale + cols / 2.0 + 0.5 + T(kPadding),
+                                  &residual[left_front_num + right_front_num + i]);
+
+        }
+
+        //右后轮
+
+        for (int i = 0; i < m_right_rear_cloud.size(); ++i) {
+            const Eigen::Matrix<T, 2, 1> point((T(m_right_rear_cloud[i].x) - cx),
+                                               (T(m_right_rear_cloud[i].y) - cy));
+            //减去经过车辆旋转计算的左前中心
+            const Eigen::Matrix<T, 2, 1> tanslate_point = rotation_matrix * point - wheel_center_normal_right_rear;
+
+            interpolator.Evaluate(tanslate_point(1, 0) * m_scale + rows / 2.0 + 0.5 + T(kPadding),
+                                  tanslate_point(0, 0) * m_scale + cols / 2.0 + 0.5 + T(kPadding),
+                                  &residual[left_front_num + right_front_num + left_rear_num + i]);
+
+        }
+
+        return true;
+    }
+
+private:
+
+};
+
+detect_wheel_ceres::detect_wheel_ceres()
+{
+    /////创建地图
+    int cols=800;
+    int rows=200;
+    int L=std::min(rows,cols);
+    cv::Mat map=cv::Mat::ones(L,L,CV_64F);
+    //map=map*255;
+    cv::Point center(L/2,L/2);
+
+
+    float K=L*0.08;
+    for(int n=0;n<L;n+=2)
+    {
+        cv::Size size(n+2,n+2);
+        cv::Rect rect(center-cv::Point(n/2,n/2),size);
+        if(n<K*2)
+            cv::rectangle(map,rect,3.0*float(K*2-n)/float(L));
+        else
+            cv::rectangle(map,rect,float(n-K*2)/float(L));
+
+    }
+    cv::resize(map,map,cv::Size(cols,rows));
+    cv::GaussianBlur(map,m_map,cv::Size(7,7),3,3);
+}
+detect_wheel_ceres::~detect_wheel_ceres(){}
+
+
+
+bool detect_wheel_ceres::detect(std::vector<pcl::PointCloud<pcl::PointXYZ>> cloud_vec,
+                                double& cx,double& cy,double& theta,double& wheel_base,double& width,double& front_theta)
+{
+    //清理点云
+    m_left_front_cloud.clear();
+    m_right_front_cloud.clear();
+    m_left_rear_cloud.clear();
+    m_right_rear_cloud.clear();
+    //重新计算点云,按方位分割
+    //第一步,计算整体中心,主轴方向
+    pcl::PointCloud<pcl::PointXYZ> cloud_all;
+    for(int i=0;i<cloud_vec.size();++i)
+    {
+        cloud_all+=cloud_vec[i];
+    }
+
+    if(cloud_all.size()<20)
+        return false;
+
+    Eigen::Vector4f centroid;
+    pcl::compute3DCentroid(cloud_all, centroid);
+    double center_x=centroid[0];
+    double center_y=centroid[1];
+    //计算外接旋转矩形
+    std::vector<cv::Point2f> points_cv;
+    for(int i=0;i<cloud_all.size();++i)
+    {
+        points_cv.push_back(cv::Point2f(cloud_all[i].x,cloud_all[i].y));
+    }
+    cv::RotatedRect rotate_rect=cv::minAreaRect(points_cv);
+    //计算旋转矩形与X轴的夹角
+    cv::Point2f vec;
+    cv::Point2f vertice[4];
+    rotate_rect.points(vertice);
+
+    float len1 = pow(vertice[0].x - vertice[1].x, 2.0) + pow(vertice[0].y - vertice[1].y, 2.0);
+    float len2 = pow(vertice[1].x - vertice[2].x, 2.0) + pow(vertice[1].y - vertice[2].y, 2.0);
+    // 寻找长边，倾角为长边与x轴夹角
+    if (len1 > len2)
+    {
+        vec.x = vertice[0].x - vertice[1].x;
+        vec.y = vertice[0].y - vertice[1].y;
+    }
+    else
+    {
+        vec.x = vertice[1].x - vertice[2].x;
+        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);
+    //第二步, 将没分点云旋转回去,计算点云重心所在象限
+    for(int i=0;i<cloud_vec.size();++i)
+    {
+        pcl::PointCloud<pcl::PointXYZ> cloud=cloud_vec[i];
+        Eigen::Affine3f traslation = Eigen::Affine3f::Identity();//初始化变换矩阵为单位矩阵
+        // 平移
+        traslation.translation() << -center_x, -center_y, 0.0;
+        pcl::PointCloud<pcl::PointXYZ> translate_cloud;
+        pcl::transformPointCloud(cloud, translate_cloud, traslation);
+
+        // 旋转; Z 轴上旋转angle_x 弧度,Y轴上旋转0弧度
+        Eigen::Affine3f rotation = Eigen::Affine3f::Identity();
+        rotation.rotate(Eigen::AngleAxisf(-angle_x*M_PI/180.0, Eigen::Vector3f::UnitZ()));
+
+        pcl::PointCloud<pcl::PointXYZ> transformed_cloud;
+        pcl::transformPointCloud(translate_cloud, transformed_cloud, rotation);
+
+        //计算重心
+        Eigen::Vector4f centroid;
+        pcl::compute3DCentroid(transformed_cloud, centroid);
+        double x=centroid[0];
+        double y=centroid[1];
+        //计算象限
+        if(x>0&&y>0)
+        {
+            //第一象限
+            m_left_front_cloud=cloud;
+        }
+        if(x>0 && y<0)
+        {
+            //第四象限
+            m_right_front_cloud=cloud;
+        }
+        if(x<0 && y>0)
+        {
+            //第二象限
+            m_left_rear_cloud=cloud;
+        }
+        if(x<0 && y<0)
+        {
+            //第三象限
+            m_right_rear_cloud=cloud;
+
+        }
+
+    }
+    cx=center_x;
+    cy=center_y;
+    theta=-angle_x*M_PI/180.0;
+    return Solve(cx,cy,theta,wheel_base,width,front_theta);
+}
+
+bool detect_wheel_ceres::Solve(double& x,double& y,double& theta,double& wheel_base,double& width,double& front_theta)
+{
+    double SCALE=300.0;
+    double cx=x;
+    double cy=y;
+    double init_theta=theta;
+    double init_wheel_base=2.7;
+    double init_width=1.55;
+    double init_theta_front=0*M_PI/180.0;
+
+    double variable[] = {cx,cy,init_theta,init_wheel_base,init_width,init_theta_front};
+
+    // 第二部分：构建寻优问题
+    ceres::Problem problem;
+    //使用自动求导，将之前的代价函数结构体传入，第一个1是输出维度，即残差的维度，第二个1是输入维度，即待寻优参数x的维度。
+    ceres::CostFunction* cost_function =new
+            ceres::AutoDiffCostFunction<CostFunctor, ceres::DYNAMIC, 6>(
+                    new CostFunctor(m_left_front_cloud,m_right_front_cloud,m_left_rear_cloud,m_right_rear_cloud,m_map,SCALE),
+                    m_left_front_cloud.size()+m_right_front_cloud.size()+m_left_rear_cloud.size()+m_right_rear_cloud.size());
+    problem.AddResidualBlock(cost_function, NULL, variable); //向问题中添加误差项，本问题比较简单，添加一个就行。
+
+
+    //第三部分： 配置并运行求解器
+    ceres::Solver::Options options;
+    options.use_nonmonotonic_steps=false;
+    options.linear_solver_type = ceres::DENSE_QR; //配置增量方程的解法
+    options.max_num_iterations=20;
+    options.num_threads=1;
+    options.minimizer_progress_to_stdout = false;//输出到cout
+    ceres::Solver::Summary summary;//优化信息
+    ceres::Solve(options, &problem, &summary);//求解!!!
+
+    //std::cout << summary.BriefReport() << "\n";//输出优化的简要信息
+
+    /*printf("x:%.3f,y:%.3f,theta:%.3f\nlength:%.3f,width:%.3f\ntheta_front:%.3f\n",
+           x[0],x[1],x[2]*180.0/M_PI,x[3],x[4],x[5]*180.0/M_PI);*/
+
+    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());
+
+
+    x=variable[0];
+    y=variable[1];
+    theta=(-variable[2])*180.0/M_PI;
+    wheel_base=variable[3];
+    width=variable[4];
+    front_theta=-(variable[5]*180.0/M_PI);
+
+    if(theta>180.0)
+        theta=theta-180.0;
+    if(theta<0)
+        theta+=180.0;
+
+    //检验
+    if(loss>0.03)
+        return false;
+    if (width < 1.350 || width > 2.000 || wheel_base > 3.000 || wheel_base < 2.200)
+    {
+        return false;
+    }
+
+    width+=0.15;//车宽+10cm
+    //printf(" x:%.3f  y: %.3f  wheel:%.3f  width:%.3f    theta : %.3f   front theta: %.3f\n",x,y,wheel_base,width,theta,front_theta);
+    return true;
+
+}

wj_lidar/detect_wheel_ceres.h

@@ -0,0 +1,36 @@
+//
+// Created by zx on 2020/7/1.
+//
+
+#ifndef CERES_TEST_DETECT_WHEEL_CERES_H
+#define CERES_TEST_DETECT_WHEEL_CERES_H
+
+#include <ceres/ceres.h>
+#include <glog/logging.h>
+#include <pcl/point_types.h>
+#include <pcl/point_cloud.h>
+#include <pcl/common/centroid.h>
+#include <opencv2/opencv.hpp>
+
+class detect_wheel_ceres {
+public:
+    detect_wheel_ceres();
+    ~detect_wheel_ceres();
+    bool detect(std::vector<pcl::PointCloud<pcl::PointXYZ>> cloud_vec,
+                double& cx,double& cy,double& theta,double& wheel_base,double& width,double& front_theta);
+
+protected:
+    bool Solve(double& x,double& y,double& theta,double& wheel_base,double& width,double& front_theta);
+
+private:
+    pcl::PointCloud<pcl::PointXYZ>          m_left_front_cloud;     //左前点云
+    pcl::PointCloud<pcl::PointXYZ>          m_right_front_cloud;    //右前点云
+    pcl::PointCloud<pcl::PointXYZ>          m_left_rear_cloud;      //左后点云
+    pcl::PointCloud<pcl::PointXYZ>          m_right_rear_cloud;     //右后点云
+
+public:
+    cv::Mat                                 m_map;                  //保存一份地图用作匹配
+};
+
+
+#endif //CERES_TEST_DETECT_WHEEL_CERES_H

wj_lidar/fence_controller.cpp

@@ -314,7 +314,7 @@ Error_manager Fence_controller::execute_task(Task_Base* task)
                 bool correctness = false;
 
                 LOG(INFO) << "--------callback find terminal------" << terminal_id;
-                double x = 0, y = 0, c = 0, wheelbase = 0, width = 0;
+                double x = 0, y = 0, c = 0, wheelbase = 0, width = 0,front_theta=0;
                 // 获取最新点云并保存到total_cloud中
 
                 pcl::PointCloud<pcl::PointXYZ>::Ptr total_cloud(new pcl::PointCloud<pcl::PointXYZ>);
@@ -330,7 +330,7 @@ Error_manager Fence_controller::execute_task(Task_Base* task)
                 LOG(INFO) << "--------callback get cloud, size: " << total_cloud->size();
 
                 // 获取检测结果
-                Error_manager code = m_region_workers_vector[i]->get_wheel_result(total_cloud, x, y, c, wheelbase, width);
+                Error_manager code = m_region_workers_vector[i]->get_wheel_result(total_cloud, x, y, c, wheelbase, width,front_theta);
                 time_t tt = time(0);
                 struct tm *tc = localtime(&tt);
                 char buf[255] = {0};
@@ -535,7 +535,7 @@ void Fence_controller::wheel_msg_handling_thread(Fence_controller *fc)
                         if (terminal_id >= 0 && fc->m_region_workers_vector[i]->get_id() == terminal_id)
                         {
                             LOG(INFO) << "--------callback find terminal------" << terminal_id;
-                            double x = 0, y = 0, c = 0, wheelbase = 0, width = 0;
+                            double x = 0, y = 0, c = 0, wheelbase = 0, width = 0,front_theta=0;
                             // 获取最新点云并保存到total_cloud中
 //                            fc->m_cloud_mutex.lock();
                             pcl::PointCloud<pcl::PointXYZ>::Ptr total_cloud(new pcl::PointCloud<pcl::PointXYZ>);
@@ -555,7 +555,7 @@ void Fence_controller::wheel_msg_handling_thread(Fence_controller *fc)
                             char whole_filename[255];
                             memset(whole_filename, 0, 255);
                             // 获取检测结果
-                            Error_manager code = fc->m_region_workers_vector[i]->get_wheel_result(total_cloud, x, y, c, wheelbase, width);
+                            Error_manager code = fc->m_region_workers_vector[i]->get_wheel_result(total_cloud, x, y, c, wheelbase, width,front_theta);
                             // 根据是否成功生成对应文件名
                             if (code == SUCCESS)
                             {
@@ -585,6 +585,7 @@ void Fence_controller::wheel_msg_handling_thread(Fence_controller *fc)
                             result.set_c(c);
                             result.set_wheel_base(wheelbase * 1000.0);
                             result.set_width(width * 1000.0);
+                            result.set_front_theta(front_theta*180.0/M_PI);
                             if (result.correctness())
                                 break;
                         }

wj_lidar/globalmsg.pb.cc

@@ -241,10 +241,11 @@ const ::google::protobuf::uint32 TableStruct::offsets[] GOOGLE_PROTOBUF_ATTRIBUT
   GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(::globalmsg::resultInfo, width_),
   GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(::globalmsg::resultInfo, height_),
   GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(::globalmsg::resultInfo, error_),
+  GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(::globalmsg::resultInfo, front_theta_),
   0,
   2,
   3,
-  11,
+  12,
   4,
   5,
   6,
@@ -253,6 +254,7 @@ const ::google::protobuf::uint32 TableStruct::offsets[] GOOGLE_PROTOBUF_ATTRIBUT
   9,
   10,
   1,
+  11,
   GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(::globalmsg::algMsg, _has_bits_),
   GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(::globalmsg::algMsg, _internal_metadata_),
   ~0u,  // no _extensions_
@@ -278,9 +280,9 @@ static const ::google::protobuf::internal::MigrationSchema schemas[] GOOGLE_PROT
   { 0, 10, sizeof(::globalmsg::msg)},
   { 15, 24, sizeof(::globalmsg::laserMsg)},
   { 28, 35, sizeof(::globalmsg::plcMsg)},
-  { 37, 54, sizeof(::globalmsg::resultInfo)},
-  { 66, 74, sizeof(::globalmsg::algMsg)},
-  { 77, 84, sizeof(::globalmsg::SysMsg)},
+  { 37, 55, sizeof(::globalmsg::resultInfo)},
+  { 68, 76, sizeof(::globalmsg::algMsg)},
+  { 79, 86, sizeof(::globalmsg::SysMsg)},
 };
 
 static ::google::protobuf::Message const * const file_default_instances[] = {
@@ -324,27 +326,27 @@ void AddDescriptorsImpl() {
       "\020queue_data_count\030\002 \001(\005\022\023\n\013cloud_count\030\003"
       " \001(\005\022\n\n\002id\030\004 \002(\005\"F\n\006plcMsg\022(\n\nplc_status"
       "\030\001 \001(\0162\024.globalmsg.plcStatus\022\022\n\nplc_valu"
-      "es\030\002 \003(\005\"\320\001\n\nresultInfo\022\016\n\004time\030\001 \002(\t:\000\022"
+      "es\030\002 \003(\005\"\345\001\n\nresultInfo\022\016\n\004time\030\001 \002(\t:\000\022"
       "\023\n\013correctness\030\002 \002(\010\022\025\n\rpark_space_id\030\003 "
       "\001(\005\022\021\n\tlaser_ids\030\004 \001(\005\022\t\n\001x\030\005 \001(\001\022\t\n\001y\030\006"
       " \001(\001\022\t\n\001c\030\007 \001(\001\022\022\n\nwheel_base\030\010 \001(\001\022\016\n\006l"
       "ength\030\t \001(\001\022\r\n\005width\030\n \001(\001\022\016\n\006height\030\013 \001"
-      "(\001\022\017\n\005error\030\014 \001(\t:\000\"^\n\006algMsg\022%\n\006result\030"
-      "\001 \003(\0132\025.globalmsg.resultInfo\022\031\n\021thread_q"
-      "ueue_size\030\002 \001(\005\022\022\n\010log_path\030\003 \001(\t:\000\"E\n\006S"
-      "ysMsg\022\r\n\003log\030\001 \002(\t:\000\022,\n\005level\030\002 \001(\0162\023.gl"
-      "obalmsg.logLevel:\010eSysInfo*0\n\004type\022\010\n\004eP"
-      "LC\020\000\022\n\n\006eLaser\020\001\022\010\n\004eAlg\020\002\022\010\n\004eLog\020\003*]\n\013"
-      "laserStatus\022\023\n\017eLaserConnected\020\000\022\026\n\022eLas"
-      "erDisconnected\020\001\022\016\n\neLaserBusy\020\002\022\021\n\reLas"
-      "erUnknown\020\003*V\n\tplcStatus\022\021\n\rePLCConnecte"
-      "d\020\000\022\024\n\020ePLCDisconnected\020\001\022\017\n\013ePLCRefused"
-      "\020\002\022\017\n\013ePLCUnknown\020\003*E\n\010logLevel\022\014\n\010eSysI"
-      "nfo\020\000\022\013\n\007eSysLog\020\001\022\017\n\013eSysWarning\020\002\022\r\n\te"
-      "SysError\020\003"
+      "(\001\022\017\n\005error\030\014 \001(\t:\000\022\023\n\013front_theta\030\r \001(\001"
+      "\"^\n\006algMsg\022%\n\006result\030\001 \003(\0132\025.globalmsg.r"
+      "esultInfo\022\031\n\021thread_queue_size\030\002 \001(\005\022\022\n\010"
+      "log_path\030\003 \001(\t:\000\"E\n\006SysMsg\022\r\n\003log\030\001 \002(\t:"
+      "\000\022,\n\005level\030\002 \001(\0162\023.globalmsg.logLevel:\010e"
+      "SysInfo*0\n\004type\022\010\n\004ePLC\020\000\022\n\n\006eLaser\020\001\022\010\n"
+      "\004eAlg\020\002\022\010\n\004eLog\020\003*]\n\013laserStatus\022\023\n\017eLas"
+      "erConnected\020\000\022\026\n\022eLaserDisconnected\020\001\022\016\n"
+      "\neLaserBusy\020\002\022\021\n\reLaserUnknown\020\003*V\n\tplcS"
+      "tatus\022\021\n\rePLCConnected\020\000\022\024\n\020ePLCDisconne"
+      "cted\020\001\022\017\n\013ePLCRefused\020\002\022\017\n\013ePLCUnknown\020\003"
+      "*E\n\010logLevel\022\014\n\010eSysInfo\020\000\022\013\n\007eSysLog\020\001\022"
+      "\017\n\013eSysWarning\020\002\022\r\n\teSysError\020\003"
   };
   ::google::protobuf::DescriptorPool::InternalAddGeneratedFile(
-      descriptor, 1090);
+      descriptor, 1111);
   ::google::protobuf::MessageFactory::InternalRegisterGeneratedFile(
     "globalmsg.proto", &protobuf_RegisterTypes);
 }
@@ -1575,6 +1577,7 @@ const int resultInfo::kLengthFieldNumber;
 const int resultInfo::kWidthFieldNumber;
 const int resultInfo::kHeightFieldNumber;
 const int resultInfo::kErrorFieldNumber;
+const int resultInfo::kFrontThetaFieldNumber;
 #endif  // !defined(_MSC_VER) || _MSC_VER >= 1900
 
 resultInfo::resultInfo()
@@ -1669,7 +1672,7 @@ void resultInfo::Clear() {
         reinterpret_cast<char*>(&wheel_base_) -
         reinterpret_cast<char*>(&correctness_)) + sizeof(wheel_base_));
   }
-  if (cached_has_bits & 3840u) {
+  if (cached_has_bits & 7936u) {
     ::memset(&length_, 0, static_cast<size_t>(
         reinterpret_cast<char*>(&laser_ids_) -
         reinterpret_cast<char*>(&length_)) + sizeof(laser_ids_));
@@ -1860,6 +1863,20 @@ bool resultInfo::MergePartialFromCodedStream(
         break;
       }
 
+      // optional double front_theta = 13;
+      case 13: {
+        if (static_cast< ::google::protobuf::uint8>(tag) ==
+            static_cast< ::google::protobuf::uint8>(105u /* 105 & 0xFF */)) {
+          set_has_front_theta();
+          DO_((::google::protobuf::internal::WireFormatLite::ReadPrimitive<
+                   double, ::google::protobuf::internal::WireFormatLite::TYPE_DOUBLE>(
+                 input, &front_theta_)));
+        } else {
+          goto handle_unusual;
+        }
+        break;
+      }
+
       default: {
       handle_unusual:
         if (tag == 0) {
@@ -1908,7 +1925,7 @@ void resultInfo::SerializeWithCachedSizes(
   }
 
   // optional int32 laser_ids = 4;
-  if (cached_has_bits & 0x00000800u) {
+  if (cached_has_bits & 0x00001000u) {
     ::google::protobuf::internal::WireFormatLite::WriteInt32(4, this->laser_ids(), output);
   }
 
@@ -1957,6 +1974,11 @@ void resultInfo::SerializeWithCachedSizes(
       12, this->error(), output);
   }
 
+  // optional double front_theta = 13;
+  if (cached_has_bits & 0x00000800u) {
+    ::google::protobuf::internal::WireFormatLite::WriteDouble(13, this->front_theta(), output);
+  }
+
   if (_internal_metadata_.have_unknown_fields()) {
     ::google::protobuf::internal::WireFormat::SerializeUnknownFields(
         _internal_metadata_.unknown_fields(), output);
@@ -1994,7 +2016,7 @@ void resultInfo::SerializeWithCachedSizes(
   }
 
   // optional int32 laser_ids = 4;
-  if (cached_has_bits & 0x00000800u) {
+  if (cached_has_bits & 0x00001000u) {
     target = ::google::protobuf::internal::WireFormatLite::WriteInt32ToArray(4, this->laser_ids(), target);
   }
 
@@ -2044,6 +2066,11 @@ void resultInfo::SerializeWithCachedSizes(
         12, this->error(), target);
   }
 
+  // optional double front_theta = 13;
+  if (cached_has_bits & 0x00000800u) {
+    target = ::google::protobuf::internal::WireFormatLite::WriteDoubleToArray(13, this->front_theta(), target);
+  }
+
   if (_internal_metadata_.have_unknown_fields()) {
     target = ::google::protobuf::internal::WireFormat::SerializeUnknownFieldsToArray(
         _internal_metadata_.unknown_fields(), target);
@@ -2127,7 +2154,7 @@ size_t resultInfo::ByteSizeLong() const {
     }
 
   }
-  if (_has_bits_[8 / 32] & 3840u) {
+  if (_has_bits_[8 / 32] & 7936u) {
     // optional double length = 9;
     if (has_length()) {
       total_size += 1 + 8;
@@ -2143,6 +2170,11 @@ size_t resultInfo::ByteSizeLong() const {
       total_size += 1 + 8;
     }
 
+    // optional double front_theta = 13;
+    if (has_front_theta()) {
+      total_size += 1 + 8;
+    }
+
     // optional int32 laser_ids = 4;
     if (has_laser_ids()) {
       total_size += 1 +
@@ -2210,7 +2242,7 @@ void resultInfo::MergeFrom(const resultInfo& from) {
     }
     _has_bits_[0] |= cached_has_bits;
   }
-  if (cached_has_bits & 3840u) {
+  if (cached_has_bits & 7936u) {
     if (cached_has_bits & 0x00000100u) {
       length_ = from.length_;
     }
@@ -2221,6 +2253,9 @@ void resultInfo::MergeFrom(const resultInfo& from) {
       height_ = from.height_;
     }
     if (cached_has_bits & 0x00000800u) {
+      front_theta_ = from.front_theta_;
+    }
+    if (cached_has_bits & 0x00001000u) {
       laser_ids_ = from.laser_ids_;
     }
     _has_bits_[0] |= cached_has_bits;
@@ -2263,6 +2298,7 @@ void resultInfo::InternalSwap(resultInfo* other) {
   swap(length_, other->length_);
   swap(width_, other->width_);
   swap(height_, other->height_);
+  swap(front_theta_, other->front_theta_);
   swap(laser_ids_, other->laser_ids_);
   swap(_has_bits_[0], other->_has_bits_[0]);
   _internal_metadata_.Swap(&other->_internal_metadata_);

wj_lidar/globalmsg.pb.h

@@ -775,6 +775,13 @@ class resultInfo : public ::google::protobuf::Message /* @@protoc_insertion_poin
   double height() const;
   void set_height(double value);
 
+  // optional double front_theta = 13;
+  bool has_front_theta() const;
+  void clear_front_theta();
+  static const int kFrontThetaFieldNumber = 13;
+  double front_theta() const;
+  void set_front_theta(double value);
+
   // optional int32 laser_ids = 4;
   bool has_laser_ids() const;
   void clear_laser_ids();
@@ -808,6 +815,8 @@ class resultInfo : public ::google::protobuf::Message /* @@protoc_insertion_poin
   void clear_has_height();
   void set_has_error();
   void clear_has_error();
+  void set_has_front_theta();
+  void clear_has_front_theta();
 
   // helper for ByteSizeLong()
   size_t RequiredFieldsByteSizeFallback() const;
@@ -826,6 +835,7 @@ class resultInfo : public ::google::protobuf::Message /* @@protoc_insertion_poin
   double length_;
   double width_;
   double height_;
+  double front_theta_;
   ::google::protobuf::int32 laser_ids_;
   friend struct ::protobuf_globalmsg_2eproto::TableStruct;
   friend void ::protobuf_globalmsg_2eproto::InitDefaultsresultInfoImpl();
@@ -1628,13 +1638,13 @@ inline void resultInfo::set_park_space_id(::google::protobuf::int32 value) {
 
 // optional int32 laser_ids = 4;
 inline bool resultInfo::has_laser_ids() const {
-  return (_has_bits_[0] & 0x00000800u) != 0;
+  return (_has_bits_[0] & 0x00001000u) != 0;
 }
 inline void resultInfo::set_has_laser_ids() {
-  _has_bits_[0] |= 0x00000800u;
+  _has_bits_[0] |= 0x00001000u;
 }
 inline void resultInfo::clear_has_laser_ids() {
-  _has_bits_[0] &= ~0x00000800u;
+  _has_bits_[0] &= ~0x00001000u;
 }
 inline void resultInfo::clear_laser_ids() {
   laser_ids_ = 0;
@@ -1881,6 +1891,30 @@ inline void resultInfo::set_allocated_error(::std::string* error) {
   // @@protoc_insertion_point(field_set_allocated:globalmsg.resultInfo.error)
 }
 
+// optional double front_theta = 13;
+inline bool resultInfo::has_front_theta() const {
+  return (_has_bits_[0] & 0x00000800u) != 0;
+}
+inline void resultInfo::set_has_front_theta() {
+  _has_bits_[0] |= 0x00000800u;
+}
+inline void resultInfo::clear_has_front_theta() {
+  _has_bits_[0] &= ~0x00000800u;
+}
+inline void resultInfo::clear_front_theta() {
+  front_theta_ = 0;
+  clear_has_front_theta();
+}
+inline double resultInfo::front_theta() const {
+  // @@protoc_insertion_point(field_get:globalmsg.resultInfo.front_theta)
+  return front_theta_;
+}
+inline void resultInfo::set_front_theta(double value) {
+  set_has_front_theta();
+  front_theta_ = value;
+  // @@protoc_insertion_point(field_set:globalmsg.resultInfo.front_theta)
+}
+
 // -------------------------------------------------------------------
 
 // algMsg

wj_lidar/globalmsg.proto

@@ -61,6 +61,7 @@ message resultInfo
 	optional double width=10;
 	optional double height=11;
 	optional string error=12 [default=""];
+	optional double front_theta=13;
 }
 message algMsg
 {

wj_lidar/region_detect.cpp

@@ -236,8 +236,62 @@ Error_manager Region_detector::isRect(std::vector<cv::Point2f> &points, bool pri
     }
 }
 
+/*
+ * 仅仅聚类
+ */
+Error_manager Region_detector::clustering_only(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_in,
+        std::vector<pcl::PointCloud<pcl::PointXYZ>> &seg_clouds, bool print)
+{
+    // 1.判断参数合法性
+    if (cloud_in->size() <= 0)
+        return Error_manager(Error_code::WJ_REGION_EMPTY_CLOUD);
 
+    // 2.聚类
+    pcl::search::KdTree<pcl::PointXYZ>::Ptr kdtree(new pcl::search::KdTree<pcl::PointXYZ>);
+    kdtree->setInputCloud(cloud_in);
 
+    pcl::EuclideanClusterExtraction<pcl::PointXYZ> clustering;
+    // 设置聚类的最小值 2cm (small values may cause objects to be divided
+    // in several clusters, whereas big values may join objects in a same cluster).
+    clustering.setClusterTolerance(0.2);
+    // 设置聚类的小点数和最大点云数
+    clustering.setMinClusterSize(10);
+    clustering.setMaxClusterSize(500);
+    clustering.setSearchMethod(kdtree);
+    clustering.setInputCloud(cloud_in);
+    std::vector<pcl::PointIndices> clusters;
+    clustering.extract(clusters);
+    if(clusters.size() <= 0)
+        return Error_manager(Error_code::WJ_REGION_EMPTY_CLOUD);
+
+    for (int i = 0; i < clusters.size(); ++i)
+    {
+        seg_clouds.push_back(pcl::PointCloud<pcl::PointXYZ>());
+    }
+
+    int j = 0;
+    for (std::vector<pcl::PointIndices>::const_iterator it = clusters.begin(); it != clusters.end(); ++it)
+    {
+        pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_cluster(new pcl::PointCloud<pcl::PointXYZ>);
+        //创建新的点云数据集cloud_cluster，将所有当前聚类写入到点云数据集中
+        for (std::vector<int>::const_iterator pit = it->indices.begin(); pit != it->indices.end(); ++pit)
+        {
+            cloud_cluster->points.push_back(cloud_in->points[*pit]);
+            cloud_cluster->width = cloud_cluster->points.size();
+            cloud_cluster->height = 1;
+            cloud_cluster->is_dense = true;
+        }
+        seg_clouds[j] = *cloud_cluster;
+        j++;
+    }
+
+    if (print)
+    {
+        LOG(INFO) << " cluster classes:" << clusters.size();
+    }
+
+    return SUCCESS;
+}
 
 
 /**
@@ -344,6 +398,31 @@ Error_manager Region_detector::detect(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_
 }
 
 
+/**
+ * ceres优化
+ * 优化变量:中心点、角度、轮距与宽度的四轮点云检测函数
+ * */
+Error_manager Region_detector::detect(pcl::PointCloud<pcl::PointXYZ>::Ptr &cloud_in, double &x, double &y, double &c,
+        double &wheelbase, double &width, double& front_wheel_theta,bool print)
+{
+    Error_manager result = Error_manager(Error_code::SUCCESS);
+    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered(pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>));
+    // 1.预处理
+    result = preprocess(cloud_in, cloud_filtered);
+    if (result != SUCCESS)
+        return result;
+
+    // 2.聚类
+    std::vector<pcl::PointCloud<pcl::PointXYZ>> seg_clouds;
+    result = clustering_only(cloud_filtered, seg_clouds, print);
+    if (result != SUCCESS)
+        return result;
+
+    if(!m_detector_ceres.detect(seg_clouds,x,y,c,wheelbase,width,front_wheel_theta))
+        return WJ_REGION_CERES_SOLVE_ERROR;
+
+    return Error_manager(Error_code::SUCCESS);
+}
 
 
 /**

wj_lidar/region_detect.h

@@ -41,7 +41,7 @@ using google::protobuf::Message;
 
 #include "../tool/StdCondition.h"
 #include "opencv2/opencv.hpp"
-
+#include "detect_wheel_ceres.h"
 /**
  * 万集区域检测算法类
  * */
@@ -56,6 +56,9 @@ public:
     Error_manager detect(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_in);
     // 检测传入点云，识别为四类返回中心点、角度、轮距与宽度
     Error_manager detect(pcl::PointCloud<pcl::PointXYZ>::Ptr &cloud_in, double &x, double &y, double &c, double &wheelbase, double &width, bool print=true);
+    //通过ceres检测中心,旋转,轴距,宽度,前轮旋转
+    Error_manager detect(pcl::PointCloud<pcl::PointXYZ>::Ptr &cloud_in, double &x, double &y, double &c,double& front_wheel_theta,
+            double &wheelbase, double &width, bool print=true);
     // 获得区域id
     int get_region_id();
 
@@ -63,13 +66,20 @@ private:
     // 预处理，直通滤波限制点云范围
     Error_manager preprocess(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_in, pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_out);
     // 点云聚类，寻找四类点云并检验是否近似矩形
-    Error_manager clustering(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_in, std::vector<cv::Point2f>& corner_points, std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> &seg_clouds, bool print=false);
+    Error_manager clustering(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_in, std::vector<cv::Point2f>& corner_points,
+            std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> &seg_clouds, bool print=false);
+    //仅仅聚类
+    Error_manager clustering_only(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_in,
+                             std::vector<pcl::PointCloud<pcl::PointXYZ>> &seg_clouds, bool print=false);
     // 判断是否足够近似矩形
     Error_manager isRect(std::vector<cv::Point2f>& points, bool print=false);
 
 private:
     wj::Region m_region_param;      // 区域范围参数
     std::atomic<int> m_region_id;   // 区域id
+
+    detect_wheel_ceres              m_detector_ceres;   //ceres优化对象
+
 };
 
 #endif //REGION_DETECT_H

wj_lidar/region_worker.cpp

@@ -80,6 +80,28 @@ Error_manager Region_worker::get_wheel_result(pcl::PointCloud<pcl::PointXYZ>::Pt
     return result;
 }
 
+
+/**
+ * ceres结果
+ * 获取轮距等测量结果,包括前轮旋转
+ * */
+Error_manager Region_worker::get_wheel_result(pcl::PointCloud<pcl::PointXYZ>::Ptr &cloud_in, double &x, double &y, double &c,
+        double &wheelbase, double &width,double& front_theta)
+{
+    Error_manager result;
+    std::lock_guard<std::mutex> lck(m_mutex);
+    if (m_detector)
+    {
+        LOG(INFO) << "worker getting result";
+        result = m_detector->detect(cloud_in, x, y, c, wheelbase, width,front_theta);
+    }
+    else
+    {
+        result = Error_manager(Error_code::POINTER_IS_NULL);
+    }
+    return result;
+}
+
 /**
  * 外部调用更新区域检测用点云
  * */
@@ -122,8 +144,8 @@ void Region_worker::detect_loop(Region_worker *worker)
             int code = REGION_WORKER_RESULT_DEFAULT;
             int border_status = REGION_WORKER_RESULT_DEFAULT;
             worker->mb_cloud_updated = false;
-            double x,y,angle,wheelbase,width;
-            Error_manager result = worker->m_detector->detect(worker->m_cloud, x, y, angle, wheelbase, width, false);
+            double x,y,angle,wheelbase,width,front_theta;
+            Error_manager result = worker->m_detector->detect(worker->m_cloud, x, y, angle, wheelbase, width,front_theta, false);
 
             if(worker->mp_verify_handle == nullptr)
             {
@@ -178,7 +200,7 @@ void Region_worker::detect_loop(Region_worker *worker)
             if (p!=0 && duration > 500 && ((worker->m_last_sent_code != worker->m_last_read_code) || worker->m_last_border_status != border_status) && worker->m_read_code_count > 3)
             {
                 worker->m_last_sent_code = worker->m_last_read_code;
-		worker->m_last_border_status = border_status;
+		        worker->m_last_border_status = border_status;
                 p->update_data(code, border_status, worker->m_detector->get_region_id());
                 worker->m_update_plc_time = std::chrono::steady_clock::now();
             }

wj_lidar/region_worker.h

@@ -43,6 +43,11 @@ public:
     int get_id();
     // 获得中心点、角度等测量数据
     Error_manager get_wheel_result(pcl::PointCloud<pcl::PointXYZ>::Ptr &cloud_in, double &x, double &y, double &c, double &wheelbase, double &width);
+
+    // 获得中心点、角度等测量数据,前轮旋转
+    Error_manager get_wheel_result(pcl::PointCloud<pcl::PointXYZ>::Ptr &cloud_in, double &x, double &y, double &c,
+            double &wheelbase, double &width,double& front_theta);
+
 private:
     static cv::RotatedRect create_rotate_rect(float length,float width,float angle,float x,float y);
 private: