对应节点：/alaserMapping
功能：基于前述的corner_less_sharp特征点和surf_less_flat特征点，进行帧与submap的点云特征配准，对Odometry线程计算的位姿进行finetune
1原理
利用激光雷达数据进行机器人的位姿估计。具体来说，它通过订阅激光雷达的不同类型点云话题，获取尖锐角点、不尖锐角点、平坦平面和不平坦平面等信息。然后，对这些点云数据进行处理和关联，利用 Ceres 库进行优化求解，估计机器人的位姿变换。最后，将位姿信息发布到 ROS 话题中，供其他节点使用。
2实现
-
特征点提取：从当前Scan中提取特征点，这些特征点可以是显著的边线点或平面点，它们在环境中易于识别和稳定。
-
最近邻搜索：对于每个特征点，在全局地图cube中查找其最近的五个特征点作为参考点。这通常通过高效的最近邻搜索算法（如KD树）实现。
-
构建匹配模型：
边线点：以五个最近邻点的均值位置为中心，使用它们的方向信息（可能通过 PCA或其他方法计算得到的主方向）定义一个参考直线。目标是最小化当前边线点到这条直线的距离，从而构建一个非线性优化问题。
平面点：对于平面点，利用五个最近邻点的法向量（也可通过PCA或其他方法估计）来定义一个参考平面。目标是使当前平面点在法线方向上与这五个点的平均平面的距离最小，同样构建一个非线性优化问题。 -
非线性优化：使用适当的优化算法（如Levenberg-Marquardt算法、高斯-牛顿法或梯度下降法等）求解上述非线性优化问题，以找到使得代价函数（即特征点到参考线/面的距离平方和）最小的LiDAR位姿。
-
位姿更新：根据优化结果更新LiDAR的位姿，这可能涉及旋转和平移的调整。
// 主处理线程
void process()
{
while(1)
{
// 确保需要的buffer里都有值
while (!cornerLastBuf.empty() && !surfLastBuf.empty() &&
!fullResBuf.empty() && !odometryBuf.empty())
{
mBuf.lock();
// 以cornerLastBuf为基准，把时间戳小于其的全部pop出去
while (!odometryBuf.empty() && odometryBuf.front()->header.stamp.toSec() < cornerLastBuf.front()->header.stamp.toSec())
odometryBuf.pop();
// 如果里程计信息为空跳出本次循环
if (odometryBuf.empty())
{
mBuf.unlock();
break;
}
// 如果面特征信息不为空，面特征时间戳小于特征时间戳则取出面特征数据
while (!surfLastBuf.empty() && surfLastBuf.front()->header.stamp.toSec() < cornerLastBuf.front()->header.stamp.toSec())
surfLastBuf.pop();
// 如果面特征信息为空跳出本次循环
if (surfLastBuf.empty())
{
mBuf.unlock();
break;
}
// 如果全部点信息不为空，全部点云时间戳小于角特征时间戳则取出全部点云信息
while (!fullResBuf.empty() && fullResBuf.front()->header.stamp.toSec() < cornerLastBuf.front()->header.stamp.toSec())
fullResBuf.pop();
// 全部点云信息为空则跳出
if (fullResBuf.empty())
{
mBuf.unlock();
break;
}
// 记录时间戳
timeLaserCloudCornerLast = cornerLastBuf.front()->header.stamp.toSec();
timeLaserCloudSurfLast = surfLastBuf.front()->header.stamp.toSec();
timeLaserCloudFullRes = fullResBuf.front()->header.stamp.toSec();
timeLaserOdometry = odometryBuf.front()->header.stamp.toSec();
// 原则上取出来的时间戳都是一样的，如果不一定说明有问题了
if (timeLaserCloudCornerLast != timeLaserOdometry ||
timeLaserCloudSurfLast != timeLaserOdometry ||
timeLaserCloudFullRes != timeLaserOdometry)
{
printf("time corner %f surf %f full %f odom %f \n", timeLaserCloudCornerLast, timeLaserCloudSurfLast, timeLaserCloudFullRes, timeLaserOdometry);
printf("unsync messeage!");
mBuf.unlock();
break;
}
// 点云全部转成pcl的数据格式
// 清空上次全部点云，并接收新的
laserCloudCornerLast->clear();
pcl::fromROSMsg(*cornerLastBuf.front(), *laserCloudCornerLast);
cornerLastBuf.pop();
laserCloudSurfLast->clear();
pcl::fromROSMsg(*surfLastBuf.front(), *laserCloudSurfLast);
surfLastBuf.pop();
laserCloudFullRes->clear();
pcl::fromROSMsg(*fullResBuf.front(), *laserCloudFullRes);
fullResBuf.pop();
// lidar odom的结果转成eigen数据格式
// 接收里程计坐标系下的四元数与位移
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();
// 角特征不为空，堆入角特征，输出目前运行实时
// 考虑到实时性，就把队列里其他的都pop出去，不然可能出现处理延时的情况
while(!cornerLastBuf.empty())
{
cornerLastBuf.pop();
printf("drop lidar frame in mapping for real time performance \n");
}
mBuf.unlock();
TicToc t_whole;
// 根据前端结果，得到后端的一个初始估计值
transformAssociateToMap();
TicToc t_shift;
// 根据初始估计值计算寻找当前位姿在地图中的索引，一个各自边长是50m
// 后端的地图本质上是一个以当前点为中心，一个珊格地图
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;
// 如果小于25就向下去整，相当于四舍五入的一个过程
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--;
// 如果当前珊格索引小于3,就说明当前点快接近地图边界了，需要进行调整，相当于地图整体往x正方向移动
while (centerCubeI < 3)
{
for (int j = 0; j < laserCloudHeight; j++)
{
for (int k = 0; k < laserCloudDepth; k++)
{
int i = laserCloudWidth - 1;
// 从x最大值开始
pcl::PointCloud<PointType>::Ptr laserCloudCubeCornerPointer =
laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k];
pcl::PointCloud<PointType>::Ptr laserCloudCubeSurfPointer =
laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k];
// 整体右移
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] =
laserCloudCubeCornerPointer;
laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =
laserCloudCubeSurfPointer;
// 该点云清零，由于是指针操作，相当于最左边的格子清空了
laserCloudCubeCornerPointer->clear();
laserCloudCubeSurfPointer->clear();
}
}
// 索引右移
centerCubeI++;
laserCloudCenWidth++;
}
// 同理x如果抵达右边界，就整体左移
while (centerCubeI >= laserCloudWidth - 3)
{
for (int j = 0; j < laserCloudHeight; j++)
{
for (int k = 0; k < laserCloudDepth; k++)
{
int i = 0;
pcl::PointCloud<PointType>::Ptr laserCloudCubeCornerPointer =
laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k];
pcl::PointCloud<PointType>::Ptr laserCloudCubeSurfPointer =
laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k];
// 整体左移
for (; i < laserCloudWidth - 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];
}
laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =
laserCloudCubeCornerPointer;
laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =
laserCloudCubeSurfPointer;
laserCloudCubeCornerPointer->clear();
laserCloudCubeSurfPointer->clear();
}
}
centerCubeI--;
laserCloudCenWidth--;
}
// y和z的操作同理
while (centerCubeJ < 3)
{
for (int i = 0; i < laserCloudWidth; i++)
{
for (int k = 0; k < laserCloudDepth; k++)
{
int j = laserCloudHeight - 1;
pcl::PointCloud<PointType>::Ptr laserCloudCubeCornerPointer =
laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k];
pcl::PointCloud<PointType>::Ptr laserCloudCubeSurfPointer =
laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k];
for (; j >= 1; j--)
{
laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =
laserCloudCornerArray[i + laserCloudWidth * (j - 1) + laserCloudWidth * laserCloudHeight * k];
laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =
laserCloudSurfArray[i + laserCloudWidth * (j - 1) + laserCloudWidth * laserCloudHeight * k];
}
laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =
laserCloudCubeCornerPointer;
laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =
laserCloudCubeSurfPointer;
laserCloudCubeCornerPointer->clear();
laserCloudCubeSurfPointer->clear();
}
}
centerCubeJ++;
laserCloudCenHeight++;
}
while (centerCubeJ >= laserCloudHeight - 3)
{
for (int i = 0; i < laserCloudWidth; i++)
{
for (int k = 0; k < laserCloudDepth; k++)
{
int j = 0;
pcl::PointCloud<PointType>::Ptr laserCloudCubeCornerPointer =
laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k];
pcl::PointCloud<PointType>::Ptr laserCloudCubeSurfPointer =
laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k];
for (; j < laserCloudHeight - 1; j++)
{
laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =
laserCloudCornerArray[i + laserCloudWidth * (j + 1) + laserCloudWidth * laserCloudHeight * k];
laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =
laserCloudSurfArray[i + laserCloudWidth * (j + 1) + laserCloudWidth * laserCloudHeight * k];
}
laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =
laserCloudCubeCornerPointer;
laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =
laserCloudCubeSurfPointer;
laserCloudCubeCornerPointer->clear();
laserCloudCubeSurfPointer->clear();
}
}
centerCubeJ--;
laserCloudCenHeight--;
}
while (centerCubeK < 3)
{
for (int i = 0; i < laserCloudWidth; i++)
{
for (int j = 0; j < laserCloudHeight; j++)
{
int k = laserCloudDepth - 1;
pcl::PointCloud<PointType>::Ptr laserCloudCubeCornerPointer =
laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k];
pcl::PointCloud<PointType>::Ptr laserCloudCubeSurfPointer =
laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k];
for (; k >= 1; k--)
{
laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =
laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * (k - 1)];
laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =
laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * (k - 1)];
}
laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =
laserCloudCubeCornerPointer;
laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =
laserCloudCubeSurfPointer;
laserCloudCubeCornerPointer->clear();
laserCloudCubeSurfPointer->clear();
}
}
centerCubeK++;
laserCloudCenDepth++;
}
while (centerCubeK >= laserCloudDepth - 3)
{
for (int i = 0; i < laserCloudWidth; i++)
{
for (int j = 0; j < laserCloudHeight; j++)
{
int k = 0;
pcl::PointCloud<PointType>::Ptr laserCloudCubeCornerPointer =
laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k];
pcl::PointCloud<PointType>::Ptr laserCloudCubeSurfPointer =
laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k];
for (; k < laserCloudDepth - 1; k++)
{
laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =
laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * (k + 1)];
laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =
laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * (k + 1)];
}
laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =
laserCloudCubeCornerPointer;
laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =
laserCloudCubeSurfPointer;
laserCloudCubeCornerPointer->clear();
laserCloudCubeSurfPointer->clear();
}
}
centerCubeK--;
laserCloudCenDepth--;
}
// 以上操作相当于维护了一个局部地图，保证当前帧不在这个局部地图的边缘，这样才可以从地图中获取足够的约束
int laserCloudValidNum = 0;
int laserCloudSurroundNum = 0;
// 从当前格子为中心，选出地图中一定范围的点云
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)
{
// 把各自的索引记录下来
laserCloudValidInd[laserCloudValidNum] = i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k;
laserCloudValidNum++;
laserCloudSurroundInd[laserCloudSurroundNum] = i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k;
laserCloudSurroundNum++;
}
}
}
}
laserCloudCornerFromMap->clear();
laserCloudSurfFromMap->clear();
// 开始构建用来这一帧优化的小的局部地图
for (int i = 0; i < laserCloudValidNum; i++)
{
*laserCloudCornerFromMap += *laserCloudCornerArray[laserCloudValidInd[i]];
*laserCloudSurfFromMap += *laserCloudSurfArray[laserCloudValidInd[i]];
}
int laserCloudCornerFromMapNum = laserCloudCornerFromMap->points.size();
int laserCloudSurfFromMapNum = laserCloudSurfFromMap->points.size();
// 为了减少运算量，对点云进行下采样
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();
printf("map prepare time %f ms\n", t_shift.toc());
printf("map corner num %d surf num %d \n", laserCloudCornerFromMapNum, laserCloudSurfFromMapNum);
// 最终的有效点云数目进行判断
if (laserCloudCornerFromMapNum > 10 && laserCloudSurfFromMapNum > 50)
{
TicToc t_opt;
TicToc t_tree;
// 送入kdtree便于最近邻搜索
kdtreeCornerFromMap->setInputCloud(laserCloudCornerFromMap);
kdtreeSurfFromMap->setInputCloud(laserCloudSurfFromMap);
printf("build tree time %f ms \n", t_tree.toc());
// 建立对应关系的迭代次数不超2次
for (int iterCount = 0; iterCount < 2; iterCount++)
{
//ceres::LossFunction *loss_function = NULL;
// 建立ceres问题
ceres::LossFunction *loss_function = new ceres::HuberLoss(0.1);
ceres::LocalParameterization *q_parameterization =
new ceres::EigenQuaternionParameterization();
ceres::Problem::Options problem_options;
ceres::Problem problem(problem_options);
problem.AddParameterBlock(parameters, 4, q_parameterization);
problem.AddParameterBlock(parameters + 4, 3);
TicToc t_data;
int corner_num = 0;
// 构建角点相关的约束
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);
// 地图中寻找和该点最近的5个点
kdtreeCornerFromMap->nearestKSearch(pointSel, 5, pointSearchInd, pointSearchSqDis);
//如果五个点中最远的那个还小于1米，则求解协方差矩阵
if (pointSearchSqDis[4] < 1.0)
{
std::vector<Eigen::Vector3d> nearCorners;
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);
}
// 计算这五个点的均值
center = center / 5.0;
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);
// 根据特征值分解情况看看是不是真正的线特征
// 特征向量就是线特征的方向
Eigen::Vector3d unit_direction = saes.eigenvectors().col(2);
Eigen::Vector3d curr_point(pointOri.x, pointOri.y, pointOri.z);
// 最大特征值大于次大特征值的3倍认为是线特征
if (saes.eigenvalues()[2] > 3 * saes.eigenvalues()[1])
{
Eigen::Vector3d point_on_line = center;
Eigen::Vector3d point_a, point_b;
// 根据拟合出来的线特征方向，以平均点为中心构建两个虚拟点
point_a = 0.1 * unit_direction + point_on_line;
point_b = -0.1 * unit_direction + point_on_line;
// 构建约束，和lidar odom约束一致
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++;
}
}
/*
else if(pointSearchSqDis[4] < 0.01 * sqrtDis)
{
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;
}
center = center / 5.0;
Eigen::Vector3d curr_point(pointOri.x, pointOri.y, pointOri.z);
ceres::CostFunction *cost_function = LidarDistanceFactor::Create(curr_point, center);
problem.AddResidualBlock(cost_function, loss_function, parameters, parameters + 4);
}
*/
}
int surf_num = 0;
// 构建面点约束
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);
kdtreeSurfFromMap->nearestKSearch(pointSel, 5, pointSearchInd, pointSearchSqDis);
Eigen::Matrix<double, 5, 3> matA0;
Eigen::Matrix<double, 5, 1> matB0 = -1 * Eigen::Matrix<double, 5, 1>::Ones();
// 构建平面方程Ax + By +Cz + 1 = 0
// 通过构建一个超定方程来求解这个平面方程
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;
//printf(" pts %f %f %f ", matA0(j, 0), matA0(j, 1), matA0(j, 2));
}
// find the norm of plane
// 调用eigen接口求解该方程，解就是这个平面的法向量
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
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++;
}
}
/*
else if(pointSearchSqDis[4] < 0.01 * sqrtDis)
{
Eigen::Vector3d center(0, 0, 0);
for (int j = 0; j < 5; j++)
{
Eigen::Vector3d tmp(laserCloudSurfFromMap->points[pointSearchInd[j]].x,
laserCloudSurfFromMap->points[pointSearchInd[j]].y,
laserCloudSurfFromMap->points[pointSearchInd[j]].z);
center = center + tmp;
}
center = center / 5.0;
Eigen::Vector3d curr_point(pointOri.x, pointOri.y, pointOri.z);
ceres::CostFunction *cost_function = LidarDistanceFactor::Create(curr_point, center);
problem.AddResidualBlock(cost_function, loss_function, parameters, parameters + 4);
}
*/
}
//printf("corner num %d used corner num %d \n", laserCloudCornerStackNum, corner_num);
//printf("surf num %d used surf num %d \n", laserCloudSurfStackNum, surf_num);
printf("mapping data assosiation time %f ms \n", t_data.toc());
// 调用ceres求解
TicToc t_solver;
ceres::Solver::Options options;
options.linear_solver_type = ceres::DENSE_QR;
options.max_num_iterations = 4;
options.minimizer_progress_to_stdout = false;
options.check_gradients = false;
options.gradient_check_relative_precision = 1e-4;
ceres::Solver::Summary summary;
ceres::Solve(options, &problem, &summary);
printf("mapping solver time %f ms \n", t_solver.toc());
//printf("time %f \n", timeLaserOdometry);
//printf("corner factor num %d surf factor num %d\n", corner_num, surf_num);
//printf("result q %f %f %f %f result t %f %f %f\n", parameters[3], parameters[0], parameters[1], parameters[2],
// parameters[4], parameters[5], parameters[6]);
}
printf("mapping optimization time %f \n", t_opt.toc());
}
else
{
ROS_WARN("time Map corner and surf num are not enough");
}
transformUpdate();
TicToc t_add;
// 将优化后的当前帧角点加到局部地图中去
for (int i = 0; i < laserCloudCornerStackNum; i++)
{
// 该点根据位姿投到地图坐标系
pointAssociateToMap(&laserCloudCornerStack->points[i], &pointSel);
// 算出这个点所在的格子的索引
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)
{
// 根据xyz的索引计算在一位数组中的索引
int cubeInd = cubeI + laserCloudWidth * cubeJ + laserCloudWidth * laserCloudHeight * cubeK;
laserCloudCornerArray[cubeInd]->push_back(pointSel);
}
}
// 面点也做同样的处理
for (int i = 0; i < laserCloudSurfStackNum; i++)
{
pointAssociateToMap(&laserCloudSurfStack->points[i], &pointSel);
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);
}
}
printf("add points time %f ms\n", t_add.toc());
TicToc t_filter;
// 把当前帧涉及到的局部地图的珊格做一个下采样
for (int i = 0; i < laserCloudValidNum; i++)
{
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;
}
printf("filter time %f ms \n", t_filter.toc());
TicToc t_pub;
//publish surround map for every 5 frame
// 每隔5帧对外发布一下
if (frameCount % 5 == 0)
{
laserCloudSurround->clear();
// 把该当前帧相关的局部地图发布出去
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帧发布全量的局部地图
if (frameCount % 20 == 0)
{
pcl::PointCloud<PointType> laserCloudMap;
// 21 × 21 × 11 = 4851
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);
}
int laserCloudFullResNum = laserCloudFullRes->points.size();
// 把当前帧发布出去
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);
printf("mapping pub time %f ms \n", t_pub.toc());
printf("whole mapping time %f ms +++++\n", t_whole.toc());
// 发布当前位姿
nav_msgs::Odometry odomAftMapped;
odomAftMapped.header.frame_id = "/camera_init";
odomAftMapped.child_frame_id = "/aft_mapped";
odomAftMapped.header.stamp = ros::Time().fromSec(timeLaserOdometry);
odomAftMapped.pose.pose.orientation.x = q_w_curr.x();
odomAftMapped.pose.pose.orientation.y = q_w_curr.y();
odomAftMapped.pose.pose.orientation.z = q_w_curr.z();
odomAftMapped.pose.pose.orientation.w = q_w_curr.w();
odomAftMapped.pose.pose.position.x = t_w_curr.x();
odomAftMapped.pose.pose.position.y = t_w_curr.y();
odomAftMapped.pose.pose.position.z = t_w_curr.z();
pubOdomAftMapped.publish(odomAftMapped);
// 发布当前轨迹
geometry_msgs::PoseStamped laserAfterMappedPose;
laserAfterMappedPose.header = odomAftMapped.header;
laserAfterMappedPose.pose = odomAftMapped.pose.pose;
laserAfterMappedPath.header.stamp = odomAftMapped.header.stamp;
laserAfterMappedPath.header.frame_id = "/camera_init";
laserAfterMappedPath.poses.push_back(laserAfterMappedPose);
pubLaserAfterMappedPath.publish(laserAfterMappedPath);
// 发布tf
static tf::TransformBroadcaster br;
tf::Transform transform;
tf::Quaternion q;
transform.setOrigin(tf::Vector3(t_w_curr(0),
t_w_curr(1),
t_w_curr(2)));
q.setW(q_w_curr.w());
q.setX(q_w_curr.x());
q.setY(q_w_curr.y());
q.setZ(q_w_curr.z());
transform.setRotation(q);
br.sendTransform(tf::StampedTransform(transform, odomAftMapped.header.stamp, "/camera_init", "/aft_mapped"));
frameCount++;
}
std::chrono::milliseconds dura(2);
std::this_thread::sleep_for(dura);
}
}
扫一扫
2950
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
