自动驾驶障碍物点云转占据栅格和多边形及3D包围盒重建

    基于障碍物点云投影后的占据栅格，生成占据图像、栅格高程图像，基于占据图像生成障碍物轮廓外接多边形，最小外接矩；基于高程图像和占据图像的轮廓外接矩形，重建点云3D包围盒，算法已封装成独立的函数，ros下使用C++实现，使用了PCL、OPENCV、ROS：

void points2polygon( pcl::PointCloud<pcl::PointXYZI>::Ptr cloud, std::vector<std::vector<cv::Point2f> >  &polygons, std::vector<std::vector<cv::Point2f> >  &rects, jsk_recognition_msgs::BoundingBoxArray &obs_array,std::vector<signed char> &ocGrid )
{

    int point_number = 10;

    float cellResolution = 0.2;

    float xMin = 0.2;

    float yMin = -7;

    float xMax = 40;

    float yMax = 7;

    float xCells = (xMax-xMin)/cellResolution + 1;

    float yCells = (yMax-yMin)/cellResolution + 1;

    int size = xCells * yCells;//栅格总数
    std::vector<int> countGrid(size);  //栅格中点云数量
    std::vector<float> count_height_max(size, -10);//栅格中点云z最大值
    std::vector<float> count_height_min(size, 10);//栅格中点云z最小值

     //将每个点云分配到各个网格，计算每个栅格内Z值最值
    for (size_t i = 0; i < cloud->points.size(); i++)
    {
       int xc = (int) ((cloud->points[i].x - xMin) / cellResolution); //取整后默认按照cellResolution将点分配到ｃｅｌｌ
       int yc = (int) ((cloud->points[i].y - yMin) / cellResolution);
       if(xc<0||xc>=xCells||yc<0||yc>=yCells) continue;
       countGrid[yc * xCells + xc]++; //统计一个cell中垂直方向满足条件的点数

       if(cloud->points[i].z > count_height_max[yc * xCells + xc])
           count_height_max[yc * xCells + xc] = cloud->points[i].z;
       if(cloud->points[i].z < count_height_min[yc * xCells + xc])
           count_height_min[yc * xCells + xc] = cloud->points[i].z;
    }

    ocGrid.resize(size,0); //占据栅格
    //std::unordered_set<int> obs_indice; //占据序号

    cv::Mat image = cv::Mat::zeros(xCells, yCells, CV_8UC1);
    cv::Mat img_zmax(xCells, yCells, CV_32FC1, cv::Scalar(-10.0f));
    cv::Mat img_zmin(xCells, yCells, CV_32FC1, cv::Scalar(10.0f));

    for (int i = 0; i < size; i++)  //size:xCells * yCells
    {
      if (countGrid[i] >= point_number)
      {
         ocGrid[i] = 100;
        // obs_indice.insert(i);

         int y=xCells-1- i%xCells;
         int x=yCells-1- i/xCells;
         if(x<0||x>yCells-1||y<0||y>xCells-1) continue;
         image.at<uchar>(y,x)= 255;

         img_zmax.at<float>(y,x) = count_height_max[i];
         img_zmin.at<float>(y,x) = count_height_min[i];
      }
    }

    //形态学滤波
    // 定义结构元素（核）
    cv::Mat kernel = cv::getStructuringElement(cv::MORPH_RECT, cv::Size(3, 3));

    // 进行闭运算
    cv::Mat closedImage;
    cv::morphologyEx(image, closedImage, cv::MORPH_CLOSE, kernel);

    //膨胀
    cv::Mat exp_closedImage;
    cv::dilate(closedImage,exp_closedImage,kernel);  //解决单个栅格不能生成最小外接矩

    //轮廓提取
    std::vector<std::vector<cv::Point>> contours;
    cv::findContours(exp_closedImage, contours, cv::RETR_EXTERNAL, cv::CHAIN_APPROX_SIMPLE);

    if(contours.size() > 0)
    {
         //轮廓简化
         std::vector<std::vector<cv::Point>> approxContours;
         approxContours.reserve(contours.size());
         //每个轮廓内z最大最小值
         std::vector<double> rect_z_max(contours.size());
         std::vector<double> rect_z_min(contours.size());
         std::vector<cv::RotatedRect> minRect(contours.size());
         for (int n = 0; n < contours.size(); n++)
         {
             // 最小外接矩形
              minRect[n] = cv::minAreaRect(contours[n]);

            //计算轮廓内最小最大值z
            // 创建一个与图像大小相同的黑色掩模
            cv::Mat mask = cv::Mat::zeros(img_zmin.size(),CV_8UC1);

            // 在掩模上绘制填充的轮廓（白色）
            std::vector<std::vector<cv::Point>> contours_m = {contours[n]};
            cv::drawContours(mask, contours_m, -1, cv::Scalar(255), cv::FILLED);

            // 计算最小值和最大值（忽略0值，即掩模外的区域）
            double minVal,maxVal;
            cv::minMaxLoc(img_zmin, &minVal, nullptr,nullptr, nullptr, mask);
            rect_z_min[n] = minVal;
            cv::minMaxLoc(img_zmax, nullptr, &maxVal, nullptr, nullptr, mask);
            rect_z_max[n] = maxVal;
            //轮廓简化
            double epsilon = 0.01 * cv::arcLength(contours[n], true);  // 1% 周长
            std::vector<cv::Point> approx;
            cv::approxPolyDP(contours[n], approx, epsilon, true);
            approxContours.push_back(approx);
         }

         polygons.reserve(approxContours.size());
         for (size_t j = 0; j < approxContours.size(); j++)
         {          
             //坐标转换
             std::vector<cv::Point2f> global_pos;
             global_pos.reserve(approxContours[j].size());
             for(int num = 0; num < approxContours[j].size(); num++)
             {
                //MAT转车辆坐标
                cv::Point  p_mat = approxContours[j][num];
                int p_grid_y = yCells-1 - p_mat.x;
                int p_grid_x = xCells-1 - p_mat.y;
                float pos_y = (p_grid_y * cellResolution) + yMin + cellResolution/2;
                float pos_x = (p_grid_x * cellResolution) + xMin + cellResolution/2;
                //float pos_z =0;
                cv::Point2f pos_car(pos_x,pos_y );
                //转全局坐标系
                global_pos.push_back(pos_car);
             }

             polygons.push_back(global_pos);
         }

         //最小外接矩形坐标系转换
         rects.reserve(minRect.size());
         obs_array.boxes.reserve(minRect.size());

         for(int i = 0; i < minRect.size(); i++)
         {
            cv::RotatedRect rect = minRect[i];

            cv::Point2f vertices[4];
            rect.points(vertices);
            std::vector<cv::Point2f> rect_car;  //车辆坐标系4个顶点
            for(int num = 0; num < 4; num++)
            {

                int p_grid_y = yCells-1 - vertices[num].x;
                int p_grid_x = xCells-1 - vertices[num].y;
                float pos_y = (p_grid_y * cellResolution) + yMin + cellResolution/2;
                float pos_x = (p_grid_x * cellResolution) + xMin + cellResolution/2;
                cv::Point2f pos_rect(pos_x, pos_y );
                rect_car.push_back(pos_rect);
            }
            rects.push_back(rect_car);

            //计算包围盒高度最小最大值
            double z_min = rect_z_min[i];
            double z_max = rect_z_max[i];
            //计算包围盒中心点
            float center_x = (rect_car[0].x + rect_car[1].x + rect_car[2].x + rect_car[3].x) / 4.0;
            float center_y = (rect_car[0].y + rect_car[1].y + rect_car[2].y + rect_car[3].y) / 4.0;
            double center_z = (z_min + z_max) / 2.0;
            //计算朝向
            // 使用第一边向量计算旋转角度
            double len_x = rect_car[1].x - rect_car[0].x;
            double len_y = rect_car[1].y - rect_car[0].y;
            float yaw = atan2(len_y, len_x);  // 计算绕z轴的旋转角度
            tf2::Quaternion q;
            q.setRPY(0, 0, yaw);

            //计算尺寸
            // 将顶点转换到局部坐标系
            std::vector<cv::Point2f> local_vertices(4);
            tf2::Quaternion inv_q = q.inverse();

            for (int j = 0; j < 4; ++j)
            {
                tf2::Vector3 global_vec(rect_car[j].x - center_x,rect_car[j].y - center_y,0);
                tf2::Vector3 local_vec = tf2::quatRotate(inv_q, global_vec);
                local_vertices[j].x = local_vec.x();
                local_vertices[j].y = local_vec.y();
            }

            // 在局部坐标系中计算长宽
            float x_min = std::min({local_vertices[0].x, local_vertices[1].x,
                                   local_vertices[2].x, local_vertices[3].x});
            float x_max = std::max({local_vertices[0].x, local_vertices[1].x,
                                   local_vertices[2].x, local_vertices[3].x});
            float y_min = std::min({local_vertices[0].y, local_vertices[1].y,
                                   local_vertices[2].y, local_vertices[3].y});
            float y_max = std::max({local_vertices[0].y, local_vertices[1].y,
                                   local_vertices[2].y, local_vertices[3].y});
            float length = x_max - x_min;  // 局部x方向尺寸
            float width = y_max - y_min;   // 局部y方向尺寸
            float height = z_max - z_min;    // z方向尺寸

            float expand = 0.0;
            jsk_recognition_msgs::BoundingBox obs_bounding;
            obs_bounding.header.stamp = ros::Time::now();
            obs_bounding.header.frame_id = "truck_1/base_link";
            obs_bounding.pose.position.x = center_x;
            obs_bounding.pose.position.y = center_y;
            obs_bounding.pose.position.z = center_z;
            obs_bounding.pose.orientation.x = q.x();
            obs_bounding.pose.orientation.y = q.y();
            obs_bounding.pose.orientation.z = q.z();
            obs_bounding.pose.orientation.w = q.w();
            obs_bounding.dimensions.x = length + expand;
            obs_bounding.dimensions.y = width + expand;
            obs_bounding.dimensions.z = height;
            obs_bounding.label = 0;
            obs_bounding.value = 0.0;
            obs_array.boxes.push_back(obs_bounding);
         }
    }
}