激光SLAM理论与实践-第五期第七次作业

不加辣先生

已于 2022-05-30 17:45:24 修改

阅读量1.4k

点赞数 3

分类专栏：激光slam理论与实践文章标签：自动驾驶

于 2021-02-01 11:07:55 首次发布

本文链接：https://blog.csdn.net/weixin_44023934/article/details/113502009

版权

激光slam理论与实践专栏收录该内容

8 篇文章 37 订阅

订阅专栏

1、章节：

1、激光SLAM理论与实践-第五期第一次作业（矩阵坐标变换）
2、激光SLAM理论与实践-第五期第二次作业（里程计标定）
3、激光SLAM理论与实践-第五期第三次作业（去运动畸变）
4、激光SLAM理论与实践-第五期第四次作业（-帧间匹配算法，imls-icp和csm）
5、激光SLAM理论与实践-第五期第五次作业（高斯牛顿法优化）
6、激光SLAM理论与实践-第五期第六次作业 (g2o优化方法)
7、激光SLAM理论与实践-第五期第七次作业（mapping）

2、课程PPt和源码https://download.csdn.net/download/weixin_44023934/85491811

补充代码，通过覆盖栅格建图算法进行栅格地图构建；

可以看出实验的结果，有些边厚薄不均匀
2、将第 1 题代码改为通过计数建图算法进行栅格地图构建；

在没有设定m需要大于35就等于100前，直接将m赋给地图，边界效果很模糊：

加入约束后，边界比较清晰：
3、将第 1 题代码改为通过 TSDF 建图算法进行栅格地图构建；

该算法利用了加权的最小二乘的方法，使得边界生成虚拟的线，可以得出边界比较清晰，而且没有过多的噪点，但是实验的运行时间过长，代码复杂度高，而且实验地图生成需要在进行全部扫描结束后，实时性不强。

第四题、简答题，开放性答案：总结比较课堂所学的 3 种建图算法的优劣。
答：计数和覆盖栅格方法都利用了加法思想，每一次扫描就进行一次加法，优点，运算速率快，及时生成地图，可以同时建图定位；
缺点，由于结果是加法，每一次噪声都会叠加，所以地图出现过多的噪点，无法准确建图。
TSDF方法特点同第三题实验分析。

全部代码

#include "occupany_mapping.h"
#include "nav_msgs/GetMap.h"
#include "sensor_msgs/PointCloud.h"
#include "sensor_msgs/PointCloud2.h"
#include "geometry_msgs/Point32.h"

/**
 * Increments all the grid cells from (x0, y0) to (x1, y1);
 * //不包含(x1,y1)
 * 2D画线算法　来进行计算两个点之间的grid cell
 * @param x0
 * @param y0
 * @param x1
 * @param y1
 */
//插值函数
double interpolation(double A,double B,double a,double b)
{
    double value = (b*A-a*B) / (b-a);
    return value = a==b? A:value;
}
std::vector<GridIndex> TraceLine(int x0, int y0, int x1, int y1)
{
    GridIndex tmpIndex;
    std::vector<GridIndex> gridIndexVector;

    bool steep = abs(y1 - y0) > abs(x1 - x0);
    //变换使得 x轴 比 y轴 的值大，使得x1比x0大，y1 不一定不 y0大
    if (steep)
    {   
        std::swap(x0, y0);
        std::swap(x1, y1);
    }
    if (x0 > x1)
    {
        std::swap(x0, x1);
        std::swap(y0, y1);
    }

    int deltaX = x1 - x0;
    int deltaY = abs(y1 - y0);
    int error = 0;
    int ystep;
    int y = y0;

    if (y0 < y1)
    {
        ystep = 1;  //正增长
    }
    else
    {
        ystep = -1; //负增长
    }

    int pointX;
    int pointY;
    for (int x = x0; x <= x1; x++)
    {
        if (steep)  //Y长
        {
            pointX = y;
            pointY = x;
        }
        else         //X长
        {
            pointX = x;
            pointY = y;
        }

        error += deltaY;

        if (2 * error >= deltaX)
        {
            y += ystep;
            error -= deltaX;
        }

        //不包含最后一个点．
        if (pointX == x1 && pointY == y1)
            continue;

        //保存所有的点
        tmpIndex.SetIndex(pointX, pointY);

        gridIndexVector.push_back(tmpIndex);
    }

    return gridIndexVector;
}

