livox_free_space可行驶区域检测

最新推荐文章于 2025-08-24 08:38:41 发布

原创最新推荐文章于 2025-08-24 08:38:41 发布 · 1k 阅读

22 ·

CC 4.0 BY-SA版权

文章标签：

#github #c++ #自动驾驶 #可行驶区域

livox_free_space

大疆旗下livox激光雷达开源的一个可行驶区域检测的小工具。

1. 依赖

ROS
Eigen
PCL

2. 安装

cd ~/your_workspace/src
git clone https://github.com/Livox-SDK/livox_free_space
cd ..
catkin_make

源码修改
src/FreeSpace.cpp中int LivoxFreeSpace::GenerateFreeSpace(float* fPoint_In, int pointNum, std::vector & free_space)加一个return 0;

3. 启动

cd ~/workspace
source devel/setup.bash
roslaunch livox_ros_driver livox_lidar.launch

# 播放rosbag
rosbag play [XXX.bag] [/src_topic_name]:=/points_raw
# 有多个lidar消息时，可能rviz出来点云会闪，所以可以设置只播放一个点云消息
rosbag play [XXX.bag] --topics [/src_topic_name]
# 映射话题
rosrun topic_tools relay [/src_topic_name] /points_raw

4. 原理

FreeSpace_node.cpp中的main函数通过循环调用GenerateFreeSpace(pc)生成可行驶区域，发送消息。

void GenerateFreeSpace(pcl::PointCloud<pcl::PointXYZI> & pc)
......
float *data=(float*)calloc(dnum*4,sizeof(float));
    std::vector<float> free_space;
    for (int p=0; p<dnum; ++p)
    {
        data[p*4+0] = pc.points[p].x;
        data[p*4+1] = pc.points[p].y;
        data[p*4+2] = pc.points[p].z;
        data[p*4+3] = pc.points[p].intensity;
    }       
    livox_free_space.GenerateFreeSpace(data, dnum, free_space);
......

主要看这部分代码就可以了，用一维数组来保存点云信息（后续代码中都采用了这种方式，牢记+1是y坐标，+2是z坐标，不写是x坐标），之后调用livox_free_space类的GenerateFreeSpace函数生成可行驶区域，下面具体介绍这个函数实现。

int LivoxFreeSpace::GenerateFreeSpace(float* fPoints1, int pointNum, std::vector<float> & free_space)
// fPoints: 输入的点云[IN]
// pointNum: 输入点云数量[IN]
// free_space: 可行驶区域[OUT]

具体主要做了几件事：降采样、体素化、地面分割、测量自车到非地面点的最小距离、扩充可行驶区域。

4.1 参数设置

// FreeSpace.hpp
// 地面分割范围
#define GND_IMG_NX1 24
#define GND_IMG_NY1 20
// 地面分割分辨率
#define GND_IMG_DX1 4
#define GND_IMG_DY1 4
// 地面分割偏移
#define GND_IMG_OFFX1 40
#define GND_IMG_OFFY1 40

// 栅格化范围
#define DN_SAMPLE_IMG_NX 500
#define DN_SAMPLE_IMG_NY 200
#define DN_SAMPLE_IMG_NZ 100
// 栅格化分辨率
#define DN_SAMPLE_IMG_DX 0.4
#define DN_SAMPLE_IMG_DY 0.4
#define DN_SAMPLE_IMG_DZ 0.2
// 栅格化偏移
#define DN_SAMPLE_IMG_OFFX 50 // -50～150
#define DN_SAMPLE_IMG_OFFY 40 // -40～40
#define DN_SAMPLE_IMG_OFFZ 10// -10～10
// 降采样范围及分辨率
#define DENOISE_IMG_NX 200 
#define DENOISE_IMG_NY 80 
#define DENOISE_IMG_NZ 100
#define DENOISE_IMG_DX 1 
#define DENOISE_IMG_DY 1 
#define DENOISE_IMG_DZ 0.2

// FreeSpace.cpp
// filter是半径为5的圆周上的点
int filter_x[28]={-1,0,1,-3,-2,2,3,-4,4,-4,4,-5,5,-5,5,-5,5,-1,0,1,-3,-2,2,3,-4,4,-4,4};
int filter_y[28]={-5,-5,-5,-4,-4,-4,-4,-3,-3,-2,-2,-1,-1,0,0,1,1,5,5,5,4,4,4,4,3,3,2,2};
// all是半径为5的圆内的所有点
int all_x[89]={-1,0,1, 
                -3,-2,-1,0,1,2,3, 
                -4,-3,-2,-1,0,1,2,3,4, 
                -4,-3,-2,-1,0,1,2,3,4, 
                -5,-4,-3,-2,-1,0,1,2,3,4,5,
                -5,-4,-3,-2,-1,0,1,2,3,4,5,
                -5,-4,-3,-2,-1,0,1,2,3,4,5,
                -1,0,1,
                -3,-2-1,0,1,2,3,
                -4,-3,-2,-1,0,1,2,3,4,
                -4,-3,-2,-1,0,1,2,3,4};
