ALOAM mapping

最新推荐文章于 2023-07-20 11:52:27 发布

SLAM||DUNK

最新推荐文章于 2023-07-20 11:52:27 发布

阅读量2.5k

点赞数 12

分类专栏：激光SLAM

本文链接：https://blog.csdn.net/qq_42700518/article/details/105253062

版权

激光SLAM 专栏收录该内容

14 篇文章 13 订阅

订阅专栏

LOAM建图模块总结

在这里插入图片描述

LOAM地图的设定

我们知道LOAM是一个激光里程计算法，它建立的地图只是为给里程计提供一个优化，由于在里程计的结算时采用scan-scan，前后帧两的特征并不充足，导致里程计速度快却不精确，因此，可以将很多帧的特征累计起来，形成一个地图，然后，将帧和地图匹配即scan-map，这样可以可以到地图上获取更多的约束，从而优化里程计的结果。
LOAM中的地图的概念有两个，一个大地图，另一个是用于匹配的局部地图，地图优化时，到大地图中获取当前位置附近的地图数据形成局部地图。

大地图

就是LOAM在运行时我们在rviz上看到的点云地图。但是这里要注意，这个大地图其实并不是我们常常认为的全局地图，它的大小是有限制的，运行面积过大时，这个大地图就不能显示完整的信息，不过如果运行面积不大，这个大地图的还是可以显示全的，实测kitti是可以显示全的。始终要记得，LOAM的这个地图，并不是单纯的地图构建，而是用来优化里程计的。

1、大地图的储存结构
这个大地图由很多个小的cube组成，每个cube的大小为50m×50m，大地图中划分的cube个数如下：

// 地图划分cube的数量 
const int laserCloudWidth = 21;
const int laserCloudHeight = 21;
const int laserCloudDepth = 11;
//cube的总数 
const int laserCloudNum = laserCloudWidth * laserCloudHeight * laserCloudDepth;

大地图保存在一个一维数组中，每个数组元素储存一个子cube的点云指针

pcl::PointCloud<PointType>::Ptr laserCloudCornerArray[laserCloudNum];
pcl::PointCloud<PointType>::Ptr laserCloudSurfArray[laserCloudNum];

该数组储存cube的顺序如下图

在这里插入图片描述
一个坐标为(laserCloudWidth=i, laserCloudHeight=j, laserCloudDepth=k )的cube 我们可以通过下面的索引在数组中找到：
laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k]

2、大地图的更新
大地图的更新存在两种方式:
方式一：大地图并不是固定不动的，大地图保存的是当前激光雷达周边的点云，由于大地图尺寸有限且固定，因此，大地图需要跟随激光雷达运动，这样大地图才能始终保证激光雷达处于大地图内部。
初始时，Map坐标原点位于大地图的中点，即：

// 地图原点处cell的坐标
int laserCloudCenWidth = 10;
int laserCloudCenHeight = 10;
int laserCloudCenDepth = 5;

laserCloudCenWidth，laserCloudCenHeight，laserCloudCenDepth 表示Map系原点的cube坐标，这个坐标会随着大地图的移动而变化。
通过下面方式求得当前激光雷达处于大地图哪个cube中：

// 获得在world系下的坐标      odom设置为 q_w_curr 、t_w_curr 作为Map优化的初值  
transformAssociateToMap();
TicToc t_shift;
// 计算当前激光的位置处于cell的坐标
int centerCubeI = int((t_w_curr.x() + 25.0) / 50.0) + laserCloudCenWidth;
int centerCubeJ = int((t_w_curr.y() + 25.0) / 50.0) + laserCloudCenHeight;
int centerCubeK = int((t_w_curr.z() + 25.0) / 50.0) + laserCloudCenDepth;

if (t_w_curr.x() + 25.0 < 0)
centerCubeI--;
if (t_w_curr.y() + 25.0 < 0)
centerCubeJ--;
if (t_w_curr.z() + 25.0 < 0)
centerCubeK--;

其中
int centerCubeI = int((t_w_curr.x() + 25.0) / 50.0) + laserCloudCenWidth;
50是一个cube的size, 然后，Map原点前后[-25,25)的区域是一个cube，坐标每增加50则cube加一。如下：
在这里插入图片描述
求得当前激光雷达位于cube的坐标后，接着需要判断大地图是否需要移动，如果激光雷达过于靠近大地图的边缘
(距离边缘小于3)则大地图需要向相同的方向移动移动，丢弃最远端的点云，增加近端的空间，这样才有足够的空间扩充大地图，如图：
在这里插入图片描述
部分代码如下：

