激光SLAM理论与实践 - 第六期 第7章 基于已知定位的建图 作业与心得

最新推荐文章于 2022-09-28 15:10:18 发布
最新推荐文章于 2022-09-28 15:10:18 发布
阅读量786 收藏 12
点赞数 2
分类专栏： SLAM学习 文章标签： 深蓝学院激光SLAM理论与实践 C++ 覆盖栅格 计数建图算法 TSDF
版权声明：本文为博主原创文章，遵循 CC 4.0 BY-SA 版权协议，转载请附上原文出处链接和本声明。
本文链接：https://blog.csdn.net/shamozhizhoutx/article/details/120386622
版权

  1. 补充代码，通过覆盖栅格建图算法进行栅格地图构建；（3 分）

        //start of TODO 对对应的map的cell信息进行更新．（1,2,3题内容）
    //1
    GridIndex beamPointIndex = ConvertWorld2GridIndex(world_x, world_y);
    std::vector<GridIndex> beamTraceindexes = TraceLine(robotIndex.x, robotIndex.y, beamPointIndex.x, beamPointIndex.y);
    for(auto index : beamTraceindexes)
    {
        if(isValidGridIndex(index))
        {
            int tmpLinearIndex = GridIndexToLinearIndex(index);
            if(pMap[tmpLinearIndex] == 0) continue;
            pMap[tmpLinearIndex] += mapParams.log_free;
        }else{
            std::cerr << "index is invalid!!!" << std::endl;
        }
    }
    if(isValidGridIndex(beamPointIndex))
    {
        int tmpLinearIndex = GridIndexToLinearIndex(beamPointIndex);
        
        pMap[tmpLinearIndex] += mapParams.log_occ;
        if(pMap[tmpLinearIndex] >= 100) pMap[tmpLinearIndex] = 100;
    }else{
        std::cerr << "beamPointIndex is invalid!!!" << std::endl;
    }
    //end of TODO

    运行结果如下：

        $ rosrun occupany_mapping occupany_mapping
    poses.size(): 3701
    Read Pose Good!!!
    Read Angle good:1081
    XX:1081
    laserscans.size():3701
    Read Laser Scans Good!!!!
    开始建图，请稍后...
    建图完毕

    整体结果图：
    在这里插入图片描述

    局部结果图：
    在这里插入图片描述

    地图消息说明nav_msgs/OccupancyGrid.msg。

  2. 将第 1 题代码改为通过计数建图算法进行栅格地图构建；（3 分）

        //start of TODO 对对应的map的cell信息进行更新．（1,2,3题内容）
    //2
    GridIndex beamPointIndex = ConvertWorld2GridIndex(world_x, world_y);
    std::vector<GridIndex> beamTraceindexes = TraceLine(robotIndex.x, robotIndex.y, beamPointIndex.x, beamPointIndex.y);
    for(auto index : beamTraceindexes)
    {
        if(isValidGridIndex(index))
        {
            int tmpLinearIndex = GridIndexToLinearIndex(index);
            ++pMapMisses[tmpLinearIndex];
        }else{
            std::cerr << "index is invalid!!!" << std::endl;
        }
    }
    if(isValidGridIndex(beamPointIndex))
    {
        int tmpLinearIndex = GridIndexToLinearIndex(beamPointIndex);
        ++pMapHits[tmpLinearIndex];
    }else{
        std::cerr << "beamPointIndex is invalid!!!" << std::endl;
    }
    //end of TODO

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

    运行后发现，如果阈值设置较大，地图边界不清晰，总体结果图如下：
    在这里插入图片描述

    局部结果图：
    在这里插入图片描述

    将阈值改为35后，地图边界变得清晰，总体结果图如下：
    在这里插入图片描述

    局部结果图：
    在这里插入图片描述

  3. 将第 1 题代码改为通过 TSDF 建图算法进行栅格地图构建；（4 分）

        //start of TODO 对对应的map的cell信息进行更新．（1,2,3题内容）
    //3
    double far_dist = dist + 0.142; //0.05*2*sqrt(2)
    //计算得到该激光点的世界坐标系的坐标
    double far_laser_x = far_dist * cos(angle);
    double far_laser_y = far_dist * sin(angle);

    double far_world_x = cos(theta) * far_laser_x - sin(theta) * far_laser_y + robotPose(0);
    double far_world_y = sin(theta) * far_laser_x + cos(theta) * far_laser_y + robotPose(1);
    GridIndex farBeamPointIndex = ConvertWorld2GridIndex(far_world_x, far_world_y);
    std::vector<GridIndex> farBeamTraceindexes = TraceLine(robotIndex.x, robotIndex.y, farBeamPointIndex.x, farBeamPointIndex.y);
    for(auto index : farBeamTraceindexes)
    {
        if(isValidGridIndex(index))
        {
            //栅格坐标系转换到世界坐标系
            double x = (index.x - mapParams.offset_x) * mapParams.resolution + mapParams.origin_x;
            double y = (index.y - mapParams.offset_y) * mapParams.resolution + mapParams.origin_y;
            double d = std::sqrt((x-robotPose(0))*(x-robotPose(0)) + (y-robotPose(1))*(y-robotPose(1)));
            double sdf = dist - d;
            double tsdf = std::max(-1.0, std::min(1.0, sdf/0.1));
            int tmpLinearIndex = GridIndexToLinearIndex(index);
            pMapTSDF[tmpLinearIndex] = (pMapW[tmpLinearIndex]*pMapTSDF[tmpLinearIndex] + tsdf) / (pMapW[tmpLinearIndex] + 1);
            pMapW[tmpLinearIndex] = pMapW[tmpLinearIndex] + 1;
        }else{
            std::cerr << "index is invalid!!!" << std::endl;
        }
    }
    //end of TODO

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

    for (int i = 0; i < mapParams.width-1; ++i) //x
    {
        for(int j = 0; j < mapParams.height-1; ++j) //y
        {
            GridIndex tmpOrgIndex;
            tmpOrgIndex.SetIndex(i, j);
            int tmpOrgLinearIndex = GridIndexToLinearIndex(tmpOrgIndex);
            double tmpOrgTSDF = pMapTSDF[tmpOrgLinearIndex];
            if(tmpOrgTSDF==1 || tmpOrgTSDF==-1) continue;   //去除未击中点

            GridIndex tmpUpIndex;
            tmpUpIndex.SetIndex(i, j+1);
            int tmpUpLinearIndex = GridIndexToLinearIndex(tmpUpIndex);
            double tmpUpTSDF = pMapTSDF[tmpUpLinearIndex];

            GridIndex tmpRightIndex;
            tmpRightIndex.SetIndex(i+1, j);
            int tmpRightLinearIndex = GridIndexToLinearIndex(tmpRightIndex);
            double tmpRightTSDF = pMapTSDF[tmpRightLinearIndex];

            //没有求出精确位置，直接求出地图坐标
            if((tmpOrgTSDF<0 && tmpUpTSDF>0) || (tmpOrgTSDF>0 && tmpUpTSDF<0))
            {
                if(std::fabs(tmpOrgTSDF) < std::fabs(tmpUpTSDF))
                {
                    pMap[tmpOrgLinearIndex] = 100;
                }else{
                    pMap[tmpUpLinearIndex] = 100;
                }
            }
            if((tmpOrgTSDF<0 && tmpRightTSDF>0) || (tmpOrgTSDF>0 && tmpRightTSDF<0))
            {
                if(std::fabs(tmpOrgTSDF) < std::fabs(tmpRightTSDF))
                {
                    pMap[tmpOrgLinearIndex] = 100;
                }else{
                    pMap[tmpRightLinearIndex] = 100;
                }
            }
        }
    }
    //end of TODO

    运行后总体结果图如下：
    在这里插入图片描述

    局部结果图：
    在这里插入图片描述

    转角位置不好求，没有啥好办法，这里没有求出精确位置，直接求出地图坐标，后续有好办法再更新。

  4. 简答题，开放性答案：总结比较课堂所学的 3 种建图算法的优劣。（2 分）

    覆盖栅格建图算法对栅格更新只需进行加法操作，更新速度较快；计数建图算法也是进行加法操作，但是需要额外存储misses和hits，需要额外占用一些内存。
    上述两个算法都没有考虑传感器的不确定性，如果噪声叠加，可能出现对同一障碍物有多个栅格厚度的地图。
    TSDF建图算法充分考虑传感器测量的不确定性，如果传感器的噪声服从高斯分布，那么通过TSDF进行融合，等价于通过最小二乘来进行融合，能插值出确切的曲面，构建的地图最多只有一个栅格的厚度，但是计算更复杂，更新速度较慢。

