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);