无人驾驶，感知模块

最新推荐文章于 2024-09-26 16:34:28 发布

IG_CB

最新推荐文章于 2024-09-26 16:34:28 发布

阅读量519

点赞数 5

分类专栏：经验分享日常总结文章标签：机器学习

本文链接：https://blog.csdn.net/2301_76437010/article/details/140335255

版权

经验分享同时被 2 个专栏收录

3 篇文章

订阅专栏

日常总结

1 篇文章

订阅专栏

今天在看一个无人驾驶的ROS项目

感知主要操作：分类，分割，检测，跟踪

从项目代码中了解到了许多无人驾驶方面的算法逻辑思想（这是我目前看到的，如有遗漏或错误还望指正）

1.坐标转换

无人驾驶的汽车一般通过雷达、摄像头或两者同时运用来识别障碍物。这里就以雷达为例，无人车上拥有各种各样的传感器，每个传感器的安装位置和角度又不尽相同。对于传感器的提供商，开始并不知道传感器会以什么角度，安装在什么位置，因此只能根据传感器自身建立坐标系。传感器自身坐标系并不是全车的车身坐标系，无人驾驶系统是一个多传感器整合的系统，需要将不同位置的传感器数据统一到一个固定的坐标系——自车坐标系下。

所以要通过坐标转换统一到人为规定的坐标系中

通过平移，旋转操作将雷达坐标转换成车自身坐标。

平移：

平移步骤根据传感器安装位置和自车后轴的距离进行计算，仅仅是XYZ三个方向加减运算。

旋转：

绕轴旋转需要引入角度，不是简单的加减运算，两个坐标存在角度，也得需要带上角度

这只是大致的思想具体的转换思想运用到了欧拉转换，详细见下面网址

无人驾驶技术入门（十二）| 无人驾驶中的坐标转换 - 知乎 (zhihu.com)

2.体素滤波

下面是我看项目的代码：

体素滤波的思想就是让点云数据点变少且扫描的图形不能改变

// 降采样 体素滤波      让点云变少且图形不变
void voxelFilter(const pcl::PointCloud<PointType>::Ptr input,
                 pcl::PointCloud<PointType>::Ptr &output)
{
  auto startTimePassThrough = std::chrono::steady_clock::now();
  pcl::VoxelGrid<PointType> sor;
  sor.setInputCloud(input);
  sor.setLeafSize(0.05f, 0.05f, 0.05f);                                  //设置体素大小                         // 剪枝？
  sor.filter(*output);                                                                   //执行滤波
  auto endTimePassThrough = std::chrono::steady_clock::now();
  auto elapsedTimePassThrough = std::chrono::duration_cast<std::chrono::microseconds>(endTimePassThrough - startTimePassThrough);                   //结束时间记录与耗时计算
  // std::cout << "VoxelGridFiltering took " << elapsedTimePassThrough.count() << " milliseconds" << std::endl;
  return;
}

3.地面分割

地面分割采用RANSAC算法

具体的代码如下：

#include <utility.h>
#include <set>

bool point_cmp(PointType a, PointType b) // PointType表明XYZ点云库的一个点坐标
{
    return a.z < b.z;
}