int all_y[89]={-5,-5,-5,
                -4,-4,-4,-4,-4,-4,-4,
                -3,-3,-3,-3,-3,-3,-3,-3,-3,
                -2,-2,-2,-2,-2,-2,-2,-2,-2,
                -1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,
                0,0,0,0,0,0,0,0,0,0,0,
                1,1,1,1,1,1,1,1,1,1,1,
                5,5,5,
                4,4,4,4,4,4,4,
                3,3,3,3,3,3,3,3,3,
                2,2,2,2,2,2,2,2,2};

在这里插入图片描述

4.2 降采样

int *idtrans1=(int*)calloc(pointNum,sizeof(int));  // 记录点在体素化后的序号
std::vector<int> count(DENOISE_IMG_NX*DENOISE_IMG_NY*DENOISE_IMG_NZ, 0);

for(int pid=0;pid<pointNum;pid++)
{   
    // 点云的体素索引
    int ix=(fPoints1[pid*4]+DN_SAMPLE_IMG_OFFX)/DENOISE_IMG_DX; 
    int iy=(fPoints1[pid*4+1]+DN_SAMPLE_IMG_OFFY)/DENOISE_IMG_DY; 
    int iz=(fPoints1[pid*4+2]+DN_SAMPLE_IMG_OFFZ)/DENOISE_IMG_DZ;
    idtrans1[pid]=-1;
    // 如果点云在预设范围内
    if((ix>=0)&&(ix<DENOISE_IMG_NX)&&(iy>=0)&&(iy<DENOISE_IMG_NY)&&(iz>=0)&&(iz<DENOISE_IMG_NZ)) 
    {
        int idx = iz*DENOISE_IMG_NX*DENOISE_IMG_NY+iy*DENOISE_IMG_NX+ix;   // 三维索引 -> 一维索引 类似[1,0,0] -> 100
        idtrans1[pid]=idx;
        count[idx]++;     
    }
}

for(int pid=0;pid<pointNum;pid++)
{   // idtrans1[pid] > -1 表示该点在范围内
    // count[idtrans1[pid]] < 3 表示该点在体素内的数量少于3
    // 那么就认为该点是噪点，将其置为0
    if(idtrans1[pid] > -1 && count[idtrans1[pid]] < 3)
    {
        fPoints1[pid*4] = 0;
        fPoints1[pid*4 + 1] = 0;
        fPoints1[pid*4 + 2] = 0;

    }
}

这段就是比较简单的体素化降采样和去噪，去噪后的点保存在fPoints1

4.3 栅格化

float *fPoints2=(float*)calloc(pointNum*4,sizeof(float));
int *idtrans2=(int*)calloc(pointNum,sizeof(int));  // 记录栅格内点的序号
int *idtransx=(int*)calloc(pointNum,sizeof(int));
int *idtransy=(int*)calloc(pointNum,sizeof(int));
int pntNum = 0;
this->pVImg=(unsigned char*)calloc(DN_SAMPLE_IMG_NX*DN_SAMPLE_IMG_NY*DN_SAMPLE_IMG_NZ,sizeof(unsigned char));   // 是否访问过

for(int pid=0;pid<pointNum;pid++)
{   
    // 点云的栅格体素索引
    int ix=(fPoints1[pid*4]+DN_SAMPLE_IMG_OFFX)/DN_SAMPLE_IMG_DX;
    int iy=(fPoints1[pid*4+1]+DN_SAMPLE_IMG_OFFY)/DN_SAMPLE_IMG_DY;
    int iz=(fPoints1[pid*4+2]+DN_SAMPLE_IMG_OFFZ)/DN_SAMPLE_IMG_DZ;
    idtrans1[pid]=-1;
    // 如果点云在预设范围内
    if((ix>=0)&&(ix<DN_SAMPLE_IMG_NX)&&(iy>=0)&&(iy<DN_SAMPLE_IMG_NY)&&(iz>=0)&&(iz<DN_SAMPLE_IMG_NZ))
    {
        int idx = iz*DN_SAMPLE_IMG_NX*DN_SAMPLE_IMG_NY+iy*DN_SAMPLE_IMG_NX+ix;  // 体素一维索引
        idtrans1[pid] = idx;
        idtransx[pid] = ix;
        idtransy[pid] = iy;
        if(pVImg[idx]==0)//没有访问过的栅格，只取第一个点
        {
            fPoints2[pntNum*4] = fPoints1[pid*4];             // x
            fPoints2[pntNum*4 + 1] = fPoints1[pid*4+1];       // y
            fPoints2[pntNum*4 + 2] = fPoints1[pid*4+2];       // z
            fPoints2[pntNum*4 + 3] = fPoints1[pid*4+3];       // I
            idtrans2[pntNum] = pid;
            pntNum++;
        }
        pVImg[idx] = 1;
    }
}

