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

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

1.1 Theory

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

2.3 Result

3. TSDF建图算法

3.1 Theory

Ref

1. 激光SLAM理论与实践(七)–基于已知定位的建图 ( 有课件)