void SetMapParams(void)
{
    mapParams.width = 1000;
    mapParams.height = 1000;
    mapParams.resolution = 0.05;

    //每次被击中的log变化值，覆盖栅格建图算法需要的参数
    mapParams.log_free = -1;
    mapParams.log_occ = 2;

    //每个栅格的最大最小值．
    mapParams.log_max = 100.0;
    mapParams.log_min = 0.0;

    mapParams.origin_x = 0.0;
    mapParams.origin_y = 0.0;
    

    //地图的原点，在地图的正中间
    mapParams.offset_x = 500;
    mapParams.offset_y = 500;


//个人认为是分配空间，对击中的地图，都是一维度 不击中的地图 世界地图 TSDF地图

    pMap = new unsigned char[mapParams.width * mapParams.height];
    
    //计数建图算法需要的参数
    //每个栅格被激光击中的次数
    pMapHits   = new unsigned long[mapParams.width * mapParams.height];
    //每个栅格被激光通过的次数
    pMapMisses = new unsigned long[mapParams.width * mapParams.height];

    //TSDF建图算法需要的参数
    pMapW    = new unsigned long[mapParams.width * mapParams.height];
    pMapTSDF = new double[mapParams.width * mapParams.height];



    //初始化
    for (int i = 0; i < mapParams.width * mapParams.height; i++)
    {
        //默认值
        pMap[i] = 50;
        pMapHits[i] = 0;
        pMapMisses[i] = 0;
        pMapW[i] = 0;//权重
        pMapTSDF[i] = -1;
    }
}

//从世界坐标系转换到栅格坐标系
GridIndex ConvertWorld2GridIndex(double x, double y)
{
    GridIndex index;
    //ceil返回大于等于该数的整数值，与floor相反
    index.x = std::ceil((x - mapParams.origin_x) / mapParams.resolution) + mapParams.offset_x;
    index.y = std::ceil((y - mapParams.origin_y) / mapParams.resolution) + mapParams.offset_y;

    return index;
}
//将栅格坐标转化为世界坐标
double GridIndex2ConvertWorld(int x)
{
    double x_;
    //ceil返回大于等于该数的整数值，与floor相反
    x_ = (x- mapParams.offset_x)*mapParams.resolution + mapParams.origin_x;
    return x_;
}
//将二维转化为一维
int GridIndexToLinearIndex(GridIndex index)
{
    int linear_index;
    linear_index = index.y + index.x * mapParams.width;
}

//判断index是否有效
bool isValidGridIndex(GridIndex index)
{
    if (index.x >= 0 && index.x < mapParams.width && index.y >= 0 && index.y < mapParams.height)
        return true;

    return false;
}

void DestoryMap()
{
    if (pMap != NULL)
        delete pMap;
}