沙漠之舟tx
关注
  • 2
    点赞
  • 踩
  • 12
    收藏
    觉得还不错? 一键收藏
  • 10
    评论
专栏目录
激光SLAM理论与实践(七)--基于已知定位的建图
weixin_43619346的博客
06-10 824
本章主要介绍基于已知定位的建图，从以下三个方面来介绍：
激光SLAM7-基于已知位姿的构图算法
lsxpu的专栏
02-22 548
1. 通过覆盖栅格建图算法进行栅格地图的构建 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 (con
基于已知位姿的建图方法
小牟加油
09-10 599
目前较为简单的SLAM建图过程都是将定位和建图进行分离，在确定机器人位姿后，通过激光传感器的测量量对周围环境进行建图。 如何利用激光传感器的测量信息进行建图，主要是三种方法： 1. 覆盖栅格建图算法 slam11-gridmaps- 4.pdf 3.3MB 2. 计数建图算法 3. TSDF建图算法 1.覆盖栅格建图 2. 计数建图 3. TSDF建图 整体算法的思想是最小二乘法。可以对不同测量信息进行拟合...
激光SLAM理论与实践第7章
07-07
occupany_mapping.tar.gz
激光SLAM理论与实践第2章
07-07
《激光SLAM理论与实践》第二章主要探讨了激光SLAM（Simultaneous Localization And Mapping，同时定位与建图）的基础概念和技术流程。这一章在深入理解SLAM的核心原理上，为后续章节的实践应用奠定了基础。以下是本...
激光SLAM理论与实践第一章
07-07
《激光SLAM理论与实践第一章》是关于机器人定位与建图技术的重要篇章，主要探讨了在现实环境中如何利用激光传感器进行自主导航和环境理解。在这个章节中，我们首先会接触到一个核心概念——“变换”，它是SLAM算法中...
基于单线激光雷达SLAM建图与路径规划c++实现源码+项目说明(SLAM建图、定位、路径规划).zip
最新发布
05-14
基于单线激光雷达SLAM建图与路径规划c++实现源码+项目说明(SLAM建图、定位、路径规划).zip基于单线激光雷达SLAM建图与路径规划c++实现源码+项目说明(SLAM建图、定位、路径规划).zip基于单线激光雷达SLAM建图与路径...
激光SLAM理论与实践第三章
01-20
《激光SLAM理论与实践第三章》主要涵盖了激光SLAM（Simultaneous Localization and Mapping，同时定位与建图）技术的基础知识，特别是基于C++实现的相关算法和实践应用。SLAM是机器人领域的一个核心问题，它允许无人...
基于已知建图的算法
qq_44709827的博客
08-15 292
针对深蓝学院覆盖栅格地图算法作业出现建图效果不理想情况，经分析和查找，原来是缺少一个符号
深蓝学院视觉slam十四讲第７章作业
Jevin-L的博客
07-23 2395
目录2. Bundle Adjustment2.1 文献阅读2.2 BAL-dataset3. 直接法的 Bundle Adjustment3.1 数学模型3.2 实现 2. Bundle Adjustment 2.1 文献阅读 1.矩阵Ｈ具有稀疏性，对角块矩阵有加速计算技巧 2.相机内参　位姿　3D点坐标　投影后的像素坐标； pose： 欧拉角/四元数/旋转矩阵/李代数＋平移坐标 point：齐次和齐次 3.文中提到的“网络图”即“图优化”当下几乎代替了以往的滤波算法 2.2 BAL-dataset
机器人学习--栅格地图（occupancy grid map）构建（部分代码解析）
GGY1102的博客
10-09 7288
转自：占据栅格地图构建（Occupancy Grid Map） - 知乎 用于学习参考用。 懂了占据栅格地图构建算法，就意味着SLAM问题不再是抽象的理论公式，它变成了浮现在你我脑海里的动态构建过程。这将对我们完整理解各种激光SLAM算法起着十分重要的作用。 1、地图的分类 移动机器人常见的地图有三种：尺度地图、拓扑地图、语义地图。 尺度地图：具有真实的物理尺寸，如栅格地图、特征地图、点云地图；常用于地图构建、定位、SLAM、小规模路径规划。 拓扑地图：不具备真...
激光雷达构建地图（ 覆盖栅格建图）
qq_46067306的博客
07-18 2250
介绍了覆盖栅格建图的数学推导过程
SLAM2d栅格地图构建的常用方法
啥也不懂的研究生也淦SLAM的博客
09-14 3212
SLAM2d栅格地图构建的常用方法原理与代码，包括占据栅格法、计数法、tsdf法
Chapter7-基于已知位姿的建图方法
kh0601的博客
07-01 474
Chapter7-基于已知位姿的建图方法 如有错误望不吝赐教！！！ 地图分类 尺度地图（栅格地图） 拓扑地图 语义地图 覆盖栅格建图算法 栅格地图的特征点 把环境分成一个个小栅格 每个栅格有两种状态：占用、空闲（去除一些小的通行区域） 非参模型 随着地图增大，内存暴增 天然区分可通行区域，适合进行轨迹规划 构建栅格地图 给定机器人位姿和传感器的观测数据（主要指激光雷达）data={...
激光雷达学习笔记（四）定位
机器视觉
02-28 1万+
原创博客，转载请注明出处：http://blog.csdn.net/renshengrumenglibing?viewmode=contents 机器人定位的目的是为了知道“自己在什么地方”，目前，机器人定位的方法可以分为非自主定位与自 主定位两大类。所谓非自主定位是在定位的过程中机器人需要借助机器人本身以外的装置如：全球定位 系统(GPS)、全局视觉系统等进行定位；自主定位是机器人仅依靠机
移动机器人定位与地图创建（SLAM）方法
热门推荐
彼岸花
04-22 5万+
自主移动机器人同时定位与地图创建（SLAM）方法 1.引言： 机器人的研究越来越多的得到关注和投入，随着计算机技术和人工智能的发展，智能自主移动机器人成为机器人领域的一个重要研究方向和研究热点。移动机器人的定位和地图创建是自主移动机器人领域的热点研究问题。对于已知环境中的机器人自主定位和已知机器人位置的地图创建已经有了一些实用的解决方法。然而在很多环境中机器人不能利用全局定位系统进行定位，而且
激光slam第7次作业
ChenXiaoFeng0420的博客
09-28 178
本次给出的代码中可以通过 readfile.h 中的宏定义：READ_DATA_NUMBER 来确定要读取的激光数据的帧数，建议前期调试的时候稍微取小一些(例如 100)，调试完毕之后再取大，最大不要超过3700。
激光slam理论与实践 第一周作业
10-19
激光SLAM（Simultaneous Localization and Mapping）是指通过利用激光扫描仪的数据同时实现机器人的自我定位和环境建图的过程。这项技术已经成为机器人领域中重要的研究内容之一。 在激光SLAM理论方面，主要有几个关键的概念。首先是地图构建，机器人通过扫描周围环境，将获取到的激光点云数据转化为一幅地图。同时，激光SLAM也需要实现机器人的同时自我定位，也就是在未知环境中，机器人通过分析激光数据推算出其自身的位置。 实践方面，激光SLAM需要激光传感器进行环境测量。激光传感器会在扫描过程中发射激光束，然后通过接收反射回来的激光束，来计算击中目标物体的位置。机器人通过不断地旋转或移动激光传感器，以此来获取周围环境的激光点云数据。 激光SLAM的实施过程主要包括建图、定位和配准等步骤。建图过程中，机器人通过收集周围环境的激光数据，将其转化为一幅地图。定位过程中，机器人通过对比当前获得的激光数据和已有的地图数据，从而推算出自身的位置。配准是指将不同位置、角度下获取的激光数据进行融合，从而得到整体一致性的地图。 总的来说，激光SLAM理论与实践是通过利用激光扫描仪的数据实现机器人定位以及地图构建的过程。通过激光传感器扫描环境，将激光数据转化为地图，并实现机器人的同时自我定位。激光SLAM技术在无人驾驶、工业自动化等领域有重要的应用价值。
评论 10
添加红包

请填写红包祝福语或标题

个

红包个数最小为10个

元

红包金额最低5元

当前余额3.43元 前往充值 >
需支付：10.00元
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付元
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明：

1.余额是钱包充值的虚拟货币，按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载，可以购买VIP、付费专栏及课程。

余额充值