/******************************  大地图进行移动    **********************************/
while (centerCubeI < 3)
{
	for (int j = 0; j < laserCloudHeight; j++)
	{
		for (int k = 0; k < laserCloudDepth; k++)
		{   
			int i = laserCloudWidth - 1;   												
			for (; i >= 1; i--)
			{   // 前移    向左边界移动
				laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =
					laserCloudCornerArray[i - 1 + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k];
				laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =
					laserCloudSurfArray[i - 1 + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k];
			}						
			// i=0时的点云清空准备添加新的点云 						
		    laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k].reset(new pcl::PointCloud<PointType>());
			laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k].reset(new pcl::PointCloud<PointType>());						
		}
	}
             // cube中心I坐标 ++ 
	centerCubeI++;
	laserCloudCenWidth++;
}

因为点云所在的位置是固定的，所以大地图移动即cube数组点云进行反向移动。

方式二：在位姿优化完毕后，将当前激光帧的特征添加到大地图中。如下：

// 将corner points按距离（比例尺缩小）归入相应的立方体
// 把当前时刻的点存入cube里，为下一次做准备  角点存储与 laserCloudCornerArray 
for (int i = 0; i < laserCloudCornerStackNum; i++)
{
 // 点从激光的局部坐标系 转移到世界坐标系  
pointAssociateToMap(&laserCloudCornerStack->points[i], &pointSel);
 // 获取该特征位于cell的坐标   
int cubeI = int((pointSel.x + 25.0) / 50.0) + laserCloudCenWidth;
int cubeJ = int((pointSel.y + 25.0) / 50.0) + laserCloudCenHeight;
int cubeK = int((pointSel.z + 25.0) / 50.0) + laserCloudCenDepth;

if (pointSel.x + 25.0 < 0)
cubeI--;
if (pointSel.y + 25.0 < 0)
cubeJ--;
if (pointSel.z + 25.0 < 0)
cubeK--;

if (cubeI >= 0 && cubeI < laserCloudWidth &&
cubeJ >= 0 && cubeJ < laserCloudHeight &&
cubeK >= 0 && cubeK < laserCloudDepth)
{   // 获得索引
int cubeInd = cubeI + laserCloudWidth * cubeJ + laserCloudWidth * laserCloudHeight * cubeK;
laserCloudCornerArray[cubeInd]->push_back(pointSel);
}
}
// 将当前的平面点存入 laserCloudSurfArray
for (int i = 0; i < laserCloudSurfStackNum; i++)
{
pointAssociateToMap(&laserCloudSurfStack->points[i], &pointSel);
 // 获得归属的cell坐标
int cubeI = int((pointSel.x + 25.0) / 50.0) + laserCloudCenWidth;
int cubeJ = int((pointSel.y + 25.0) / 50.0) + laserCloudCenHeight;
int cubeK = int((pointSel.z + 25.0) / 50.0) + laserCloudCenDepth;

if (pointSel.x + 25.0 < 0)
cubeI--;
if (pointSel.y + 25.0 < 0)
cubeJ--;
if (pointSel.z + 25.0 < 0)
cubeK--;

if (cubeI >= 0 && cubeI < laserCloudWidth &&
cubeJ >= 0 && cubeJ < laserCloudHeight &&
cubeK >= 0 && cubeK < laserCloudDepth)
{
int cubeInd = cubeI + laserCloudWidth * cubeJ + laserCloudWidth * laserCloudHeight * cubeK;
laserCloudSurfArray[cubeInd]->push_back(pointSel);
}
}

1、获取每个特征在Map系下的坐标。

pointAssociateToMap()

2、求得该特帧的坐标位于哪个cube中

 // 获得归属的cell坐标
int cubeI = int((pointSel.x + 25.0) / 50.0) + laserCloudCenWidth;
int cubeJ = int((pointSel.y + 25.0) / 50.0) + laserCloudCenHeight;
int cubeK = int((pointSel.z + 25.0) / 50.0) + laserCloudCenDepth;

if (pointSel.x + 25.0 < 0)
cubeI--;
if (pointSel.y + 25.0 < 0)
cubeJ--;
if (pointSel.z + 25.0 < 0)
cubeK--;