//
void OccupanyMapping(std::vector<GeneralLaserScan> &scans, std::vector<Eigen::Vector3d> &robot_poses)
{
 
    
    std::cout << "开始建图，请稍后..." << std::endl;
    //枚举所有的激光雷达数据
    for (int i = 0; i < scans.size(); i++)
    {
        GeneralLaserScan scan = scans[i];
        Eigen::Vector3d robotPose = robot_poses[i];
        //机器人的下标，转化为栅格坐标
        GridIndex robotIndex = ConvertWorld2GridIndex(robotPose(0), robotPose(1));
        for (int id = 0; id < scan.range_readings.size(); id++)
        {
            double dist = scan.range_readings[id];
            double angle = -scan.angle_readings[id]; // 激光雷达逆时针转，角度取反由
            if (std::isinf(dist) || std::isnan(dist))
                continue;
            //加入击中范围t  TSDF 算法
            //不使用 TSDF方法 把t置零 用TSDF时候可以设置为1-0.05 一般0.2
            double t = 0.3;
            //计算得到该激光点的世界坐标系的坐标
            double theta = -robotPose(2); // 激光雷达逆时针转，角度取反
            double laser_x = (dist+t) * cos(angle);
            double laser_y = (dist+t) * sin(angle);
            double world_x = cos(theta) * laser_x - sin(theta) * laser_y + robotPose(0);
            double world_y = sin(theta) * laser_x + cos(theta) * laser_y + robotPose(1);
            //  std::cout << "开始建图，请稍后1..." << std::endl;
            //start of TODO 对对应的map的cell信息进行更新．（1,2,3题内容）
            GridIndex M_index = ConvertWorld2GridIndex(world_x,world_y);
            if(!isValidGridIndex(M_index)) continue;

            std::vector<GridIndex> free_index = TraceLine(robotIndex.x,robotIndex.y,M_index.x,M_index.y);
           #if 0  //覆盖栅格方法
            // std::cout << "覆盖栅格建图方法" << std::endl;
            for(int k= 0;k<free_index.size();k++)//遍历一条线的每一个空点
            {
            //更新空点
                //二维转一维
                int line_index = GridIndexToLinearIndex(free_index[k]);
                //更新
                int value  = pMap[line_index];// pMap[line_index] 只保存正数 unsinged！！！！！！！！！
                value += mapParams.log_free; 
                //最小为0 
                if( value < 0) pMap[line_index] = 0;
                else pMap[line_index] =value;
            }
            //更新被占用点
                int occ_index = GridIndexToLinearIndex( M_index);
                //更新
                pMap[occ_index] += mapParams.log_occ;
                //最小为0
                if(pMap[occ_index] > 100) pMap[occ_index] = 100;
            // #else  //计数方法
            //  std::cout << "计数建图方法" << std::endl;
            for(int j= 0;j<free_index.size();j++)//遍历一条线的每一个空点
            {
                    // std::cout << "开始建图，请稍后2..." << std::endl;
            //更新空点
                //二维转一维
                GridIndex tmp_free =free_index[j];
                int line_index1 = GridIndexToLinearIndex(tmp_free );
                //更新
                 ++pMapMisses[line_index1]; 
                //最小为0
            }
            //更新被占用点
                int occ_index1 = GridIndexToLinearIndex( M_index);
                //更新
                ++pMapHits[occ_index1];
             #endif

             #if 1  //TSDF算法
            //  std::cout << "TSDF算法建图方法" << std::endl;
             double TSDF,tsdf,x,y,delta_x,delta_y,b;
             for(int i = 0;i< free_index.size();i++)
             {  //栅格点转世界坐标
                x= (free_index[i].x-mapParams.offset_x)*mapParams.resolution +mapParams.origin_x;
                y= (free_index[i].y-mapParams.offset_y)*mapParams.resolution +mapParams.origin_y;
                //计算栅格点点到机器人的实际距离
                delta_x =x-robotPose(0);
                delta_y =y-robotPose(1);
                b = std::pow(std::pow( delta_x,2)+std::pow( delta_y,2),0.5);
                //sdfi(x)
                 TSDF = std::max(-1.0,std::min(1.0,(dist-b)/t));
                 //求出二维栅格坐标转一维序号
                 int pose_num = GridIndexToLinearIndex(free_index[i]);
                 //TSDFi(x)
                 pMapTSDF[pose_num] = (pMapW[pose_num]*pMapTSDF[pose_num]+TSDF)/(pMapW[pose_num]+1);
                 //Wi(x)
                 ++pMapW[pose_num];
             }
             #endif
            //end of TODO
        }
    }
    //start of TODO 通过计数建图算法或TSDF算法对栅格进行更新（2,3题内容）
    #if 0//计数方法

    for(int i = 0 ;i< mapParams.height*mapParams.width;i++)
         if(pMapHits[i] || pMapMisses[i]) 
         {
             int m = 100 * pMapHits[i] / (pMapMisses[i]+pMapHits[i]); 
         if (m > 35)
             {
                 pMap[i] = m ;
             }
             else
             {
                 pMap[i] = m;
             }
             
         }

    

    #endif

    #if 1 //TSDF算法
    for(int i= 0;i<mapParams.height-2;i++)  //设i 为y轴
    {
        for(int j = 0;j<mapParams.width-2;j++)  //设j为x轴  
        { 
            int line_value = j + i*mapParams.height;
            int line_x = line_value +1;//往x轴移动一格
            int line_y = line_value +mapParams.height;//往y轴方向移动一格
            //转世界坐标
            double A_x = GridIndex2ConvertWorld(j);
            double A_y = GridIndex2ConvertWorld(i);
            double B_x = GridIndex2ConvertWorld(j+1);
            double B_y = GridIndex2ConvertWorld(i+1);
            double a,b,b1,x,y;
            a = pMapTSDF[line_value];  b= pMapTSDF[line_x];  b1 = pMapTSDF[line_y];
            if( a*b1 < 0){ //x方向 
                   x = A_x;
                   y = interpolation(A_y,B_y,a,b1);//插值
                   pMap[GridIndexToLinearIndex(ConvertWorld2GridIndex(x,y))] = 100;
               }
           else if( a*b < 0){ //y方向
                   x = interpolation(A_x,B_x,a,b);
                   y = A_y;
                   pMap[GridIndexToLinearIndex(ConvertWorld2GridIndex(x,y))] = 100;
               }
                   
        }
    }

    #endif
    //end of TODO
    std::cout << "建图完毕" << std::endl;
}

