激光SLAM7-基于已知位姿的构图算法

原创

已于 2023-02-22 22:43:06 修改 · 704 阅读

标签

#算法

于 2022-02-22 23:43:11 首次发布

该博客介绍了激光SLAM中如何利用已知位姿信息构建栅格地图，包括覆盖栅格和计数建图两种算法。在覆盖栅格建图部分，详细讲解了理论、代码实现和结果展示；而在计数建图中同样涵盖了理论、代码和结果分析。此外，还提及了TSDF建图的理论基础。

1. 通过覆盖栅格建图算法进行栅格地图的构建

1.1 Theory

算法流程
判断是否为free

1.2 code

这里没有判断idx和hitPtIndex是否有效:

//start of TODO 对对应的map的cell信息进行更新．（1,2,3题内容）
GridIndex hitPtIndex = ConvertWorld2GridIndex(world_x, world_y);
std::vector<GridIndex> freeIndexs = TraceLine(robotIndex.x, robotIndex.y, hitPtIndex.x, hitPtIndex.y);
for (const auto& idx : freeIndexs) {
   
   
    const auto& linIdx = GridIndexToLinearIndex(idx);
    if (pMap[linIdx] == 0) {
   
   
        continue;
    }

    pMap[linIdx] += mapParams.log_free;
}

const auto& linIdx = GridIndexToLinearIndex(hitPtIndex);
pMap[linIdx] += mapParams.log_occ;
if (pMap[linIdx] > mapParams.log_max) {
   
   
    pMap[linIdx] = mapParams.log_max;
}
//end of TODO

1.3 result

栅格地图

2. 通过计数建图算法进行栅格地图的构建

2.1 Theory

在这里插入图片描述

2.2 Code

//start of TODO 对对应的map的cell信息进行更新．（1,2,3题内容）
GridIndex hitPtIndex = ConvertWorld2GridIndex(world_x, world_y);
std::vector<GridIndex> freeIndexs = TraceLine(robotIndex.x, robotIndex.y, hitPtIndex.x, hitPtIndex.y);
for (const auto& idx : freeIndexs) {
   
   
    if (!isValidGridIndex(idx)) {
   
   
        continue;
    }
    const auto& linIdx = GridIndexToLinearIndex(idx);
    pMapMisses[linIdx] += 1;
}

if (isValidGridIndex(hitPtIndex)) {
   
   
    const auto& linIdx = GridIndexToLinearIndex(hitPtIndex);
    pMapHits[linIdx] += 1;
}
//end of TODO

//start of TODO 通过计数建图算法或TSDF算法对栅格进行更新（2,3题内容）
for (uint32_t i = 0; i < mapParams.width * mapParams.height; ++i) {
   
   
    int visNum = pMapHits[i] + pMapMisses[i];
    if (visNum > 0) {
   
   
        pMap[i] = pMapHits[i] * 100 / (pMapHits[i] + pMapMisses[i]); 
    }
}
//end of TODO