3、如果该cube在大地图的范围内，则将该特征添加到该cube的点云中

if (cubeI >= 0 && cubeI < laserCloudWidth &&
cubeJ >= 0 && cubeJ < laserCloudHeight &&
cubeK >= 0 && cubeK < laserCloudDepth)
{
int cubeInd = cubeI + laserCloudWidth * cubeJ + laserCloudWidth * laserCloudHeight * cubeK;
laserCloudSurfArray[cubeInd]->push_back(pointSel);
}

优化匹配

首先将odometryBuf中接受到的里程计数据保存为q_wodom_curr、t_wodom_curr

// 获取里程计的数据放置与 q_wodom_curr t_wodom_curr
q_wodom_curr.x() = odometryBuf.front()->pose.pose.orientation.x;
q_wodom_curr.y() = odometryBuf.front()->pose.pose.orientation.y;
q_wodom_curr.z() = odometryBuf.front()->pose.pose.orientation.z;
q_wodom_curr.w() = odometryBuf.front()->pose.pose.orientation.w;
t_wodom_curr.x() = odometryBuf.front()->pose.pose.position.x;
t_wodom_curr.y() = odometryBuf.front()->pose.pose.position.y;
t_wodom_curr.z() = odometryBuf.front()->pose.pose.position.z;
odometryBuf.pop();

然后调用transformAssociateToMap() 将里程计数据转换到当前的Map坐标中，如下：
q_wmap_wodom、t_wmap_wodom是此前求得的odom->Map系的变换矩阵


// set initial guess  设置优化的初始值    将odom的位姿转换为w的位姿      Two*Tob = Twb   
// 即使用odometry的结果作为map匹配的初始值   
void transformAssociateToMap()
{
	q_w_curr = q_wmap_wodom * q_wodom_curr;
	t_w_curr = q_wmap_wodom * t_wodom_curr + t_wmap_wodom;
}

q_w_curr，t_w_curr同时也是Map优化时局部地图的位姿初值。
q_w_curr，t_w_curr通过Map重映射为ceres的优化参数parameters，定义在文件前面，如下：

double parameters[7] = {0, 0, 0, 1, 0, 0, 0};
Eigen::Map<Eigen::Quaterniond> q_w_curr(parameters);
Eigen::Map<Eigen::Vector3d> t_w_curr(parameters + 4);

这样的话，ceres对parameters进行优化的时候也会同时改变q_w_curr、t_w_curr的值。

获取当前激光雷达位置处附近125个cube的点云数据，5×5×5 的cube。

int laserCloudValidNum = 0;
int laserCloudSurroundNum = 0;
//在每一维附近5个cube(前2个，后2个，中间1个)里进行查找（250米范围内），三个维度总共125个cube
//在这125个cube里面进一步筛选在视域范围内的cube
for (int i = centerCubeI - 2; i <= centerCubeI + 2; i++)
{
for (int j = centerCubeJ - 2; j <= centerCubeJ + 2; j++)
{
	for (int k = centerCubeK - 1; k <= centerCubeK + 1; k++)
	{
		if (i >= 0 && i < laserCloudWidth &&
			j >= 0 && j < laserCloudHeight &&
			k >= 0 && k < laserCloudDepth)
		{
                        //记住视域范围内的cube索引，匹配用
			laserCloudValidInd[laserCloudValidNum] = i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k;
			laserCloudValidNum++;
                        //记住附近所有cube的索引，显示用
			laserCloudSurroundInd[laserCloudSurroundNum] = i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k;
			laserCloudSurroundNum++;
		}
	}
}
}

laserCloudValidInd、laserCloudSurroundInd 保存这些cube的索引。
根据索引，到laserCloudCornerArray、laserCloudSurfArray获取相应的数据，保存到laserCloudCornerFromMap、
laserCloudSurfFromMap 作为当前匹配的局部地图(local map)。

laserCloudCornerFromMap->clear();
laserCloudSurfFromMap->clear();
//构建特征点地图，查找匹配使用
for (int i = 0; i < laserCloudValidNum; i++)
{
//map中提取的匹配使用的边沿点
*laserCloudCornerFromMap += *laserCloudCornerArray[laserCloudValidInd[i]];
*laserCloudSurfFromMap += *laserCloudSurfArray[laserCloudValidInd[i]];
}