//发布地图．
void PublishMap(ros::Publisher &map_pub)
{
    nav_msgs::OccupancyGrid rosMap;

    rosMap.info.resolution = mapParams.resolution;
    rosMap.info.origin.position.x = 0.0;
    rosMap.info.origin.position.y = 0.0;
    rosMap.info.origin.position.z = 0.0;
    rosMap.info.origin.orientation.x = 0.0;
    rosMap.info.origin.orientation.y = 0.0;
    rosMap.info.origin.orientation.z = 0.0;
    rosMap.info.origin.orientation.w = 1.0;

    rosMap.info.origin.position.x = mapParams.origin_x;
    rosMap.info.origin.position.y = mapParams.origin_y;
    rosMap.info.width = mapParams.width;
    rosMap.info.height = mapParams.height;
    rosMap.data.resize(rosMap.info.width * rosMap.info.height);

    //0~100
    int cnt0, cnt1, cnt2;
    cnt0 = cnt1 = cnt2 = 100;
    for (int i = 0; i < mapParams.width * mapParams.height; i++)
    {
        if (pMap[i] == 50)
        {
            rosMap.data[i] = -1.0;
        }
        else
        {

            rosMap.data[i] = pMap[i];
        }
    }

    rosMap.header.stamp = ros::Time::now();
    rosMap.header.frame_id = "map";

    map_pub.publish(rosMap);
}

int main(int argc, char **argv)
{
    ros::init(argc, argv, "OccupanyMapping");

    ros::NodeHandle nodeHandler;

    ros::Publisher mapPub = nodeHandler.advertise<nav_msgs::OccupancyGrid>("laser_map", 1, true);

    std::vector<Eigen::Vector3d> robotPoses;
    std::vector<GeneralLaserScan> generalLaserScans;//包括角度 长度

    std::string basePath = "/home/lcp/ws/shenlan2d_ws/src/data";

    std::string posePath = basePath + "/pose.txt";
    std::string anglePath = basePath + "/scanAngles.txt";
    std::string scanPath = basePath + "/ranges.txt";

    //读取数据
    ReadPoseInformation(posePath, robotPoses);

    ReadLaserScanInformation(anglePath,
                             scanPath,
                             generalLaserScans);

    //设置地图信息
    //创建 及 定义参数 初始化
    SetMapParams();

    OccupanyMapping(generalLaserScans, robotPoses);

    PublishMap(mapPub);

    ros::spin();

    DestoryMap();

    std::cout << "Release Memory!!" << std::endl;
}

不加辣先生

关注

3
点赞
踩
21

收藏

觉得还不错? 一键收藏
4
评论
激光SLAM理论与实践-第五期第七次作业

补充代码，通过覆盖栅格建图算法进行栅格地图构建；可以看出实验的结果，有些边厚薄不均匀2、将第 1 题代码改为通过计数建图算法进行栅格地图构建；在没有设定m需要大于35就等于100前，直接将m赋给地图，边界效果很模糊：加入约束后，边界比较清晰：3、将第 1 题代码改为通过 TSDF 建图算法进行栅格地图构建；该算法利用了加权的最小二乘的方法，使得边界生成虚拟的线，可以得出边界比较清晰，而且没有过多的噪点，但是实验的运行时间过长，代码复杂度高，而且实验地图生成需要在进行全部扫描.
复制链接

扫一扫