LaserOdometry
这一模块（节点）主要功能是：进行点云数据配准，完成运动估计
利用ScanRegistration中提取到的特征点，建立相邻时间点云数据之间的关联，由此推断lidar的运动。我们依旧从主函数开始：
int main(int argc, char** argv)
{
ros::init(argc, argv, "laserOdometry");
ros::NodeHandle nh;
/***** 订阅6个节点，通过回调函数进行处理 *****/
ros::Subscriber subCornerPointsSharp = nh.subscribe<sensor_msgs::PointCloud2>
("/laser_cloud_sharp", 2, laserCloudSharpHandler);
ros::Subscriber subCornerPointsLessSharp = nh.subscribe<sensor_msgs::PointCloud2>
("/laser_cloud_less_sharp", 2, laserCloudLessSharpHandler);
ros::Subscriber subSurfPointsFlat = nh.subscribe<sensor_msgs::PointCloud2>
("/laser_cloud_flat", 2, laserCloudFlatHandler);
ros::Subscriber subSurfPointsLessFlat = nh.subscribe<sensor_msgs::PointCloud2>
("/laser_cloud_less_flat", 2, laserCloudLessFlatHandler);
ros::Subscriber subLaserCloudFullRes = nh.subscribe<sensor_msgs::PointCloud2>
("/velodyne_cloud_2", 2, laserCloudFullResHandler);
ros::Subscriber subImuTrans = nh.subscribe<sensor_msgs::PointCloud2>
("/imu_trans", 5, imuTransHandler);
/**** 发布4个节点 *****/
ros::Publisher pubLaserCloudCornerLast = nh.advertise<sensor_msgs::PointCloud2>
("/laser_cloud_corner_last", 2);
ros::Publisher pubLaserCloudSurfLast = nh.advertise<sensor_msgs::PointCloud2>
("/laser_cloud_surf_last", 2);
ros::Publisher pubLaserCloudFullRes = nh.advertise<sensor_msgs::PointCloud2>
("/velodyne_cloud_3", 2);
ros::Publisher pubLaserOdometry = nh.advertise<nav_msgs::Odometry> ("/laser_odom_to_init", 5);
//创建对象
nav_msgs::Odometry laserOdometry;
laserOdometry.header.frame_id = "/camera_init";
laserOdometry.child_frame_id = "/laser_odom";
//创建TransformBroadcaster对象
tf::TransformBroadcaster tfBroadcaster;
//创建坐标变换对象
tf::StampedTransform laserOdometryTrans;
laserOdometryTrans.frame_id_ = "/camera_init";
laserOdometryTrans.child_frame_id_ = "/laser_odom";
接下来是设置处理速度并进行初始化
ros::Rate rate(100);
bool status = ros::ok();
while (status) {
// 进行一次回调函数的调用（完成当前时刻的msg的订阅和上一时刻的处理后的msg的发布）
ros::spinOnce();
if (newCornerPointsSharp && newCornerPointsLessSharp && newSurfPointsFlat &&
newSurfPointsLessFlat && newLaserCloudFullRes && newImuTrans &&
fabs(timeCornerPointsSharp - timeSurfPointsLessFlat) < 0.005 &&
fabs(timeCornerPointsLessSharp - timeSurfPointsLessFlat) < 0.005 &&
fabs(timeSurfPointsFlat - timeSurfPointsLessFlat) < 0.005 &&
fabs(timeLaserCloudFullRes - timeSurfPointsLessFlat) < 0.005 &&
fabs(timeImuTrans - timeSurfPointsLessFlat) < 0.005) {
newCornerPointsSharp = false;
newCornerPointsLessSharp = false;
newSurfPointsFlat = false;
newSurfPointsLessFlat = false;
newLaserCloudFullRes = false;
newImuTrans = false;
/*********初始化 ***********/
if (!systemInited) {
/********保存上一时刻的数据******/
// exchange cornerPointsLessSharp & laserCloudCornerLast
pcl::PointCloud<PointType>::Ptr laserCloudTemp = cornerPointsLessSharp;
cornerPointsLessSharp = laserCloudCornerLast;
laserCloudCornerLast = laserCloudTemp;
// exchange surfPointsLessFlat & laserCloudSurfLast
laserCloudTemp = surfPointsLessFlat;
surfPointsLessFlat = laserCloudSurfLast;
// kd二叉树 为了方便寻找最近的点
kdtreeCornerLast->setInputCloud(laserCloudCornerLast);
kdtreeSurfLast->setInputCloud(laserCloudSurfLast);
// 发布 laserCloudCornerLast 上一时刻
sensor_msgs::PointCloud2 laserCloudCornerLast2;
pcl::toROSMsg(*laserCloudCornerLast, laserCloudCornerLast2);
laserCloudCornerLast2.header.stamp = ros::Time().fromSec(timeSurfPointsLessFlat);
laserCloudCornerLast2.header.frame_id = "/camera";
pubLaserCloudCornerLast.publish(laserCloudCornerLast2);
// 发布 laserCloudSurfLast 上一时刻
sensor_msgs::PointCloud2 laserCloudSurfLast2;
pcl::toROSMsg(*laserCloudSurfLast, laserCloudSurfLast2);
laserCloudSurfLast2.header.stamp = ros::Time().fromSec(timeSurfPointsLessFlat);
laserCloudSurfLast2.header.frame_id = "/camera";
pubLaserCloudSurfLast.publish(laserCloudSurfLast2);
// transformSum累计变换了的角度
transformSum[0] += imuPitchStart;
transformSum[2] += imuRollStart;
systemInited = true;
continue; //初始化后开始接受下一时刻的订阅数据
}
laserCloudOri->clear();
coeffSel->clear();
//更新位姿中的坐标
transform[3] -= imuVeloFromStartX * scanPeriod;
transform[4] -= imuVeloFromStartY * scanPeriod;
transform[5] -= imuVeloFromStartZ * scanPeriod;在初始化之后，就能将相邻两个时刻的点云哪来进行配准了，具体的做法我们看看论文怎么说
在这里，由于需要高速的处理，论文并没有采用一一对应的点云配准（事实上，对于点云来说，这也是很难做到的），而是在上一时刻寻找两点确定一条与当前时刻的角点距离最近的直线作为该角点的配准对应，同理，在上一时刻寻找三点确定一个与当前时刻的平面点距离最近的平面作为该平面点的配准对应。
作者给出了距离的计算公式，于是整个配准过程变成了优化过程——不断迭代使得距离之和最小。我们来看看作者给出的距离公式
根据下图能较容易推导出该公式
由三角形面积公式可知2S=c*d=absinC 则d=absinC/c=|a×b|/c
接下来是点到平面的距离
根据下图能较容易推导出该公式
首先看分子：b和c叉乘，得到平面法向量设为h。即要求a和h的点乘的模。等于|a||h|cosα。再看分母：b和c叉乘的模，即法向量h的模|h|。因此，分子除以分母即为a在h上投影的长度|a|cosα，也就是a到平面的距离。
理解了原理，我们来看这一部分的代码
// 特征点的数量足够多再开始匹配，cornerPointsSharpNum和cornerPointsSharp都是最新时刻t+1的数据
if (laserCloudCornerLastNum > 10 && laserCloudSurfLastNum > 100) {
std::vector<int> indices;
pcl::removeNaNFromPointCloud(*cornerPointsSharp,*cornerPointsSharp, indices);
int cornerPointsSharpNum = cornerPointsSharp->points.size();
// 多次迭代 LM求解前后时刻的位姿变化
for (int iterCount = 0; iterCount < 25; iterCount++) {
laserCloudOri->clear();
coeffSel->clear();
/******** 特征角点的处理 ********/
for (int i = 0; i < cornerPointsSharpNum; i++) {
// 每一次迭代都将特征点都要利用当前预测的坐标转换转换至k+1 sweep的初始位置处对应于函数 TransformToStart()
// 每一次迭代都将特征点都要利用当前预测的坐标转换转换至k+1 sweep的结束位置处对应于函数 TransformToEnd()
// pointSel是当前时刻t+1的cornerPointsSharp转换到初始imu坐标系后的点坐标，对角点一个一个做处理，设为i点
TransformToStart(&cornerPointsSharp->points[i], &pointSel);
// 每5次循环寻找一次邻域点，否则沿用上次循环的邻域点
if (iterCount % 5 == 0) {
std::vector<int> indices;
pcl::removeNaNFromPointCloud(*laserCloudCornerLast,*laserCloudCornerLast, indices);
// nearestKSearch是PCL中的K近邻域搜索，搜索上一时刻LessCornor的K邻域点
// 搜索结果: pointSearchInd是索引; pointSearchSqDis是近邻对应的平方距离(以25作为阈值)
kdtreeCornerLast->nearestKSearch(pointSel, 1, pointSearchInd, pointSearchSqDis);
/******** 在上一时刻t的LessSharp中用KD树寻找和点i最近的点j ********/
int closestPointInd = -1, minPointInd2 = -1;
if (pointSearchSqDis[0] < 25) {
closestPointInd = pointSearchInd[0];
//最近邻域点的所在层数
int closestPointScan = int(laserCloudCornerLast->points[closestPointInd].intensity);
/******** 在上一时刻t的LessSharp中点j附近2层中最近的点l ********/
float pointSqDis, minPointSqDis2 = 25;
for (int j = closestPointInd + 1; j < cornerPointsSharpNum; j++) {
// closestPointScan +(-) 2.5表示只接受(后)前2层(共4层)范围内的数据
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;
}
/******** 计算点i到jl的距离de 理想情况下ijl共线 ********/
if (pointSearchCornerInd2[i] >= 0) {
tripod1 = laserCloudCornerLast->points[pointSearchCornerInd1[i]];
tripod2 = laserCloudCornerLast->points[pointSearchCornerInd2[i]];
float x0 = pointSel.x;
float y0 = pointSel.y;
float z0 = pointSel.z;
float x1 = tripod1.x;
float y1 = tripod1.y;
float z1 = tripod1.z;
float x2 = tripod2.x;
float y2 = tripod2.y;
float z2 = tripod2.z;
// 公式分子
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));
// 对x,y,z的偏导
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;
if (iterCount >= 5) {
s = 1 - 1.8 * fabs(ld2);//增加权重，距离越远，影响影子越小
}
coeff.x = s * la;
coeff.y = s * lb;
coeff.z = s * lc;
coeff.intensity = s * ld2;
if (s > 0.1 && ld2 != 0) {
laserCloudOri->push_back(cornerPointsSharp->points[i]);
coeffSel->push_back(coeff);
}
}
}
/******** 特征平面点的处理 ********/
for (int i = 0; i < surfPointsFlatNum; i++) {
/********当前时刻t+1转换到imu初始坐标系下，对平面点一个一个做处理，设为点i ********/
TransformToStart(&surfPointsFlat->points[i], &pointSel);
if (iterCount % 5 == 0) {
kdtreeSurfLast->nearestKSearch(pointSel, 1, pointSearchInd, pointSearchSqDis);
int closestPointInd = -1, minPointInd2 = -1, minPointInd3 = -1;
/******** 上一时刻t的LessFlat中的距离点i最近的点j ********/
if (pointSearchSqDis[0] < 25) {
closestPointInd = pointSearchInd[0];
int closestPointScan = int(laserCloudSurfLast->points[closestPointInd].intensity);
/******** 在附近4层寻找距离点j最近的两点l,m ********/
float pointSqDis, minPointSqDis2 = 25, minPointSqDis3 = 25;
for (int j = closestPointInd + 1; j < surfPointsFlatNum; j++) {
if (int(laserCloudSurfLast->points[j].intensity) > closestPointScan + 2.5) {
break;
}
pointSqDis = (laserCloudSurfLast->points[j].x - pointSel.x) *
(laserCloudSurfLast->points[j].x - pointSel.x) +
(laserCloudSurfLast->points[j].y - pointSel.y) *
(laserCloudSurfLast->points[j].y - pointSel.y) +
(laserCloudSurfLast->points[j].z - pointSel.z) *
(laserCloudSurfLast->points[j].z - pointSel.z);
if (int(laserCloudSurfLast->points[j].intensity) <= closestPointScan) {
if (pointSqDis < minPointSqDis2) {
minPointSqDis2 = pointSqDis;
minPointInd2 = j;
}
} else {
if (pointSqDis < minPointSqDis3) {
minPointSqDis3 = pointSqDis;
minPointInd3 = j;
}
}
}
for (int j = closestPointInd - 1; j >= 0; j--) {
if (int(laserCloudSurfLast->points[j].intensity) < closestPointScan - 2.5) {
break;
}
pointSqDis = (laserCloudSurfLast->points[j].x - pointSel.x) *
(laserCloudSurfLast->points[j].x - pointSel.x) +
(laserCloudSurfLast->points[j].y - pointSel.y) *
(laserCloudSurfLast->points[j].y - pointSel.y) +
(laserCloudSurfLast->points[j].z - pointSel.z) *
(laserCloudSurfLast->points[j].z - pointSel.z);
if (int(laserCloudSurfLast->points[j].intensity) >= closestPointScan) {
if (pointSqDis < minPointSqDis2) {
minPointSqDis2 = pointSqDis;
minPointInd2 = j;
}
} else {
if (pointSqDis < minPointSqDis3) {
minPointSqDis3 = pointSqDis;
minPointInd3 = j;
}
}
}
}
pointSearchSurfInd1[i] = closestPointInd;
pointSearchSurfInd2[i] = minPointInd2;
pointSearchSurfInd3[i] = minPointInd3;
}
/*******计算点i到平面jlm的距离dh 理想情况下ijlm共面 *******/
if (pointSearchSurfInd2[i] >= 0 && pointSearchSurfInd3[i] >= 0) {
tripod1 = laserCloudSurfLast->points[pointSearchSurfInd1[i]];
tripod2 = laserCloudSurfLast->points[pointSearchSurfInd2[i]];
tripod3 = laserCloudSurfLast->points[pointSearchSurfInd3[i]];
// pa,pb,pc既为偏导函数的分子部分也为距离函数分母行列式内各方向分量值，ps为分母部分
float pa = (tripod2.y - tripod1.y) * (tripod3.z - tripod1.z)
- (tripod3.y - tripod1.y) * (tripod2.z - tripod1.z);
float pb = (tripod2.z - tripod1.z) * (tripod3.x - tripod1.x)
- (tripod3.z - tripod1.z) * (tripod2.x - tripod1.x);
float pc = (tripod2.x - tripod1.x) * (tripod3.y - tripod1.y)
- (tripod3.x - tripod1.x) * (tripod2.y - tripod1.y);
float pd = -(pa * tripod1.x + pb * tripod1.y + pc * tripod1.z);
float ps = sqrt(pa * pa + pb * pb + pc * pc);
pa /= ps;
pb /= ps;
pc /= ps;
pd /= ps;
//距离没有取绝对值
float pd2 = pa * pointSel.x + pb * pointSel.y + pc * pointSel.z + pd;
// 平面偏导公式，似乎后面没有用到
pointProj = pointSel;
pointProj.x -= pa * pd2;
pointProj.y -= pb * pd2;
pointProj.z -= pc * pd2;
float s = 1;
if (iterCount >= 5) {
s = 1 - 1.8 * fabs(pd2) / sqrt(sqrt(pointSel.x * pointSel.x
+ pointSel.y * pointSel.y + pointSel.z * pointSel.z));//加上影响因子
}
coeff.x = s * pa;
coeff.y = s * pb;
coeff.z = s * pc;
coeff.intensity = s * pd2;
if (s > 0.1 && pd2 != 0) {
laserCloudOri->push_back(surfPointsFlat->points[i]);
coeffSel->push_back(coeff);
}
}
}
// 如果点云数量小于10，可以不用计算位姿矩阵了
int pointSelNum = laserCloudOri->points.size();
if (pointSelNum < 10) {
continue;
}似乎后面没有用到
pointProj = pointSel;
pointProj.x -= pa * pd2;
pointProj.y -= pb * pd2;
pointProj.z -= pc * pd2;
float s = 1;
if (iterCount >= 5) {
s = 1 - 1.8 * fabs(pd2) / sqrt(sqrt(pointSel.x * pointSel.x
+ pointSel.y * pointSel.y + pointSel.z * pointSel.z));//加上影响因子
}
coeff.x = s * pa;
coeff.y = s * pb;
coeff.z = s * pc;
coeff.intensity = s * pd2;
if (s > 0.1 && pd2 != 0) {
laserCloudOri->push_back(surfPointsFlat->points[i]);
coeffSel->push_back(coeff);
}
}
}
// 如果点云数量小于10，可以不用计算位姿矩阵了
int pointSelNum = laserCloudOri->points.size();
if (pointSelNum < 10) {
continue;
}之后则是通过构建雅克比矩阵，完成LM求解，从而估计运动，我们仔细阅读论文了解其原理
接下来看看代码
cv::Mat matA(pointSelNum, 6, CV_32F, cv::Scalar::all(0));
cv::Mat matAt(6, pointSelNum, CV_32F, cv::Scalar::all(0));
cv::Mat matAtA(6, 6, CV_32F, cv::Scalar::all(0));
cv::Mat matB(pointSelNum, 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 < pointSelNum; i++) {
pointOri = laserCloudOri->points[i];
coeff = coeffSel->points[i];
float s = 1;
float srx = sin(s * transform[0]);
float crx = cos(s * transform[0]);
float sry = sin(s * transform[1]);
float cry = cos(s * transform[1]);
float srz = sin(s * transform[2]);
float crz = cos(s * transform[2]);
float tx = s * transform[3];
float ty = s * transform[4];
float tz = s * transform[5];
// J对rx,ry,rz,tx,ty,tz求解偏导，不是很明白怎么得到的代码
float arx = (-s*crx*sry*srz*pointOri.x + s*crx*crz*sry*pointOri.y + s*srx*sry*pointOri.z
+ s*tx*crx*sry*srz - s*ty*crx*crz*sry - s*tz*srx*sry) * coeff.x
+ (s*srx*srz*pointOri.x - s*crz*srx*pointOri.y + s*crx*pointOri.z
+ s*ty*crz*srx - s*tz*crx - s*tx*srx*srz) * coeff.y
+ (s*crx*cry*srz*pointOri.x - s*crx*cry*crz*pointOri.y - s*cry*srx*pointOri.z
+ s*tz*cry*srx + s*ty*crx*cry*crz - s*tx*crx*cry*srz) * coeff.z;
float ary = ((-s*crz*sry - s*cry*srx*srz)*pointOri.x
+ (s*cry*crz*srx - s*sry*srz)*pointOri.y - s*crx*cry*pointOri.z
+ tx*(s*crz*sry + s*cry*srx*srz) + ty*(s*sry*srz - s*cry*crz*srx)
+ s*tz*crx*cry) * coeff.x
+ ((s*cry*crz - s*srx*sry*srz)*pointOri.x
+ (s*cry*srz + s*crz*srx*sry)*pointOri.y - s*crx*sry*pointOri.z
+ s*tz*crx*sry - ty*(s*cry*srz + s*crz*srx*sry)
- tx*(s*cry*crz - s*srx*sry*srz)) * coeff.z;
float arz = ((-s*cry*srz - s*crz*srx*sry)*pointOri.x + (s*cry*crz - s*srx*sry*srz)*pointOri.y
+ tx*(s*cry*srz + s*crz*srx*sry) - ty*(s*cry*crz - s*srx*sry*srz)) * coeff.x
+ (-s*crx*crz*pointOri.x - s*crx*srz*pointOri.y
+ s*ty*crx*srz + s*tx*crx*crz) * coeff.y
+ ((s*cry*crz*srx - s*sry*srz)*pointOri.x + (s*crz*sry + s*cry*srx*srz)*pointOri.y
+ tx*(s*sry*srz - s*cry*crz*srx) - ty*(s*crz*sry + s*cry*srx*srz)) * coeff.z;
float atx = -s*(cry*crz - srx*sry*srz) * coeff.x + s*crx*srz * coeff.y
- s*(crz*sry + cry*srx*srz) * coeff.z;
float aty = -s*(cry*srz + crz*srx*sry) * coeff.x - s*crx*crz * coeff.y
- s*(sry*srz - cry*crz*srx) * coeff.z;
float atz = s*crx*sry * coeff.x - s*srx * coeff.y - s*crx*cry * coeff.z;
float d2 = coeff.intensity;
// A=[J的偏导]; B=[权重系数*(点到直线的距离/点到平面的距离)] 求解公式: AX=B
// 为了让左边满秩，同乘At-> At*A*X = At*B
matA.at<float>(i, 0) = arx;
matA.at<float>(i, 1) = ary;
matA.at<float>(i, 2) = arz;
matA.at<float>(i, 3) = atx;
matA.at<float>(i, 4) = aty;
matA.at<float>(i, 5) = atz;
matB.at<float>(i, 0) = -0.05 * d2;
}
cv::transpose(matA, matAt);
matAtA = matAt * matA;
matAtB = matAt * matB;
cv::solve(matAtA, matAtB, matX, cv::DECOMP_QR);//QR分解得到X不是很明白怎么得到的代码
float arx = (-s*crx*sry*srz*pointOri.x + s*crx*crz*sry*pointOri.y + s*srx*sry*pointOri.z
+ s*tx*crx*sry*srz - s*ty*crx*crz*sry - s*tz*srx*sry) * coeff.x
+ (s*srx*srz*pointOri.x - s*crz*srx*pointOri.y + s*crx*pointOri.z
+ s*ty*crz*srx - s*tz*crx - s*tx*srx*srz) * coeff.y
+ (s*crx*cry*srz*pointOri.x - s*crx*cry*crz*pointOri.y - s*cry*srx*pointOri.z
+ s*tz*cry*srx + s*ty*crx*cry*crz - s*tx*crx*cry*srz) * coeff.z;
float ary = ((-s*crz*sry - s*cry*srx*srz)*pointOri.x
+ (s*cry*crz*srx - s*sry*srz)*pointOri.y - s*crx*cry*pointOri.z
+ tx*(s*crz*sry + s*cry*srx*srz) + ty*(s*sry*srz - s*cry*crz*srx)
+ s*tz*crx*cry) * coeff.x
+ ((s*cry*crz - s*srx*sry*srz)*pointOri.x
+ (s*cry*srz + s*crz*srx*sry)*pointOri.y - s*crx*sry*pointOri.z
+ s*tz*crx*sry - ty*(s*cry*srz + s*crz*srx*sry)
- tx*(s*cry*crz - s*srx*sry*srz)) * coeff.z;
float arz = ((-s*cry*srz - s*crz*srx*sry)*pointOri.x + (s*cry*crz - s*srx*sry*srz)*pointOri.y
+ tx*(s*cry*srz + s*crz*srx*sry) - ty*(s*cry*crz - s*srx*sry*srz)) * coeff.x
+ (-s*crx*crz*pointOri.x - s*crx*srz*pointOri.y
+ s*ty*crx*srz + s*tx*crx*crz) * coeff.y
+ ((s*cry*crz*srx - s*sry*srz)*pointOri.x + (s*crz*sry + s*cry*srx*srz)*pointOri.y
+ tx*(s*sry*srz - s*cry*crz*srx) - ty*(s*crz*sry + s*cry*srx*srz)) * coeff.z;
float atx = -s*(cry*crz - srx*sry*srz) * coeff.x + s*crx*srz * coeff.y
- s*(crz*sry + cry*srx*srz) * coeff.z;
float aty = -s*(cry*srz + crz*srx*sry) * coeff.x - s*crx*crz * coeff.y
- s*(sry*srz - cry*crz*srx) * coeff.z;
float atz = s*crx*sry * coeff.x - s*srx * coeff.y - s*crx*cry * coeff.z;
float d2 = coeff.intensity;
// A=[J的偏导]; B=[权重系数*(点到直线的距离/点到平面的距离)] 求解公式: AX=B
// 为了让左边满秩，同乘At-> At*A*X = At*B
matA.at<float>(i, 0) = arx;
matA.at<float>(i, 1) = ary;
matA.at<float>(i, 2) = arz;
matA.at<float>(i, 3) = atx;
matA.at<float>(i, 4) = aty;
matA.at<float>(i, 5) = atz;
matB.at<float>(i, 0) = -0.05 * d2;
}
cv::transpose(matA, matAt);
matAtA = matAt * matA;
matAtB = matAt * matB;
cv::solve(matAtA, matAtB, matX, cv::DECOMP_QR);//QR分解得到X接下来有一个迭代第一步的处理，猜测是出现退化进行修正。然后更新位姿之后进行收敛判断
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); // 计算矩阵的特征向量E及特征向量的反对称阵V
matV.copyTo(matV2);
isDegenerate = false;
float eignThre[6] = {10, 10, 10, 10, 10, 10};
for (int i = 5; i >= 0; i--) {
if (matE.at<float>(0, i) < eignThre[i]) {
for (int j = 0; j < 6; j++) {
matV2.at<float>(i, j) = 0;
}
isDegenerate = true; // 存在比10小的特征值则出现退化
} 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<float>(0, 0);
transform[1] += matX.at<float>(1, 0);
transform[2] += matX.at<float>(2, 0);
transform[3] += matX.at<float>(3, 0);
transform[4] += matX.at<float>(4, 0);
transform[5] += matX.at<float>(5, 0);
for(int i=0; i<6; i++){
if(isnan(transform[i]))
transform[i]=0;
}
float deltaR = sqrt(
pow(rad2deg(matX.at<float>(0, 0)), 2) +
pow(rad2deg(matX.at<float>(1, 0)), 2) +
pow(rad2deg(matX.at<float>(2, 0)), 2));
float deltaT = sqrt(
pow(matX.at<float>(3, 0) * 100, 2) +
pow(matX.at<float>(4, 0) * 100, 2) +
pow(matX.at<float>(5, 0) * 100, 2));
if (deltaR < 0.1 && deltaT < 0.1) {
break;
}
最为艰难的计算过程结束了，接下来就是坐标转换了。。。。。这部分其实也让我很头疼，各个坐标系有点晕，仔细理一理
先看代码
float rx, ry, rz, tx, ty, tz;
/******** 旋转角的累计变化量 ********/
// AccumulateRotation作用将局部旋转坐标转换至全局旋转坐标
AccumulateRotation(transformSum[0], transformSum[1], transformSum[2],
-transform[0], -transform[1] * 1.05, -transform[2], rx, ry, rz);
/******** 接着转移到世界坐标系下 *******/
float x1 = cos(rz) * (transform[3] - imuShiftFromStartX)
- sin(rz) * (transform[4] - imuShiftFromStartY);
float y1 = sin(rz) * (transform[3] - imuShiftFromStartX)
+ cos(rz) * (transform[4] - imuShiftFromStartY);
float z1 = transform[5] * 1.05 - imuShiftFromStartZ;
float x2 = x1;
float y2 = cos(rx) * y1 - sin(rx) * z1;
float z2 = sin(rx) * y1 + cos(rx) * z1;
tx = transformSum[3] - (cos(ry) * x2 + sin(ry) * z2);
ty = transformSum[4] - y2;
tz = transformSum[5] - (-sin(ry) * x2 + cos(ry) * z2);
// 插入imu旋转，更新位姿
PluginIMURotation(rx, ry, rz, imuPitchStart, imuYawStart, imuRollStart,
imuPitchLast, imuYawLast, imuRollLast, rx, ry, rz);
transformSum[0] = rx;
transformSum[1] = ry;
transformSum[2] = rz;
transformSum[3] = tx;
transformSum[4] = ty;
transformSum[5] = tz;
/******** rx,ry,rz转化为四元数发布 ********/
geometry_msgs::Quaternion geoQuat = tf::createQuaternionMsgFromRollPitchYaw(rz, -rx, -ry);
laserOdometry.header.stamp = ros::Time().fromSec(timeSurfPointsLessFlat);
laserOdometry.pose.pose.orientation.x = -geoQuat.y;
laserOdometry.pose.pose.orientation.y = -geoQuat.z;
laserOdometry.pose.pose.orientation.z = geoQuat.x;
laserOdometry.pose.pose.orientation.w = geoQuat.w;
laserOdometry.pose.pose.position.x = tx;
laserOdometry.pose.pose.position.y = ty;
laserOdometry.pose.pose.position.z = tz;
pubLaserOdometry.publish(laserOdometry);
/********* laserOdometryTrans 是用于tf广播 ********/
laserOdometryTrans.stamp_ = ros::Time().fromSec(timeSurfPointsLessFlat);
laserOdometryTrans.setRotation(tf::Quaternion(-geoQuat.y, -geoQuat.z, geoQuat.x, geoQuat.w));
laserOdometryTrans.setOrigin(tf::Vector3(tx, ty, tz));
tfBroadcaster.sendTransform(laserOdometryTrans);
// TransformToEnd的作用是将k+1时刻的less特征点转移至k+1时刻的sweep的结束位置处的雷达坐标系下
int cornerPointsLessSharpNum = cornerPointsLessSharp->points.size();
for (int i = 0; i < cornerPointsLessSharpNum; i++) {
TransformToEnd(&cornerPointsLessSharp->points[i], &cornerPointsLessSharp->points[i]);
}
int surfPointsLessFlatNum = surfPointsLessFlat->points.size();
for (int i = 0; i < surfPointsLessFlatNum; i++) {
TransformToEnd(&surfPointsLessFlat->points[i], &surfPointsLessFlat->points[i]);
}
/***** 这样if条件的意义 *******/
frameCount++;
if (frameCount >= skipFrameNum + 1) {
int laserCloudFullResNum = laserCloudFullRes->points.size();
for (int i = 0; i < laserCloudFullResNum; i++) {
TransformToEnd(&laserCloudFullRes->points[i], &laserCloudFullRes->points[i]);
}
}
/******* t+1时刻数据成为上一个时刻的数据 ************/
pcl::PointCloud<PointType>::Ptr laserCloudTemp = cornerPointsLessSharp;
cornerPointsLessSharp = laserCloudCornerLast;
laserCloudCornerLast = laserCloudTemp;
laserCloudTemp = surfPointsLessFlat;
surfPointsLessFlat = laserCloudSurfLast;
laserCloudSurfLast = laserCloudTemp;
laserCloudCornerLastNum = laserCloudCornerLast->points.size();
laserCloudSurfLastNum = laserCloudSurfLast->points.size();
if (laserCloudCornerLastNum > 10 && laserCloudSurfLastNum > 100) {
kdtreeCornerLast->setInputCloud(laserCloudCornerLast);
kdtreeSurfLast->setInputCloud(laserCloudSurfLast);
}
/*———————————— 2HZ发布一次？ ————————————*/
if (frameCount >= skipFrameNum + 1) {
frameCount = 0;
sensor_msgs::PointCloud2 laserCloudCornerLast2;
pcl::toROSMsg(*laserCloudCornerLast, laserCloudCornerLast2);
laserCloudCornerLast2.header.stamp = ros::Time().fromSec(timeSurfPointsLessFlat);
laserCloudCornerLast2.header.frame_id = "/camera";
pubLaserCloudCornerLast.publish(laserCloudCornerLast2);
sensor_msgs::PointCloud2 laserCloudSurfLast2;
pcl::toROSMsg(*laserCloudSurfLast, laserCloudSurfLast2);
laserCloudSurfLast2.header.stamp = ros::Time().fromSec(timeSurfPointsLessFlat);
laserCloudSurfLast2.header.frame_id = "/camera";
pubLaserCloudSurfLast.publish(laserCloudSurfLast2);
sensor_msgs::PointCloud2 laserCloudFullRes3;
pcl::toROSMsg(*laserCloudFullRes, laserCloudFullRes3);
laserCloudFullRes3.header.stamp = ros::Time().fromSec(timeSurfPointsLessFlat);
laserCloudFullRes3.header.frame_id = "/camera";
pubLaserCloudFullRes.publish(laserCloudFullRes3);
}
}
status = ros::ok();
rate.sleep();
}
return 0;
}代码剩余的部分中，回调函数的功能较为简单，不再赘述。
最后同样附上框图方便理解
捞一下https://blog.csdn.net/qq_21842097/article/details/80926150
迁移到博客园