有了用来匹配的局部地图，接着要获取当前激光的点云数据，如下，可以看到当前与Local map匹配的点云是由
laserCloudCornerLast、laserCloudSurfLast构成的，

// 当前lidar的点云数据    用来和Local Map匹配
pcl::PointCloud<PointType>::Ptr laserCloudCornerStack(new pcl::PointCloud<PointType>());
downSizeFilterCorner.setInputCloud(laserCloudCornerLast);        // 当前的激光局部地图的角点 进行滤波
downSizeFilterCorner.filter(*laserCloudCornerStack);
int laserCloudCornerStackNum = laserCloudCornerStack->points.size();

pcl::PointCloud<PointType>::Ptr laserCloudSurfStack(new pcl::PointCloud<PointType>());
downSizeFilterSurf.setInputCloud(laserCloudSurfLast);        // 当前的激光局部地图的平面点 进行滤波
downSizeFilterSurf.filter(*laserCloudSurfStack);
int laserCloudSurfStackNum = laserCloudSurfStack->points.size();

将laserCloudCornerLast、laserCloudSurfLast滤波后，生成laserCloudCornerStack、laserCloudSurfStack即我们当前时刻激光的点云，接下来，就要将该点云去和Local Map进行匹配来优化位姿。

首先将Local map - laserCloudCornerFromMap、laserCloudSurfFromMap 设置为kdtree的搜索对象

// Local map 设置为KDTREE的搜索对象   
kdtreeCornerFromMap->setInputCloud(laserCloudCornerFromMap);
kdtreeSurfFromMap->setInputCloud(laserCloudSurfFromMap);

接着遍历当前点云的全部角点 laserCloudCornerStackNum，通过kdtree搜索Local map中与各个角点最近的5个点，然后通过主成分分析，获取直线的参数，最后再将点到直线的残差添加到ceres中。

// 遍历当前点云的全部的角点 
for (int i = 0; i < laserCloudCornerStackNum; i++)
{
	pointOri = laserCloudCornerStack->points[i];
	//double sqrtDis = pointOri.x * pointOri.x + pointOri.y * pointOri.y + pointOri.z * pointOri.z;
	pointAssociateToMap(&pointOri, &pointSel);       // 将点转到Map坐标   
	// pointSearchInd： 搜索到按距离排序的点序号    
	kdtreeCornerFromMap->nearestKSearch(pointSel, 5, pointSearchInd, pointSearchSqDis);
	/***************************************
	 寻找最近的5个点，对点云协方差矩阵进行主成份分析：
	 若这5个点分布在直线上，协方差矩阵的特征值包含一个元素显著大于其余两个，与该特征值相关的特征向量表示所处直线的方向;
	 若这5个点分布在平面上，协方差矩阵的特征值存在一个显著小的元素，与该特征值相关的特征向量表示所处平面的法线方向。
	 参考论文:2016,IROS,fast and robust 3d feature extraction from sparse point clouds
	 ***************************************/
	if (pointSearchSqDis[4] < 1.0) //5个点中最大距离不超过1才处理
	{ 
		std::vector<Eigen::Vector3d> nearCorners;     // 保存5个点的坐标 
		Eigen::Vector3d center(0, 0, 0);
		for (int j = 0; j < 5; j++)
		{
			Eigen::Vector3d tmp(laserCloudCornerFromMap->points[pointSearchInd[j]].x,
								laserCloudCornerFromMap->points[pointSearchInd[j]].y,
								laserCloudCornerFromMap->points[pointSearchInd[j]].z);
			center = center + tmp;
			nearCorners.push_back(tmp);
		}
		// 五个点的坐标的算术平均    即期望  Ex
		center = center / 5.0;
                       /*
		 *  计算协方差矩阵   ，注意协方差矩阵计算的是样本不同维度间的相关性   而不是样本之间的相关性
		 *  这里有5个样本点 ，每个样本3维   协方差矩阵 = 1/(n-1) * (X-EX)(X-EX)^T ,   X = (x1,x2,x3,x4,x5) 
		 *  这里没有除 1/n-1 但关系不大，因为特征向量的方向不会改变  
		 * */
		Eigen::Matrix3d covMat = Eigen::Matrix3d::Zero();
		for (int j = 0; j < 5; j++)
		{   
			Eigen::Matrix<double, 3, 1> tmpZeroMean = nearCorners[j] - center;     
			covMat = covMat + tmpZeroMean * tmpZeroMean.transpose();    
		}

		Eigen::SelfAdjointEigenSolver<Eigen::Matrix3d> saes(covMat);
		// if is indeed line feature
		// note Eigen library sort eigenvalues in increasing order
		Eigen::Vector3d unit_direction = saes.eigenvectors().col(2);    // 最大的特征值对应的特征向量
		Eigen::Vector3d curr_point(pointOri.x, pointOri.y, pointOri.z);
		// 如果最大的特征向量  明显比第二大的大    则认为是直线 
		if (saes.eigenvalues()[2] > 3 * saes.eigenvalues()[1])
		{ 
			Eigen::Vector3d point_on_line = center;     // 取均值作为该直线的一点 
			Eigen::Vector3d point_a, point_b;
			// 取直线的两点    中点+-0.1×(直线方向单位向量)
			point_a = 0.1 * unit_direction + point_on_line;   
			point_b = -0.1 * unit_direction + point_on_line;
                           // 添加残差  
			ceres::CostFunction *cost_function = LidarEdgeFactor::Create(curr_point, point_a, point_b, 1.0);
			problem.AddResidualBlock(cost_function, loss_function, parameters, parameters + 4);
			corner_num++;	
		}							
	}
}