创建一个二维平面栅格，用落在其中的第一个点作为该栅格的点，所有栅格点保存在fPoints2

4.4 地面分割

int LivoxFreeSpace::GroundSegment(int* pLabel,float *fPoints,int pointNum,float fSearchRadius)
// pLabel: 点的标签（0:非地面点, 1:地面点, -1:范围外的点）  [OUT]
// fPoints: 输入的栅格化后的点  [IN]
// pointNum: 输入的点的数量  [IN]

地面分割函数也分为几个步骤：地面栅格化、初步地面点划分、拟合地面平面筛选地面点

4.4.1 地面栅格化

    float *pGndImg1 = (float*)calloc(GND_IMG_NX1*GND_IMG_NY1,sizeof(float));  // 地面栅格
    int *tmpLabel1 = (int*)calloc(pointNum,sizeof(int));  // 记录点在地面栅格中的索引
    
    for(int ii=0;ii<GND_IMG_NX1*GND_IMG_NY1;ii++)
    {
        pGndImg1[ii]=100;
    }
    for(int pid=0;pid<pointNum;pid++)
    {
        int ix= (fPoints[pid*4]+GND_IMG_OFFX1)/(GND_IMG_DX1+0.000001);
        int iy= (fPoints[pid*4+1]+GND_IMG_OFFY1)/(GND_IMG_DY1+0.000001);
        if(ix<0 || ix>=GND_IMG_NX1 || iy<0 || iy>=GND_IMG_NY1)  // 该点超出范围
        {
            tmpLabel1[pid]=-1;
            continue;
        }

        int iid=ix+iy*GND_IMG_NX1;
        tmpLabel1[pid]=iid;  // 记录该点在pGndImg1中的索引

        // 地面栅格高度取点该栅格内所有点的高度最小值
        if(pGndImg1[iid]>fPoints[pid*4+2])
        {
            pGndImg1[iid]=fPoints[pid*4+2];  
        }
    }

地面栅格化的分辨率比上一步栅格化的分辨率更低，即一个地面栅格会包含多个点，因此取其中最小高度值作为地面栅格高度。

4.4.2 初步地面分割

// 1. 如果栅格内的点最大高度和最小高度差小于0.4，则认为该点是地面点
int pnum=0;
for(int pid=0;pid<pointNum;pid++)
{
    if(tmpLabel1[pid]>=0)
    {
        if(pGndImg1[tmpLabel1[pid]]+0.4>fPoints[pid*4+2])
        {
            pLabel[pid]=1;
            pnum++;
        }
    }
}
free(pGndImg1);
free(tmpLabel1);

// 2. 根据位置和高度进一步判断地面点
for(int pid=0;pid<pointNum;pid++)
{   
    // 如果是地面点
    if(pLabel[pid]==1)
    {   
        // 如果地面点高度大于1，认为该点不是地面点
        if(fPoints[pid*4+2]>1)
        {
            pLabel[pid]=0;
        }
        // 如果地面点半径小于10，并且高度大于0.5，认为该点不是地面点
        else if(fPoints[pid*4]*fPoints[pid*4]+fPoints[pid*4+1]*fPoints[pid*4+1]<100)
        {
            if(fPoints[pid*4+2]>0.5)
            {
                pLabel[pid]=0;
            }
        }

    }
    // 如果不是地面点，并且点半径小于20，并且高度小于0.2，认为该点是地面点
    else
    {
        if(fPoints[pid*4]*fPoints[pid*4]+fPoints[pid*4+1]*fPoints[pid*4+1]<400)
        {
            if(fPoints[pid*4+2]<0.2)
            {
                pLabel[pid]=1;
            }
        }
    }

}