// 地面分割采用RANSAC算法
void lidar_cluster::RANSAC(const pcl::PointCloud<PointType>::Ptr &cloud_filtered,
                           pcl::PointCloud<PointType>::Ptr &cloud_filtered_out, int maxIterations, float thre)
{

    pcl::SACSegmentation<PointType> seg; 
    pcl::PointIndices::Ptr inliers(new pcl::PointIndices);                         
    pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients);          
    pcl::PointCloud<PointType>::Ptr cloud_plane(new pcl::PointCloud<PointType>()); 

    seg.setOptimizeCoefficients(true);
    seg.setModelType(pcl::SACMODEL_PLANE); 
    seg.setMethodType(pcl::SAC_RANSAC);    /
    seg.setMaxIterations(maxIterations);
    seg.setDistanceThreshold(thre);

    // 从剩余的点云中分割最大平面组成部分
    seg.setInputCloud(cloud_filtered);    
    seg.segment(*inliers, *coefficients); 

    // Extract the planar inliers from the input cloud  从输入云中提取平面内层
    pcl::ExtractIndices<PointType> extract;
    extract.setInputCloud(cloud_filtered); 
    extract.setIndices(inliers);          
    extract.setNegative(false);          
    extract.filter(*cloud_plane); // 设置完之后执行filter才能实现
    std::cout << "PointCloud representing the planar component: " << cloud_plane->points.size() << " data points." << std::endl;

    extract.setNegative(true); 
    extract.filter(*cloud_filtered_out);

}
void lidar_cluster::ground_segmentation(const pcl::PointCloud<PointType>::Ptr &in_pc,
                                        pcl::PointCloud<PointType>::Ptr &g_not_ground_pc)
{

    g_not_ground_pc->points.clear();
    pcl::PointCloud<PointType> laserCloudIn, laserCloudIn_org;
    laserCloudIn = laserCloudIn_org = *in_pc; 
    PointType point;
    g_all_pc = laserCloudIn.makeShared(); 
    sort(laserCloudIn.points.begin(), laserCloudIn.end(), point_cmp); 
    
    pcl::PointCloud<PointType>::iterator it = laserCloudIn.points.begin();
    for (int i = 0; i < laserCloudIn.points.size(); i++)
    {
        if (laserCloudIn.points[i].z < -1 * sensor_height_) // 0.25
        {
            it++;
        }
        else
        {
            break;
        }
    }
    // remove below sensor_height_* - 1.5 points   拆下传感器下方的重量_*-1.5分
    laserCloudIn.points.erase(laserCloudIn.points.begin(), it);
    // 4. Extract init ground seeds. 提取初始地面种子
    extract_initial_seeds_(laserCloudIn); // 这个可能压根都没用上  数据都被清空了  不对  estimate_plane_用上了
    g_ground_pc = g_seeds_pc;             // 获取的地面种子存放到地面里面
    for (int i = 0; i < num_iter_; i++)
    {
        estimate_plane_();
        g_ground_pc->clear();
        g_not_ground_pc->clear();

        MatrixXf points(laserCloudIn_org.points.size(), 3);
        int j = 0;
        for (auto p : laserCloudIn_org.points)
        {
            points.row(j++) << p.x, p.y, p.z;
        }
        
        VectorXf result = points * normal_; // 平面的法线
        for (int r = 0; r < result.rows(); r++)
        {
            if (result[r] < th_dist_d_) 
           {
                g_ground_pc->points.push_back(laserCloudIn_org[r]);
            }
            else
            {
                g_not_ground_pc->points.push_back(laserCloudIn_org[r]);
            }
        }
    }
    g_all_pc->clear();
}

void lidar_cluster::extract_initial_seeds_(const pcl::PointCloud<PointType> &p_sorted)
{
    
    double sum = 0;
    int cnt = 0;
    for (int i = 0; i < p_sorted.points.size() && cnt < num_lpr_; i++)
    {
        sum += p_sorted.points[i].z;
        cnt++;
    }
    double lpr_height = cnt != 0 ? sum / cnt : 0; 
    g_seeds_pc->clear();
    
    for (int i = 0; i < p_sorted.points.size(); i++)
    {
        if (p_sorted.points[i].z < lpr_height + th_seeds_)
        {
            g_seeds_pc->points.push_back(p_sorted.points[i]);
        }
    }
    
}

void lidar_cluster::estimate_plane_(void)
{
    
    Eigen::Matrix3f cov;     
    Eigen::Vector4f pc_mean; 
    pcl::computeMeanAndCovarianceMatrix(*g_ground_pc, cov, pc_mean);
    
    JacobiSVD<MatrixXf> svd(cov, Eigen::DecompositionOptions::ComputeFullU);
    
    normal_ = (svd.matrixU().col(2));
    
    Eigen::Vector3f seeds_mean = pc_mean.head<3>();

    d_ = -(normal_.transpose() * seeds_mean)(0, 0); 
    th_dist_d_ = th_dist_ - d_;

}

其实有一些现在还没弄得太懂，弄懂了再说。（我发的东西只是我最近的领悟，类似于日常的总结，没多大帮助的话，还请见谅，谢谢！！！）