然后是遍历点云的平面点 laserCloudSurfStackNum，通过kdtree获取Local map上最近的5个点，通过QR分解求解最小二乘问题获得平面参数，最后构建点到平面的残差。

for (int i = 0; i < laserCloudSurfStackNum; i++)
{
pointOri = laserCloudSurfStack->points[i];
//double sqrtDis = pointOri.x * pointOri.x + pointOri.y * pointOri.y + pointOri.z * pointOri.z;
pointAssociateToMap(&pointOri, &pointSel);       // 将点转到Map坐标  
kdtreeSurfFromMap->nearestKSearch(pointSel, 5, pointSearchInd, pointSearchSqDis);    // Kdtree搜索5个点  

Eigen::Matrix<double, 5, 3> matA0;          // 每一行一个样本点  
Eigen::Matrix<double, 5, 1> matB0 = -1 * Eigen::Matrix<double, 5, 1>::Ones();
if (pointSearchSqDis[4] < 1.0)
{
	
	for (int j = 0; j < 5; j++)
	{
		matA0(j, 0) = laserCloudSurfFromMap->points[pointSearchInd[j]].x;
		matA0(j, 1) = laserCloudSurfFromMap->points[pointSearchInd[j]].y;
		matA0(j, 2) = laserCloudSurfFromMap->points[pointSearchInd[j]].z;
	}
	// find the norm of plane
	// 平面方程   Ax+By+Cz+D = 0 =>  (x,y,z)(A/D,B/D,C/D)^T = -1  => 求解  Ax = b ,x即为法向量   
	Eigen::Vector3d norm = matA0.colPivHouseholderQr().solve(matB0);
	double negative_OA_dot_norm = 1 / norm.norm();
	norm.normalize();

	// Here n(pa, pb, pc) is unit norm of plane   X^T*n = -1 =>  X^T*n/|n| + 1/|n| = 0  如果结果>0.2则认为有误   
	bool planeValid = true;
	for (int j = 0; j < 5; j++)
	{
		// if OX * n > 0.2, then plane is not fit well
		if (fabs(norm(0) * laserCloudSurfFromMap->points[pointSearchInd[j]].x +
				 norm(1) * laserCloudSurfFromMap->points[pointSearchInd[j]].y +
				 norm(2) * laserCloudSurfFromMap->points[pointSearchInd[j]].z + negative_OA_dot_norm) > 0.2)
		{
			planeValid = false;
			break;
		}
	}
	// 添加残差  ！！！！！！！！！！！！！！！！！！
	Eigen::Vector3d curr_point(pointOri.x, pointOri.y, pointOri.z);
	if (planeValid)
	{
		ceres::CostFunction *cost_function = LidarPlaneNormFactor::Create(curr_point, norm, negative_OA_dot_norm);
		problem.AddResidualBlock(cost_function, loss_function, parameters, parameters + 4);
		surf_num++;
	}
}
}

