3D 激光SLAM ->loam_velodyne论文与代码解析Lidar Odometry and Mapping
3D SLAM ->loam_velodyne论文与代码解析
一直做2D 的激光SLAM，最近终于接触到3D的了，想彻底的看透一个开源的3D 激光SLAM，选择了Loam_velodyne从论文到代码彻底看一下。论文：LOAM Lidar Odometry and Mapping in Real-time。
- 3D SLAM -loam_velodyne论文与代码解析
- 结构关系订阅与发布节点关系
- 具体部分的实现思想
- 特征点提取
- 特征点关联
- 运动估计
- laserMappingcpp代码阅读解析
- 迭代计算的流程
- 具体部分说明
结构关系：订阅与发布节点关系
其中最终输出构建3D地图的点云是/laser_cloud_surround;需要更改的是最左侧输入、即订阅的/velodyne_points的topic名称。Remap成为需要订阅的主题。从图中可以看出scanRegister的工作是将源数据处理成/laser_cloud_sharp & /laser_cloud_flat等等，这些点云是具有特征信息的点云。
具体的操作为在论文中给出这样一幅图：
由此可以知道laserOdometry从特征点中估计运动（10HZ），然后整合数据发送给laserMapping（1HZ），在这个过程中，算是一种数据的降采样吧。也是为了保证运行online的时效性，以及节省内存占用的大小。
具体部分的实现思想
相对于其它直接匹配两个点云，loam是通过提取特征点匹配后计算坐标变换。
特征点提取
一次扫描的点通过曲率（c）值来分类，特征点是c的最大值点–边缘点；特征点是c的最小值点–平面点。为了使特征点均匀的分布在环境中，将一次扫描划分为4个独立的子区域。每个子区域最多提供2个边缘点和4个平面点，同时特征点的选择需要满足阈值的要求。
同时需要考虑特征点选择中的一些越是：比如如果一个点被其它特征点包围，那么就不被选择；以及一些点满足c的要求不过是不稳定的特征点，比如断点等。不稳定的点如下图：
代码部分:主要是在scanRegistration.cpp这个文件的任务是提取特征点，并且发布出去。
代码具体的阅读理解：
① 论文中存储每个点的曲率用的是数组，因此需要考虑数组的大小：
float cloudCurvature[80000];
int cloudSortInd[80000];
int cloudNeighborPicked[80000];
int cloudLabel[80000];前言：N_SCANS是将3D的激光点云按照激光的接受器做了个划分，比如N_SCANS是16表明是16线的激光（程序中的默认值，作者用过velodyne16）。
对于一堆点云并不是像LaserScan（二维的数据结构）那样按照角度给出个距离值，保证每次的扫描都能够有相同大小的数据量。PointCloud2接受到的点云的大小在变化，因此在数据到达需要一些运算来判断点的一些特性。例如下面这段通过计算pitch角度值将点划分到不同的“线”中。代码坑，point.x=laserCloudIn.points[i].y；做个迷惑的赋值。Pitch=atan(z/(x2+y2));和代码中只是形式的不同。
PointType point;
std::vector算出scan ID后，又将intensity属性充分利用起来，整数部分：scan ID，小数部分：每个点扫描的时间（在startOri->endOri按照均匀划分）
float relTime = (ori - startOri) / (endOri - startOri);
point.intensity = scanID + scanPeriod * relTime;然后，将点压入到每个线中，同时更新总的点云laserCloud。
……
laserCloudScans[scanID].push_back(point);
}
cloudSize = count;
pcl::PointCloud含义说明：cloudCurvature是存储每个点的曲率，由上面的laserCloud+=……，可以知道的是这个时候的点云是按照线的数据方式存储的，线的数量自己定义。作者给出的计算公式：
float diffX = laserCloud->points[i - 5].x + laserCloud->points[i - 4].x
+ laserCloud->points[i - 3].x + laserCloud->points[i - 2].x
+ laserCloud->points[i - 1].x - 10 * laserCloud->points[i].x
+ laserCloud->points[i + 1].x + laserCloud->points[i + 2].x
+ laserCloud->points[i + 3].x + laserCloud->points[i + 4].x
+ laserCloud->points[i + 5].x;
float diffY = laserCloud->points[i - 5].y + laserCloud->points[i - 4].y
+ laserCloud->points[i - 3].y + laserCloud->points[i - 2].y
+ laserCloud->points[i - 1].y - 10 * laserCloud->points[i].y
+ laserCloud->points[i + 1].y + laserCloud->points[i + 2].y
+ laserCloud->points[i + 3].y + laserCloud->points[i + 4].y
+ laserCloud->points[i + 5].y;
float diffZ = laserCloud->points[i - 5].z + laserCloud->points[i - 4].z
+ laserCloud->points[i - 3].z + laserCloud->points[i - 2].z
+ laserCloud->points[i - 1].z - 10 * laserCloud->points[i].z
+ laserCloud->points[i + 1].z + laserCloud->points[i + 2].z
+ laserCloud->points[i + 3].z + laserCloud->points[i + 4].z
+ laserCloud->points[i + 5].z;
//jc : cloudCurvature calculate
cloudCurvature[i] = diffX * diffX + diffY * diffY + diffZ * diffZ;因为是按照线的序列存储，因此接下来能够得到起始和终止的index；在这里滤除前五个和后五个。
if (int(laserCloud->points[i].intensity) != scanCount) {
scanCount = int(laserCloud->points[i].intensity);
//N_SCANS is 16
if (scanCount > 0 && scanCount < N_SCANS) {
//std::vectorcloudSortInd是对曲率排序得到的序列：这里作者将每一线划分为等间距的6段分别处理，在每一段升序排列。 变量说明sp startPoint；ep endPoint.
for (int i = 0; i < N_SCANS; i++) {
pcl::PointCloud::Ptr surfPointsLessFlatScan(new pcl::PointCloud);
for (int j = 0; j < 6; j++) {
int sp = (scanStartInd[i] * (6 - j) + scanEndInd[i] * j) / 6;
int ep = (scanStartInd[i] * (5 - j) + scanEndInd[i] * (j + 1)) / 6 - 1;
for (int k = sp + 1; k <= ep; k++) {
for (int l = k; l >= sp + 1; l--) {
if (cloudCurvature[cloudSortInd[l]] < cloudCurvature[cloudSortInd[l - 1]]) {
int temp = cloudSortInd[l - 1];
cloudSortInd[l - 1] = cloudSortInd[l];
cloudSortInd[l] = temp;
}
}
} cloudNeighborPicked是考虑一个特征点的周围不能再设置成特征点约束的判断标志位。
cloudLabel按照如下计算，选择最大曲率的作为sharp和lessSharp，因为是按照升序排列的cloudSortInd，因此从ep->sp逐个选择：
int largestPickedNum = 0;
for (int k = ep; k >= sp; k--) {
int ind = cloudSortInd[k];
// jc : in cloudNeighborPicked array 1 is nerighbor and 0 is alone
// jc : if it 's alone and the curudCurvature is bigger then 0.1,
if (cloudNeighborPicked[ind] == 0 &&
cloudCurvature[ind] > 0.1) {
largestPickedNum++;
if (largestPickedNum <= 2)
{
cloudLabel[ind] = 2; // jc : what is the difference between 2 and 1
cornerPointsSharp.push_back(laserCloud->points[ind]);
cornerPointsLessSharp.push_back(laserCloud->points[ind]);
}
else if (largestPickedNum <= 20) {
cloudLabel[ind] = 1;
cornerPointsLessSharp.push_back(laserCloud->points[ind]);
}
else {
break;
}
cloudNeighborPicked[ind] = 1;同时如果一个点添加到了特征点中（sharp以及lessSharp），周围的点不是特征点了；通过cloudNeighborPicked做标志来判断：接着上面的截图（一个for循环内）：
② 关于scanRegistration.cpp中的imu
需要发布一个topic /imu_trans类型是sensor_msgs::PointCloud2而不是sensor_msgs::Imu
作者用4个pcl::Point XYZ类型的数组来存储IMU的信息，作者pcl用的6，坑神走起。
pcl::PointCloud imuTrans(4, 1);
imuTrans.points[0].x = imuPitchStart;
imuTrans.points[0].y = imuYawStart;
imuTrans.points[0].z = imuRollStart;
imuTrans.points[1].x = imuPitchCur;
imuTrans.points[1].y = imuYawCur;
imuTrans.points[1].z = imuRollCur;
imuTrans.points[2].x = imuShiftFromStartXCur;
imuTrans.points[2].y = imuShiftFromStartYCur;
imuTrans.points[2].z = imuShiftFromStartZCur;
imuTrans.points[3].x = imuVeloFromStartXCur;
imuTrans.points[3].y = imuVeloFromStartYCur;
imuTrans.points[3].z = imuVeloFromStartZCur;
sensor_msgs::PointCloud2 imuTransMsg;
pcl::toROSMsg(imuTrans, imuTransMsg);
imuTransMsg.header.stamp = laserCloudMsg->header.stamp;
imuTransMsg.header.frame_id = "/camera";
pubImuTrans.publish(imuTransMsg); 特征点关联
特征点的关联就是计算历程数据odometry推演，将t时刻的点云和t+1时刻的点云关联起来的一种方法（如果有imu或者里程计可以先进行一个初步的变换，作为初始信息用于匹配）。在t时刻得到的边缘点生成的线和t+1时刻提取的边缘点对应起来，对于t+1时刻的点，在t时刻找到最近的两个点（同时满足两点的线段有意义）；同样的考虑平面特征点。t时刻的特征点通过3D的KD-tree建立快速索引。计算这一时刻对应上一时刻最近的两个点，求点到线的距离；同样的考虑点到面的距离。
*代码部分： laserOdometry.cpp的实现分析
首先将上页的坑，作者补上了，转换成可以看的数据
void imuTransHandler(const sensor_msgs::PointCloud2ConstPtr& imuTrans2)
{
timeImuTrans = imuTrans2->header.stamp.toSec();
imuTrans->clear();
pcl::fromROSMsg(*imuTrans2, *imuTrans);
imuPitchStart = imuTrans->points[0].x;
imuYawStart = imuTrans->points[0].y;
imuRollStart = imuTrans->points[0].z;
imuPitchLast = imuTrans->points[1].x;
imuYawLast = imuTrans->points[1].y;
imuRollLast = imuTrans->points[1].z;
imuShiftFromStartX = imuTrans->points[2].x;
imuShiftFromStartY = imuTrans->points[2].y;
imuShiftFromStartZ = imuTrans->points[2].z;
imuVeloFromStartX = imuTrans->points[3].x;
imuVeloFromStartY = imuTrans->points[3].y;
imuVeloFromStartZ = imuTrans->points[3].z;
newImuTrans = true;
}以及数组的定义：其中pointSelCornerInd和pointSelSurfInd并没有用到……
剩下的pointSearchCornerInd1和pointSearchCornerInd2是存储最近的两个点的索引（corner） 。 另一组自然是用来存储最近点的索引（平面Surf）
判断点的个数选择处理方式，如果上一时刻的点的个数（边缘点个数大于10，平面特征点大于100）：
在迭代的过程中计算transform；iterCound%5==0的意思也就出来了，因为在迭代的过程中最近的点的匹配是随着transform的更新逐渐变化的，因此作者采用了5次迭代（计算量的一种平衡吧，每次更新transform后都更新一次最近匹配计算资源消耗大？）后再计算一次对应的最近点。这样下次通过更新的最近点匹配对来完成新的计算。
在当前帧中遍历上一帧中最近的两个点。作者没有使用nearKSearch函数直接求最近的2个点的原因是，nearKSearch求出的两个点不一定能构成合理的直线。
//jc:功能是为了计算第i个点的最近的两个点（构成线段）
for (int i = 0; i < cornerPointsSharpNum; i++) {
//jc:pointSel个人感觉pointSearch
TransformToStart(&cornerPointsSharp->points[i], &pointSel);
//jc:当iterCount是0的时候也执行。功能是为了计算第i个点的最近的两个点（构成线段）
//pointSearchCornerInd1和pointSearchCornerInd2是用来存储点的索引的数组
if (iterCount % 5 == 0)
{
std::vector<int> indices;
pcl::removeNaNFromPointCloud(*laserCloudCornerLast,*laserCloudCornerLast, indices);
//jc:kdtreeCornerLast由上次的LessSharp的corner点构造
kdtreeCornerLast->nearestKSearch(pointSel, 1, pointSearchInd, pointSearchSqDis);
int closestPointInd = -1, minPointInd2 = -1;
//jc:两桢之间的变换不会太剧烈，25作为阈值
if (pointSearchSqDis[0] < 25) {
closestPointInd = pointSearchInd[0];
//pointSel对应上一桢的那一线？closestPointScan
int closestPointScan = int(laserCloudCornerLast->points[closestPointInd].intensity);
float pointSqDis, minPointSqDis2 = 25;
//jc:找到closestPointInd后找次之的点的索引
//这里作者没有直接nearKSearch找最近的两个点，也为了考虑这两个点要有效的吧，
//下面是不同约束，不能差3线以上，不能距离过大。
for (int j = closestPointInd + 1; j < cornerPointsSharpNum; j++) {
if (int(laserCloudCornerLast->points[j].intensity) > closestPointScan + 2.5) {
break;
}
pointSqDis = (laserCloudCornerLast->points[j].x - pointSel.x) *
(laserCloudCornerLast->points[j].x - pointSel.x) +
(laserCloudCornerLast->points[j].y - pointSel.y) *
(laserCloudCornerLast->points[j].y - pointSel.y) +
(laserCloudCornerLast->points[j].z - pointSel.z) *
(laserCloudCornerLast->points[j].z - pointSel.z);
if (int(laserCloudCornerLast->points[j].intensity) > closestPointScan) {
if (pointSqDis < minPointSqDis2) {
minPointSqDis2 = pointSqDis;
minPointInd2 = j;
}
}
}
for (int j = closestPointInd - 1; j >= 0; j--) {
if (int(laserCloudCornerLast->points[j].intensity) < closestPointScan - 2.5) {
break;
}
pointSqDis = (laserCloudCornerLast->points[j].x - pointSel.x) *
(laserCloudCornerLast->points[j].x - pointSel.x) +
(laserCloudCornerLast->points[j].y - pointSel.y) *
(laserCloudCornerLast->points[j].y - pointSel.y) +
(laserCloudCornerLast->points[j].z - pointSel.z) *
(laserCloudCornerLast->points[j].z - pointSel.z);
if (int(laserCloudCornerLast->points[j].intensity) < closestPointScan) {
if (pointSqDis < minPointSqDis2) {
minPointSqDis2 = pointSqDis;
minPointInd2 = j;
}
}
}
}
pointSearchCornerInd1[i] = closestPointInd;
pointSearchCornerInd2[i] = minPointInd2;
}同理还有遍历当前帧中的Surf点在上一帧找最近的平面在循环
for (int i = 0; i < surfPointsFlatNum; i++) 中完成。。。Surf的思想和corner的大同小异，这里以及下面以corner的对应点计算以及误差计算来表述作者的思想。
运动估计
假设：雷达的运动是连续的。将所有对应到的点求到直线的距离到面的距离之和最短然后按照Levenberg-Marquardt算法迭代计算，得到两帧之间的变换，最后通过累计计算odom。在这里，需要得到的是距离对坐标变换的偏导数。
首先是计算偏导数，这一部分手打出来麻烦，暂时省去，……下面是作者推到偏导数然后得到的表达式，厉害了，具体含义鞋子图片中了……：
迭代更新transform即变换矩阵
cv::transpose(matA, matAt);
matAtA = matAt * matA;
matAtB = matAt * matB;
cv::solve(matAtA, matAtB, matX, cv::DECOMP_QR);
if (iterCount == 0) {
cv::Mat matE(1, 6, CV_32F, cv::Scalar::all(0));
cv::Mat matV(6, 6, CV_32F, cv::Scalar::all(0));
cv::Mat matV2(6, 6, CV_32F, cv::Scalar::all(0));
cv::eigen(matAtA, matE, matV);
matV.copyTo(matV2);
isDegenerate = false;
float eignThre[6] = {10, 10, 10, 10, 10, 10};
for (int i = 5; i >= 0; i--) {
if (matE.at(0, i) < eignThre[i]) {
for (int j = 0; j < 6; j++) {
matV2.at(i, j) = 0;
}
isDegenerate = true;
} else {
break;
}
}
matP = matV.inv() * matV2;
}
if (isDegenerate) {
cv::Mat matX2(6, 1, CV_32F, cv::Scalar::all(0));
matX.copyTo(matX2);
matX = matP * matX2;
}
transform[0] += matX.at(0, 0);
transform[1] += matX.at(1, 0);
transform[2] += matX.at(2, 0);
transform[3] += matX.at(3, 0);
transform[4] += matX.at(4, 0);
transform[5] += matX.at(5, 0); 累计计算总的里程计数据：transformSum；之后会作为/laser_odom_to_init主题发布出去，程序中作者并不喜欢用tf变换，而是节点的订阅。mapping节点订阅。
laserMapping.cpp代码阅读解析
迭代计算的流程
1.从划分好的地图中加载当前激光扫描所在的区域部分
kdtreeCornerFromMap->setInputCloud(laserCloudCornerFromMap);
kdtreeSurfFromMap->setInputCloud(laserCloudSurfFromMap);2.迭代开始for(int iterCount=0;iterCount<10;iterCount++)
laserCloudOri->clear();//迭代中的中间变量
coefSel->clear();3.计算corner点到kdtreeCornerFromMap中提出来的5个最近点的匹配关系。
// pointOri是激光点转换到map坐标系下的表示
//每次迭代更新transformTobeMapped，而pointOri是在transformTobeMapped变换后的点
laserCloudOri->push_back(pointOri);
// coeff的四列分别是距离对(x,y,z)的导数和点到匹配直线的距离。
coeffSel->push_back(coeff);4.计算surf点到kdtreeSurfFromMap中的点形成面的匹配关系
从中取最近的5个点->判断点能否拟合平面(planeValid) -> surf点到面的距离小于阈值5.计算迭代的delta,根据pointOri和coeffSel计算之后更新transformTobeMapped
具体部分说明
1.空间划分，将新获得的corner和surf激光点分别映射到每个立方体laserCloudCornerArray和laserCloudSurfArray
int laserCloudCenWidth = 10;
int laserCloudCenHeight = 5;
int laserCloudCenDepth = 10;
const int laserCloudWidth = 21;
const int laserCloudHeight = 11;
const int laserCloudDepth = 21;
const int laserCloudNum = laserCloudWidth * laserCloudHeight * laserCloudDepth;
for (int i = 0; i < laserCloudNum; i++) {
laserCloudCornerArray[i].reset(new pcl::PointCloud());
laserCloudSurfArray[i].reset(new pcl::PointCloud());
laserCloudCornerArray2[i].reset(new pcl::PointCloud());
laserCloudSurfArray2[i].reset(new pcl::PointCloud());
}
//以surf点为例，加入的方式
for (int i = 0; i < laserCloudSurfStackNum; i++) {
//根据transformTobeMapped将每个点变换到map坐标系下
pointAssociateToMap(&laserCloudSurfStack->points[i], &pointSel);
//根据点在map坐标系的位置，计算点在laserCloudSurfArray中的索引值
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);
}
} 2.匹配对应点
laserCloudCornerFromMap和laserCloudSurfMap分别是从laser_cloud_corner_last和laser_cloud_surf_last
对应在laserCloudCornerArray和laserCloudSurfArray中激活的区域的点的叠加。
// 在已经划分好的空间内搜索最近的5个点
kdtreeCornerFromMap->nearestKSearch(pointSel, 5, pointSearchInd, pointSearchSqDis);
// 如果5个最近点中最远的距离也小于1m，认为是潜在匹配线段
// 构建这五个点的(x,y,z)方向的3*3的协方差矩阵，之后根据特征根来判断是否能拟合成直线。
// 判断的方法是最大的特征根大于次大的特征根3倍。
// 如果使用matlab，
// eig(cov(x,y,z)),其中的x,y,z是点云的各个分量向量。
if (pointSearchSqDis[4] < 1.0) {
float cx = 0;
float cy = 0;
float cz = 0;
for (int j = 0; j < 5; j++) {
cx += laserCloudCornerFromMap->points[pointSearchInd[j]].x;
cy += laserCloudCornerFromMap->points[pointSearchInd[j]].y;
cz += laserCloudCornerFromMap->points[pointSearchInd[j]].z;
}
cx /= 5;
cy /= 5;
cz /= 5;
float a11 = 0;
float a12 = 0;
float a13 = 0;
float a22 = 0;
float a23 = 0;
float a33 = 0;
for (int j = 0; j < 5; j++) {
float ax = laserCloudCornerFromMap->points[pointSearchInd[j]].x - cx;
float ay = laserCloudCornerFromMap->points[pointSearchInd[j]].y - cy;
float az = laserCloudCornerFromMap->points[pointSearchInd[j]].z - cz;
a11 += ax * ax;
a12 += ax * ay;
a13 += ax * az;
a22 += ay * ay;
a23 += ay * az;
a33 += az * az;
}
a11 /= 5;
a12 /= 5;
a13 /= 5;
a22 /= 5;
a23 /= 5;
a33 /= 5;
// 5个点的协方差矩阵
matA1.at<float>(0, 0) = a11;
matA1.at<float>(0, 1) = a12;
matA1.at<float>(0, 2) = a13;
matA1.at<float>(1, 0) = a12;
matA1.at<float>(1, 1) = a22;
matA1.at<float>(1, 2) = a23;
matA1.at<float>(2, 0) = a13;
matA1.at<float>(2, 1) = a23;
matA1.at<float>(2, 2) = a33;
// 特征根matD1，对应的特征向量matV1.
cv::eigen(matA1, matD1, matV1);
if (matD1.at<float>(0, 0) > 3 * matD1.at<float>(0, 1)) {
float x0 = pointSel.x;
float y0 = pointSel.y;
float z0 = pointSel.z;
float x1 = cx + 0.1 * matV1.at<float>(0, 0);
float y1 = cy + 0.1 * matV1.at<float>(0, 1);
float z1 = cz + 0.1 * matV1.at<float>(0, 2);
float x2 = cx - 0.1 * matV1.at<float>(0, 0);
float y2 = cy - 0.1 * matV1.at<float>(0, 1);
float z2 = cz - 0.1 * matV1.at<float>(0, 2);
// 计算(x0,y0,z0)到线段(x1,y1,z1)(x2,y2,z2)的距离。
float a012 = sqrt(((x0 - x1)*(y0 - y2) - (x0 - x2)*(y0 - y1))
* ((x0 - x1)*(y0 - y2) - (x0 - x2)*(y0 - y1))
+ ((x0 - x1)*(z0 - z2) - (x0 - x2)*(z0 - z1))
* ((x0 - x1)*(z0 - z2) - (x0 - x2)*(z0 - z1))
+ ((y0 - y1)*(z0 - z2) - (y0 - y2)*(z0 - z1))
* ((y0 - y1)*(z0 - z2) - (y0 - y2)*(z0 - z1)));
float l12 = sqrt((x1 - x2)*(x1 - x2) + (y1 - y2)*(y1 - y2) + (z1 - z2)*(z1 - z2));
// la=diff(ld2)/diff(x0)，表示点point0到point1和point2直线距离对x0的偏导
float la = ((y1 - y2)*((x0 - x1)*(y0 - y2) - (x0 - x2)*(y0 - y1))
+ (z1 - z2)*((x0 - x1)*(z0 - z2) - (x0 - x2)*(z0 - z1))) / a012 / l12;
float lb = -((x1 - x2)*((x0 - x1)*(y0 - y2) - (x0 - x2)*(y0 - y1))
- (z1 - z2)*((y0 - y1)*(z0 - z2) - (y0 - y2)*(z0 - z1))) / a012 / l12;
float lc = -((x1 - x2)*((x0 - x1)*(z0 - z2) - (x0 - x2)*(z0 - z1))
+ (y1 - y2)*((y0 - y1)*(z0 - z2) - (y0 - y2)*(z0 - z1))) / a012 / l12;
// 点到线段距离公式
float ld2 = a012 / l12;
pointProj = pointSel;
pointProj.x -= la * ld2;
pointProj.y -= lb * ld2;
pointProj.z -= lc * ld2;
float s = 1 - 0.9 * fabs(ld2);
coeff.x = s * la;
coeff.y = s * lb;
coeff.z = s * lc;
coeff.intensity = s * ld2;
if (s > 0.1) {
laserCloudOri->push_back(pointOri);
coeffSel->push_back(coeff);
}
}
}
}3.迭代计算步长
float srx = sin(transformTobeMapped[0]);
float crx = cos(transformTobeMapped[0]);
float sry = sin(transformTobeMapped[1]);
float cry = cos(transformTobeMapped[1]);
float srz = sin(transformTobeMapped[2]);
float crz = cos(transformTobeMapped[2]);
int laserCloudSelNum = laserCloudOri->points.size();
if (laserCloudSelNum < 50) {
continue;
}
// matA 是雅克比矩阵，matAt*matA*matX = matAt*matB;
// 其中matX是步长，（roll , ptich ,yaw,x,y,z）
cv::Mat matA(laserCloudSelNum, 6, CV_32F, cv::Scalar::all(0));
cv::Mat matAt(6, laserCloudSelNum, CV_32F, cv::Scalar::all(0));
cv::Mat matAtA(6, 6, CV_32F, cv::Scalar::all(0));
cv::Mat matB(laserCloudSelNum, 1, CV_32F, cv::Scalar::all(0));
cv::Mat matAtB(6, 1, CV_32F, cv::Scalar::all(0));
cv::Mat matX(6, 1, CV_32F, cv::Scalar::all(0));
for (int i = 0; i < laserCloudSelNum; i++) {
pointOri = laserCloudOri->points[i];
coeff = coeffSel->points[i];
float arx = (crx*sry*srz*pointOri.x + crx*crz*sry*pointOri.y - srx*sry*pointOri.z) * coeff.x
+ (-srx*srz*pointOri.x - crz*srx*pointOri.y - crx*pointOri.z) * coeff.y
+ (crx*cry*srz*pointOri.x + crx*cry*crz*pointOri.y - cry*srx*pointOri.z) * coeff.z;
float ary = ((cry*srx*srz - crz*sry)*pointOri.x
+ (sry*srz + cry*crz*srx)*pointOri.y + crx*cry*pointOri.z) * coeff.x
+ ((-cry*crz - srx*sry*srz)*pointOri.x
+ (cry*srz - crz*srx*sry)*pointOri.y - crx*sry*pointOri.z) * coeff.z;
float arz = ((crz*srx*sry - cry*srz)*pointOri.x + (-cry*crz-srx*sry*srz)*pointOri.y)*coeff.x
+ (crx*crz*pointOri.x - crx*srz*pointOri.y) * coeff.y
+ ((sry*srz + cry*crz*srx)*pointOri.x + (crz*sry-cry*srx*srz)*pointOri.y)*coeff.z;
matA.at(i, 0) = arx;
matA.at(i, 1) = ary;
matA.at(i, 2) = arz;
matA.at(i, 3) = coeff.x;
matA.at(i, 4) = coeff.y;
matA.at(i, 5) = coeff.z;
matB.at(i, 0) = -coeff.intensity;
}
cv::transpose(matA, matAt);
matAtA = matAt * matA;
matAtB = matAt * matB;
cv::solve(matAtA, matAtB, matX, cv::DECOMP_QR);
if (iterCount == 0) {
cv::Mat matE(1, 6, CV_32F, cv::Scalar::all(0));
cv::Mat matV(6, 6, CV_32F, cv::Scalar::all(0));
cv::Mat matV2(6, 6, CV_32F, cv::Scalar::all(0));
cv::eigen(matAtA, matE, matV);
matV.copyTo(matV2);
isDegenerate = false;
float eignThre[6] = {100, 100, 100, 100, 100, 100};
for (int i = 5; i >= 0; i--) {
if (matE.at(0, i) < eignThre[i]) {
for (int j = 0; j < 6; j++) {
matV2.at(i, j) = 0;
}
isDegenerate = true;
} else {
break;
}
}
matP = matV.inv() * matV2;
}
if (isDegenerate) {
cv::Mat matX2(6, 1, CV_32F, cv::Scalar::all(0));
matX.copyTo(matX2);
matX = matP * matX2;
}
transformTobeMapped[0] += matX.at(0, 0);
transformTobeMapped[1] += matX.at(1, 0);
transformTobeMapped[2] += matX.at(2, 0);
transformTobeMapped[3] += matX.at(3, 0);
transformTobeMapped[4] += matX.at(4, 0);
transformTobeMapped[5] += matX.at(5, 0);
float deltaR = sqrt(
pow(rad2deg(matX.at(0, 0)), 2) +
pow(rad2deg(matX.at(1, 0)), 2) +
pow(rad2deg(matX.at(2, 0)), 2));
float deltaT = sqrt(
pow(matX.at(3, 0) * 100, 2) +
pow(matX.at(4, 0) * 100, 2) +
pow(matX.at(5, 0) * 100, 2));
if (deltaR < 0.05 && deltaT < 0.05) {
break;
}
}
// jc : in this function ,update the transformAftMapped
transformUpdate(); 4.发布点云
if (mapFrameCount >= mapFrameNum) {
mapFrameCount = 0;
laserCloudSurround2->clear();
for (int i = 0; i < laserCloudSurroundNum; i++) {
int ind = laserCloudSurroundInd[i];
*laserCloudSurround2 += *laserCloudCornerArray[ind];
*laserCloudSurround2 += *laserCloudSurfArray[ind];
}
laserCloudSurround->clear();
downSizeFilterCorner.setInputCloud(laserCloudSurround2);
downSizeFilterCorner.filter(*laserCloudSurround);
sensor_msgs::PointCloud2 laserCloudSurround3;
pcl::toROSMsg(*laserCloudSurround, laserCloudSurround3);
laserCloudSurround3.header.stamp = ros::Time().fromSec(timeLaserOdometry);
laserCloudSurround3.header.frame_id = "/camera_init";
pubLaserCloudSurround.publish(laserCloudSurround3);
}5.特征点示意图
cornerPoints, 彩色的是sharp，白色lessSharp
同样是平面点