pcl::PointCloud<pcl::PointXYZI>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZI>());
int in_zone[pointNum] = {0};
// 点半径小于20的地面点，转成pcl格式，存入cloud
for(int pid=0;pid<pointNum;pid++)
{       
    if(fPoints[pid*4]*fPoints[pid*4]+fPoints[pid*4+1]*fPoints[pid*4+1]<400)
    {
        in_zone[pid] = 1;
        if (pLabel[pid]==1)
        {
            pcl::PointXYZI p;
            p.x = fPoints[pid*4];
            p.y = fPoints[pid*4 + 1];
            p.z = fPoints[pid*4 + 2];
            p.intensity = fPoints[pid*4 + 3];
            cloud->points.push_back(p);
        }
    }
}

初步判断地面点：

根据地面栅格内的点的高度进行判断，若与地面栅格高度差小于0.4m，则是地面点。
判断地面点的位置和高度，如果高度>1m，或者半径<10m且高度>0.5m，将其判定为非地面点；判断非地面点的位置和高度，半径<20m且高度<0.2m，将其判定为地面点。
第一条0.4可能是个经验参数，第二条考虑到了如果目标离lidar比较近，点云可能覆盖不全目标，导致误判断为地面点。
总得来说，地面点判定基于纯规则，难以避免经验值的使用，需要根据具体工况去调整。

4.4.3 拟合地面平面，再筛选地面点

Eigen::Matrix3f cov;
Eigen::Vector4f pc_mean;
Eigen::MatrixXf normal_;       // Eigen::Vector3f

pcl::computeMeanAndCovarianceMatrix(*cloud, cov, pc_mean);
Eigen::JacobiSVD<Eigen::MatrixXf> svd(cov,Eigen::DecompositionOptions::ComputeFullU);
normal_ = (svd.matrixU().col(2));     // 地面平面的法向量
Eigen::Vector3f seeds_mean = pc_mean.head<3>();
//  normal.T * [x,y,z] = -d
float d_ = -(normal_.transpose()*seeds_mean)(0,0);  // 点到地面平面的垂直距离 d_

std::cout << "d_:" << d_ << std::endl;

float th_dist_d_ = 0.3 - d_;
Eigen::MatrixXf points(pointNum, 3);
for(int k = 0; k < pointNum; k++)
{
    points.row(k) << fPoints[k*4], fPoints[k*4+ 1], fPoints[k*4+ 2];
}

Eigen::VectorXf result = points * normal_;  // 地面点云中的每个点到地面平面的垂直距离
// 根据距离判断是否为地面点
for (int k = 0; k < pointNum; k++)
{
    if (!in_zone[k])
        continue;
    if (result[k] < th_dist_d_)
    {
        pLabel[k] = 1;
    }
    else
    {
        pLabel[k] = 0;
    }

}

gnum=0;
for(int pid=0;pid<pointNum;pid++)
{
    if(pLabel[pid]==1)
    {
        gnum++;
    }
}

return gnum;

根据上一步筛选的地面点组成的点云，拟合平面并计算地面法相向量，根据每个点到平面的距离判别是否是地面点。没搞懂这个d_表示什么距离，以及距离阈值根据什么取的。

4.5 计算射线方向距离最小值

// fPoints: 输入的非地面点云[IN] 
// n: 非地面点数量[IN]
// free_space: 射线方向最小距离[OUT]
// free_space_n: 角度数[IN] 360°
void LivoxFreeSpace::FreeSpace(float* fPoints, int n, float* free_space, int free_space_n)
{
    int thetaId;
    float distance;

    for(int ii=0; ii < free_space_n; ii++)
    {
        free_space[ii] = 2500;
    }

    // 计算每个角度上的最小距离
    for(int pid=0;pid<n;pid++)
    {
        if(fPoints[pid*4+2] < 3) // points of high tree, buildings are rejected
        {   
            // 距离自车太近就忽略
            if (abs(fPoints[pid*4 + 1]) < 1.2 && abs(fPoints[pid*4]) < 2.5) 
                continue;
            distance = fPoints[pid*4]*fPoints[pid*4] + fPoints[pid*4+1]*fPoints[pid*4+1];
            thetaId = int((atan2f(fPoints[pid*4+1], fPoints[pid*4]) + FREE_PI) * 180.0 / FREE_PI + 0.5);
            thetaId = thetaId % free_space_n;
            // 距离取最小值
            if(free_space[thetaId] > distance && distance > 1)
            {
                free_space[thetaId] = distance;
            }
        }
    }

}

计算每个点的角度，该方向的可行驶区域距离取非地面点云的最小值。

4.6 扩充可行驶区域

void LivoxFreeSpace::FreeSpaceFilter(float* free_space_small, int n , std::vector<float> & free_space)
// free_space_small: 360个点，每个点表示该方向上的最小距离[IN]
// n: 360[IN]
// free_space: 360个点，每个点表示该方向上的最大距离[OUT]

