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

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.2double 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;//最小为0if(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 TODOstd::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~100int 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;
}

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

激光SLAM理论与实践-第五期第一次作业（矩阵坐标变换）
章节: 1.激光SLAM理论与实践-第五期第一次作业(矩阵坐标变换) 2.激光SLAM理论与实践-第五期第二次作业(里程计标定) 3.激光SLAM理论与实践-第五期第三次作业(去运动畸变) 4. ...
激光SLAM理论与实践 - 第四期作业解答（课件和作业网盘）
课件:https://www.lanzoui.com/b0akjnaeh 密码:g48p 作业:https://www.lanzoui.com/b0akgckuj 密码:3nfx 目录第一章作业第 ...
激光SLAM理论与实践第二次作业（里程计标定）
1.章节: 1.激光SLAM理论与实践-第五期第一次作业(矩阵坐标变换) 2.激光SLAM理论与实践-第五期第二次作业(里程计标定) 3.激光SLAM理论与实践-第五期第三次作业(去运动畸变) ...
激光SLAM理论与实践(一)--激光SLAM简要介绍
疫情原因一直在家,所以把之前学习的某蓝学院的激光SLAM的教程做一个学习笔记记录一下.话不多说,直接开始! 第一章主要是是激光SLAM的基本脉络的简介,综述类的介绍,主要分为以下四部分,讲了激光SLA ...
深蓝学院-视觉SLAM理论与实践-第十二期-第2章作业
熟悉Eigen矩阵运算设线性方程 A x = b Ax = b Ax=b,在 A A A为方阵的前提下,请回答以下问题: 问题1:什么条件下,x有解且唯一? 矩阵A非奇异,满秩. 问题2:高斯消元法 ...
深蓝学院-视觉SLAM理论与实践-第十二期-第4章作业
图像去畸变程序思路根据相机内参,将无畸变的像素坐标反向投影到归一化平面通过畸变模型计算无畸变归一化坐标对应的含有畸变的归一化坐标利用相机内参,将含有畸变的归一化坐标投影到相机坐标系中源代码 ...
深蓝学院-视觉SLAM理论与实践-第十二期-第3章作业
群的性质问题1:$ {Z, +} $ 是否为群?若是,验证其满足群定义:若不是,说明理由是群,满足如下性质封闭性:任何两个整数相加结果依然是整数结合律:a+b+b=a+(b+c)a+b+b = ...
深蓝学院SLAM理论与实践课程
深蓝学院SLAM理论与实践课程本文主要包括以下内容: 课件本人的作业答案本文目的: 分析讨论正在学习本课程的朋友建议独立完成作业课程评价: 这门课程是一门入门课程,适合SLAM入门者.机器人 ...
OOAD实践之路——真实案例解析OO理论与实践（五、需求分析之前的故事）
查看本系列全部文章: <OOA&D实践之路--真实案例解析OO理论与实践>索引贴高质量软件的第一要素到目前为止,我们做了很多工作,但是我一直在强调这些都还不是需求分 ...

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

1、章节：

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

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

最新文章

热门文章

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

1、章节：

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

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

最新文章

热门文章

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

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