BKG traversabilitymapping 中的所使用变量的说明

最新推荐文章于 2024-08-16 07:57:02 发布

jiweinanyi

最新推荐文章于 2024-08-16 07:57:02 发布

阅读量464

点赞数

分类专栏： VO dense SLAM RRT 文章标签： SLAM 地形建模 BKG

本文链接：https://blog.csdn.net/jiweinanyi/article/details/116137146

版权

VO 同时被 3 个专栏收录

14 篇文章

订阅专栏

dense SLAM

5 篇文章

订阅专栏

RRT

4 篇文章

订阅专栏

前言

最近的工作主要集中在地形建模与主动探索上面，目前主要以BKG为框架，下面对代码所使用的变量进行整理。

首先是地图方面的参数，地图是以0.1分辨率的栅格构成的，上一层是1m分辨率的Cube，再上面才是全局地图

mapResolution=0.1m  地图栅格分辨率
mapCubeLength=1.0m  地图cube的大小
mapCubeArrayLength=mapCubeLength/mapResolution=10 每个Cube包含多少个栅格
mapArrayLength = 2000/mapCubeLength  一个全局地图包括的Cube维度，2000个
rootCubeIndex = mapArrayLength/2 根坐标系原点在整个地图的中心

然后是***滤波相关的参数***，这里要注意第0环对应的是最上面的一环还是最下面的一环，需要和LeGO-LOAM相确认

scanNumCurbFilter = 8;  //32 for VPL-64  从第0环到第scanNumCurbFilter环，进行negativeCurbFuilter和positiveCurbFilter的运行，前者是根据两个条件判断，首先相邻距离是否是连续上升或者下降的，其次是最后的点与最初的点是否是相近的。后者是根据相邻区域内的点的高度是否在一定阈值内。
scanNumSlopeFilter = 10; //40 for VPL-64 从第0环到第scanNumSlopeFilter环计算斜率
scanNumMax = max(scanNumCurbFilter, scanNumSlopeFilter);

接下来是滤波器阈值的相关参数，

sensorRangeLimit = 12; //只考虑12m以内的点
filterHeightLimit = 0.1; //每个栅格存储的最大最小高度值
filterAngleLimit  = 20; //斜率滤波的时候用到的
filterHeightMapArrayLength = sensorRangeLimit*2 / mapResolution; //感知范围内的栅格的维度

2D Map Publish Params

localMapLength = 20; // length of the local occupancy grid map (meter)
localMapArrayLength = localMapLength / mapResolution;//局部地图的维度

可视化参数

visualizationRadius = 200; //可视化的范围
visualizationFrequency = 2; //每2帧可视化一次

可行使区域的参数

traversabilityObserveTimeTh = 10; //被稳定观测到的次数
traversabilityCalculatingDistance = 8.0; //超过8m不计算和更新

接下来看关键的结构体数据
栅格信息存储的结构体，主要存储在地图一维序列中的索引，在二维cube地图中的索引，在cube中栅格的索引，

struct grid_t{
    int mapID;
    int cubeX;
    int cubeY;
    int gridX;
    int gridY;
    int gridIndex;
};

然后是一个栅格中的数据定义，每个cell对应该落在栅格中的一个点

struct mapCell_t{

    PointType *xyz; // it's a pointer to the corresponding point in the point cloud of submap

    grid_t grid;

    float log_odds;

    int observeTimes;
    
    float occupancy, occupancyVar;
    float elevation, elevationVar;

    mapCell_t(){

        log_odds = 0.5;
        observeTimes = 0;

        elevation = -FLT_MAX;
        elevationVar = 1e3;

        occupancy = 0; // initialized as unkown
        occupancyVar = 1e3;
    }

    void updatePoint(){
        xyz->z = elevation;
        xyz->intensity = occupancy;
    }
    void updateElevation(float elevIn, float varIn){
        elevation = elevIn;
        elevationVar = varIn;
        updatePoint();
    }
    void updateOccupancy(float occupIn){
        occupancy = occupIn;
        updatePoint();
    }
};

然后看一个cube地图的初始化

struct childMap_t{

    vector<vector<mapCell_t*> > cellArray;
    int subInd; //sub-map's index in 1d mapArray
    int indX; // sub-map's x index in 2d array mapArrayInd
    int indY; // sub-map's y index in 2d array mapArrayInd
    float originX; // sub-map's x root coordinate
    float originY; // sub-map's y root coordinate
    pcl::PointCloud<PointType> cloud;

    childMap_t(int id, int indx, int indy){

        subInd = id;
        indX = indx;
        indY = indy;
        originX = (indX - rootCubeIndex) * mapCubeLength - mapCubeLength/2.0;
        originY = (indY - rootCubeIndex) * mapCubeLength - mapCubeLength/2.0;

        // allocate and initialize each cell
        cellArray.resize(mapCubeArrayLength);
        for (int i = 0; i < mapCubeArrayLength; ++i)
            cellArray[i].resize(mapCubeArrayLength);

        for (int i = 0; i < mapCubeArrayLength; ++i)
            for (int j = 0; j < mapCubeArrayLength; ++j)
                cellArray[i][j] = new mapCell_t;
        // allocate point cloud for visualization
        cloud.points.resize(mapCubeArrayLength*mapCubeArrayLength);

        // initialize cell pointer to cloud point
        for (int i = 0; i < mapCubeArrayLength; ++i)
            for (int j = 0; j < mapCubeArrayLength; ++j)
                cellArray[i][j]->xyz = &cloud.points[i + j*mapCubeArrayLength];

        // initialize each point in the point cloud, also each cell
        for (int i = 0; i < mapCubeArrayLength; ++i){
            for (int j = 0; j < mapCubeArrayLength; ++j){
                
                // point cloud initialization
                int index = i + j * mapCubeArrayLength;
                cloud.points[index].x = originX + i * mapResolution;
                cloud.points[index].y = originY + j * mapResolution;
                cloud.points[index].z = std::numeric_limits<float>::quiet_NaN();
                cloud.points[index].intensity = cellArray[i][j]->occupancy;

                // cell position in the array of submap
                cellArray[i][j]->grid.mapID = subInd;
                cellArray[i][j]->grid.cubeX = indX;
                cellArray[i][j]->grid.cubeY = indY;
                cellArray[i][j]->grid.gridX = i;
                cellArray[i][j]->grid.gridY = j;
                cellArray[i][j]->grid.gridIndex = index;
            }
        }
    }
};