根据上一步的射线方向的最小距离填充内部可行驶区域，并且对没有障碍物的方向将可行驶区域扩展到半径50m范围。

4.6.1 最小距离范围内可行驶区域

    float pixel_size = 0.2;     // 可行驶区域分辨率
    float delta_d_in_r = 0.13;  // 用于后续计算遍历角度步长
    float delta_r = 0.15; // 遍历半径的步长,delta_d_in_r is smaller than pixel_size and delta_r, to make sure all pixels are covered
    Eigen::MatrixXi src = Eigen::MatrixXi::Zero(100/pixel_size, 100/pixel_size);  // 最小范围内可行驶区域
    Eigen::MatrixXi dst = Eigen::MatrixXi::Zero(100/pixel_size, 100/pixel_size);  // 50m内可行驶区域
    
    // 生成遍历角度的步长
    std::vector<float> delta_t;
    for (float j = 0.0001; j < 50; j += delta_r) // Prepare the delta theta of different radius
    {
        delta_t.push_back(delta_d_in_r/j);
    } 

    for (int i = 0; i < 360; i++)
    {   
        // r 取当前射线和相邻两条射线的最小值
        float r = min(free_space_small[i], free_space_small[(i + 1) % n]);
        r = min(r, free_space_small[(i - 1 + n) % n]);
        r = sqrt(r);
        int k = 0;
        // 遍历半径
        for (float j = 0; j < r - 0.5; j += delta_r)
        {
            float dt = delta_t[k++];
            float theta = (i - 180)*FREE_PI/180.0;
            // 遍历当前半径下，当前射线附近的角度
            for (float t = theta - 0.01; t < theta + 0.01; t+=dt)
            {
                float x = j*cos(t);
                float y = j*sin(t);
                int m =int((50.0 - x) / pixel_size);
                int nn =int((50.0 - y) / pixel_size);
                src(m, nn) = 1;
            }
        }
    }

上一步计算了各个方向距离障碍物的最小距离，这步将自车到障碍物的范围内，可行驶区域栅格src点都设置为可行驶区域。
由于按照射线方向遍历，半径越大，射线之间的距离越远，因此需要在这些地方填补点。
代码中遍历不同圈半径后，再在这个半径上遍历射线角度附近的点，将其设置为可行驶区域。

4.6.2 生成50m内可行驶区域

for (int i = 0; i < 360; i++)
    {
        for (float j = 0; j < 49; j += delta_r)
        {
            float x = j * cos((i - 180)*FREE_PI/180.0);
            float y = j * sin((i - 180)*FREE_PI/180.0);
            int m =int((50.0 - x) / pixel_size);
            int nn =int((50.0 - y) / pixel_size);
            int theta = int(atan2f(y, x) * 180.0 / FREE_PI + 180.0 + 0.5);
            theta = theta % n;
            float r = min(free_space_small[theta], free_space_small[(theta + 1) % n]);
            r = min(r, free_space_small[(theta - 1 + n) % n]);
            if (r > j*j + 1)
            {
                int result = 0;
                // 遍历当前点圆周上的28个点，如果不全是free space，则该点也不是free space
                for (int k = 0; k < 28; k++)  
                {
                    result += src(m + filter_x[k], nn + filter_y[k]);
                }
                if (result < 28) // check if position (m, nn) is in free space
                    break;
                // 遍历当前点圆周的89个点，如果该点周围有free space，则该点也是free space
                for (int k = 0; k < 89; k++) 
                {
                    dst(m+all_x[k], nn+all_y[k]) = max(1, dst(m+all_x[k], nn+all_y[k]));   // 这max有啥用？1和2没区别
                }
                dst(m, nn) = 2;
            }
        }
    }

因为之前划分非地面点云时范围比较小，对于没有障碍物的射线方向，将其可行驶区域设置到50m范围。
对于没有障碍的射线方向，free_space_small应该是一个比较大的值，对于满足条件的点，首先判断其附近半径为5的圆周上的28个点是否都为可行驶区域，如果不全是，则该点也不是；反之，则将其附近半径为5的圆内所有点都设置为可行驶区域。

4.6.3 栅格坐标转换为世界坐标

for (int i = 0; i < dst.rows(); i++)
    {
        for (int j = 0; j < dst.cols(); j++)
        {
            if (dst(i, j) > 0)
            {
                float x = (100.0 - i*pixel_size) - 50.0;
                float y = (100.0 - j*pixel_size) - 50.0;
                free_space.push_back(x);
                free_space.push_back(y);
                free_space.push_back(255);                    
            }
        }
    }