接下来就可以执行优化了，优化完成后的结果更新在 q_w_curr、t_w_curr。
调用 transformUpdate()，获取最新的odom->map的变换矩阵 q_wmap_wodom、 t_wmap_wodom。
因为优化的位于Map系上的位姿为 Twb = TwoTob => Two = TwbTob^-1

// q_w_curr   t_w_curr是指优化的结果  
// q_wodom_curr   t_wodom_curr 是指odometry结果  
// 优化后的结果不同 则认为存在两个全局坐标  优化后的为W‘  优化前的为W   
void transformUpdate()
{
	q_wmap_wodom = q_w_curr * q_wodom_curr.inverse();         // 求出Rwo  
	t_wmap_wodom = t_w_curr - q_wmap_wodom * t_wodom_curr;    // 求出Two
}

接下来，将当前的点云的角点与平面点，按照位置存放在相应的cell中，角点存放在 laserCloudCornerArray，
平面点存放在 laserCloudSurfArray 。

// 将corner points按距离（比例尺缩小）归入相应的立方体
// 把当前时刻的点存入cube里，为下一次做准备  角点存储与 laserCloudCornerArray 
for (int i = 0; i < laserCloudCornerStackNum; i++)
{
// 点从激光的局部坐标系 转移到世界坐标系  
pointAssociateToMap(&laserCloudCornerStack->points[i], &pointSel);
// 获取位于cell的坐标    {laserCloudCenWidth，laserCloudCenHeight，laserCloudCenDepth} 表示坐标原点的cell 
int cubeI = int((pointSel.x + 25.0) / 50.0) + laserCloudCenWidth;
int cubeJ = int((pointSel.y + 25.0) / 50.0) + laserCloudCenHeight;
int cubeK = int((pointSel.z + 25.0) / 50.0) + laserCloudCenDepth;

if (pointSel.x + 25.0 < 0)
cubeI--;
if (pointSel.y + 25.0 < 0)
cubeJ--;
if (pointSel.z + 25.0 < 0)
cubeK--;

if (cubeI >= 0 && cubeI < laserCloudWidth &&
cubeJ >= 0 && cubeJ < laserCloudHeight &&
cubeK >= 0 && cubeK < laserCloudDepth)
    //只挑选-laserCloudCenWidth * 50.0 < point.x < laserCloudCenWidth * 50.0范围内的点，y和z同理
    //按照尺度放进不同的组，每个组的点数量各异
{   // 获得索引
int cubeInd = cubeI + laserCloudWidth * cubeJ + laserCloudWidth * laserCloudHeight * cubeK;
laserCloudCornerArray[cubeInd]->push_back(pointSel);
}
}

// 将当前的平面点存入 laserCloudSurfArray
for (int i = 0; i < laserCloudSurfStackNum; i++)
{
pointAssociateToMap(&laserCloudSurfStack->points[i], &pointSel);
            // 获得归属的cell坐标
int cubeI = int((pointSel.x + 25.0) / 50.0) + laserCloudCenWidth;
int cubeJ = int((pointSel.y + 25.0) / 50.0) + laserCloudCenHeight;
int cubeK = int((pointSel.z + 25.0) / 50.0) + laserCloudCenDepth;

if (pointSel.x + 25.0 < 0)
	cubeI--;
if (pointSel.y + 25.0 < 0)
	cubeJ--;
if (pointSel.z + 25.0 < 0)
	cubeK--;

if (cubeI >= 0 && cubeI < laserCloudWidth &&
	cubeJ >= 0 && cubeJ < laserCloudHeight &&
	cubeK >= 0 && cubeK < laserCloudDepth)
{
	int cubeInd = cubeI + laserCloudWidth * cubeJ + laserCloudWidth * laserCloudHeight * cubeK;
	laserCloudSurfArray[cubeInd]->push_back(pointSel);
}
}

其中 pointAssociateToMap()

// 点云转移到Map坐标下
void pointAssociateToMap(PointType const *const pi, PointType *const po)
{
    Eigen::Vector3d point_curr(pi->x, pi->y, pi->z);
	// Twi = Two*Toi
	Eigen::Vector3d point_w = q_w_curr * point_curr + t_w_curr;
	po->x = point_w.x();
	po->y = point_w.y();
	po->z = point_w.z();
	po->intensity = pi->intensity;
	//po->intensity = 1.0;
}

当前点云添加后，再对局部地图进行一次滤波，取出多余重复的点。

// laserCloudValidNum 是局部地图中子cube的数量    对局部地图进行滤波 
for (int i = 0; i < laserCloudValidNum; i++)
{   // 局部Map的索引
	int ind = laserCloudValidInd[i];
	
	pcl::PointCloud<PointType>::Ptr tmpCorner(new pcl::PointCloud<PointType>());
	downSizeFilterCorner.setInputCloud(laserCloudCornerArray[ind]);
	downSizeFilterCorner.filter(*tmpCorner);
	laserCloudCornerArray[ind] = tmpCorner;
	
	pcl::PointCloud<PointType>::Ptr tmpSurf(new pcl::PointCloud<PointType>());
	downSizeFilterSurf.setInputCloud(laserCloudSurfArray[ind]);
	downSizeFilterSurf.filter(*tmpSurf);
	laserCloudSurfArray[ind] = tmpSurf;
}

至此建图部分就完成啦，不过还有一些可视化的内容要完成，不然就啥也看不到。。
首先，是每5帧就发布一次局部地图，也就是当前激光位置附近125个cube组成地图。
发布者是 pubLaserCloudSurround，发布的话题为 “/laser_cloud_surround”

// publish surround map for every 5 frame
if (frameCount % 5 == 0)
{
	laserCloudSurround->clear();
	// 构造当前位置附近的局部地图    laserCloudSurroundNum也是储存局部地图cube的个数  
	for (int i = 0; i < laserCloudSurroundNum; i++)
	{
		int ind = laserCloudSurroundInd[i];
		*laserCloudSurround += *laserCloudCornerArray[ind];
		*laserCloudSurround += *laserCloudSurfArray[ind];
	}

	sensor_msgs::PointCloud2 laserCloudSurround3;
	pcl::toROSMsg(*laserCloudSurround, laserCloudSurround3);
	laserCloudSurround3.header.stamp = ros::Time().fromSec(timeLaserOdometry);
	laserCloudSurround3.header.frame_id = "/camera_init";
	pubLaserCloudSurround.publish(laserCloudSurround3);
}

每隔20帧，输出一次全局地图
发布者为 pubLaserCloudMap ，发布话题为 /laser_cloud_map 。

// 每20帧  输出全局地图
if (frameCount % 20 == 0)
{
	pcl::PointCloud<PointType> laserCloudMap;
	// 4851是地图包含的cube的最大数量
	for (int i = 0; i < 4851; i++)
	{
		laserCloudMap += *laserCloudCornerArray[i];
		laserCloudMap += *laserCloudSurfArray[i];
	}
	sensor_msgs::PointCloud2 laserCloudMsg;
	pcl::toROSMsg(laserCloudMap, laserCloudMsg);
	laserCloudMsg.header.stamp = ros::Time().fromSec(timeLaserOdometry);
	laserCloudMsg.header.frame_id = "/camera_init";
	pubLaserCloudMap.publish(laserCloudMsg);
}

此外还需要发布实时的激光点云，也就是下面图中五颜六色的点
发布者 pubLaserCloudFullRes，话题为 “/velodyne_cloud_registered” 。

//发布实时的激光点云
int laserCloudFullResNum = laserCloudFullRes->points.size();
// 滤波后的激光点转到Map系中  
for (int i = 0; i < laserCloudFullResNum; i++)
{
   pointAssociateToMap(&laserCloudFullRes->points[i], &laserCloudFullRes->points[i]);
}
sensor_msgs::PointCloud2 laserCloudFullRes3;
pcl::toROSMsg(*laserCloudFullRes, laserCloudFullRes3);
laserCloudFullRes3.header.stamp = ros::Time().fromSec(timeLaserOdometry);
laserCloudFullRes3.header.frame_id = "/camera_init";
pubLaserCloudFullRes.publish(laserCloudFullRes3);

在这里插入图片描述

SLAM||DUNK

关注

12
点赞
踩
43

收藏

觉得还不错? 一键收藏
16
评论
ALOAM mapping

首先将odometryBuf中接受到的里程计数据保存为q_wodom_curr、t_wodom_curr// 获取里程计的数据放置与 q_wodom_curr t_wodom_currq_wodom_curr.x() = odometryBuf.front()->pose.pose.orientation.x;q_wodom_curr.y() = odometryBuf.front()...
复制链接

扫一扫