LOAM_velodyne 2.laserOdometry 激光里程计
目录
注解：
代码流程：
1、主函数： main
2.LaserOdometry类中的 setup 函数：
3、回调函数laserCloudSharpHandler
4、回调laserCloudLessSharpHandler
5、回调laserCloudFlatHandler
6、回调laserCloudLessFlatHandler
7、回调laserCloudFullResHandler
8、IMU回调imuTransHandler
9、process函数
三、核心函数 BasicLaserOdometry::process函数
总结：
注解：
功能：进行点云匹配，完成运动估计（帧间匹配）。
具体实现：利用ScanRegistration中提取到的特征点，建立相邻时间点云数据之间的关联，由此推断lidar的运动。
备注：绿色的函数指在下面解释，红色的函数指本文单独的函数解释
代码流程：
1、主函数： main
/** Main node entry point. */
int main(int argc, char **argv)
{
ros::init(argc, argv, "laserOdometry");
ros::NodeHandle node;
ros::NodeHandle privateNode("~");
loam::LaserOdometry laserOdom(0.1);
if (laserOdom.setup(node, privateNode)) {
// initialization successful
laserOdom.spin();
}
return 0;
}1.loam::LaserOdometry类的构造
声明 ：
explicit LaserOdometry(float scanPeriod = 0.1, uint16_t ioRatio = 2, size_t maxIterations = 25);
参数解释：激光周期0.1， 输入帧与输出帧的比率2， 最大迭代次数25
定义： 初始化odom和odom_tf
LaserOdometry::LaserOdometry(float scanPeriod, uint16_t ioRatio, size_t maxIterations):
BasicLaserOdometry(scanPeriod, maxIterations),
_ioRatio(ioRatio)
{
// initialize odometry and odometry tf messages
_laserOdometryMsg.header.frame_id = "/camera_init";
_laserOdometryMsg.child_frame_id = "/laser_odom";
_laserOdometryTrans.frame_id_ = "/camera_init";
_laserOdometryTrans.child_frame_id_ = "/laser_odom";
}
BasicLaserOdometry类的定义
BasicLaserOdometry::BasicLaserOdometry(float scanPeriod, size_t maxIterations) :
_scanPeriod(scanPeriod),
_systemInited(false),
_frameCount(0),
_maxIterations(maxIterations),
_deltaTAbort(0.1),
_deltaRAbort(0.1),
_cornerPointsSharp(new pcl::PointCloud()),
_cornerPointsLessSharp(new pcl::PointCloud()),
_surfPointsFlat(new pcl::PointCloud()),
_surfPointsLessFlat(new pcl::PointCloud()),
_laserCloud(new pcl::PointCloud()),
_lastCornerCloud(new pcl::PointCloud()),
_lastSurfaceCloud(new pcl::PointCloud()),
_laserCloudOri(new pcl::PointCloud()),
_coeffSel(new pcl::PointCloud())
{} 2.LaserOdometry类中的 setup 函数：
读取参数：
scanPeriod：激光周期 0.1 ， ioRatio：输入帧与输出帧的比率 0.1 ， maxIterations：最大迭代次数 25 , deltaTAbort：的优化中止位置阈值 ， deltaRAbort：优化中止角度阈值
发布话题：
_pubLaserCloudCornerLast = node.advertise("/laser_cloud_corner_last", 2);
_pubLaserCloudSurfLast = node.advertise("/laser_cloud_surf_last", 2);
_pubLaserCloudFullRes = node.advertise("/velodyne_cloud_3", 2);
_pubLaserOdometry = node.advertise("/laser_odom_to_init", 5); 接收话题：
_subCornerPointsSharp = node.subscribe
("/laser_cloud_sharp", 2, &LaserOdometry::laserCloudSharpHandler, this);
_subCornerPointsLessSharp = node.subscribe
("/laser_cloud_less_sharp", 2, &LaserOdometry::laserCloudLessSharpHandler, this);
_subSurfPointsFlat = node.subscribe
("/laser_cloud_flat", 2, &LaserOdometry::laserCloudFlatHandler, this);
_subSurfPointsLessFlat = node.subscribe
("/laser_cloud_less_flat", 2, &LaserOdometry::laserCloudLessFlatHandler, this);
_subLaserCloudFullRes = node.subscribe
("/velodyne_cloud_2", 2, &LaserOdometry::laserCloudFullResHandler, this);
_subImuTrans = node.subscribe
("/imu_trans", 5, &LaserOdometry::imuTransHandler, this); 3.LaserOdometry类中的 spin函数
ros::Rate rate(100);
bool status = ros::ok();
// loop until shutdown
while (status)
{
ros::spinOnce();
// try processing new data
process();
status = ros::ok();
rate.sleep();
}检测ros状态，同时执行process函数
3、回调函数laserCloudSharpHandler
1.将接收到的角点的时间记录
_timeCornerPointsSharp = cornerPointsSharpMsg->header.stamp;2.将原有角点清除，并把当前角点数据转化为pcl，pcl::removeNaNFromPointCloud(laserCloudIn, laserCloudIn, indices)
cornerPointsSharp()->clear();
pcl::fromROSMsg(*cornerPointsSharpMsg, *cornerPointsSharp());3.移除NAN点，并有新角点标记设为true
pcl::removeNaNFromPointCloud(*cornerPointsSharp(), *cornerPointsSharp(), indices);
_newCornerPointsSharp = true;4、回调laserCloudLessSharpHandler
与上面类似
_timeCornerPointsLessSharp = cornerPointsLessSharpMsg->header.stamp;
cornerPointsLessSharp()->clear();
pcl::fromROSMsg(*cornerPointsLessSharpMsg, *cornerPointsLessSharp());
std::vector indices;
pcl::removeNaNFromPointCloud(*cornerPointsLessSharp(), *cornerPointsLessSharp(), indices);
_newCornerPointsLessSharp = true; 5、回调laserCloudFlatHandler
与上面类似
_timeSurfPointsFlat = surfPointsFlatMsg->header.stamp;
surfPointsFlat()->clear();
pcl::fromROSMsg(*surfPointsFlatMsg, *surfPointsFlat());
std::vector indices;
pcl::removeNaNFromPointCloud(*surfPointsFlat(), *surfPointsFlat(), indices);
_newSurfPointsFlat = true; 6、回调laserCloudLessFlatHandler
与上面类似
_timeSurfPointsLessFlat = surfPointsLessFlatMsg->header.stamp;
surfPointsLessFlat()->clear();
pcl::fromROSMsg(*surfPointsLessFlatMsg, *surfPointsLessFlat());
std::vector indices;
pcl::removeNaNFromPointCloud(*surfPointsLessFlat(), *surfPointsLessFlat(), indices);
_newSurfPointsLessFlat = true; 7、回调laserCloudFullResHandler
与上面类似
_timeLaserCloudFullRes = laserCloudFullResMsg->header.stamp;
laserCloud()->clear();
pcl::fromROSMsg(*laserCloudFullResMsg, *laserCloud());
std::vector indices;
pcl::removeNaNFromPointCloud(*laserCloud(), *laserCloud(), indices);
_newLaserCloudFullRes = true; 8、IMU回调imuTransHandler
_timeImuTrans = imuTransMsg->header.stamp;
pcl::PointCloud imuTrans;
pcl::fromROSMsg(*imuTransMsg, imuTrans);
updateIMU(imuTrans);
_newImuTrans = true; 上述中updateIMU数据
assert(4 == imuTrans.size());
_imuPitchStart = imuTrans.points[0].x;
_imuYawStart = imuTrans.points[0].y;
_imuRollStart = imuTrans.points[0].z;
_imuPitchEnd = imuTrans.points[1].x;
_imuYawEnd = imuTrans.points[1].y;
_imuRollEnd = imuTrans.points[1].z;
_imuShiftFromStart = imuTrans.points[2];
_imuVeloFromStart = imuTrans.points[3];IMU数据发布时：
void BasicScanRegistration::updateIMUTransform()
{
_imuTrans[0].x = _imuStart.pitch.rad();
_imuTrans[0].y = _imuStart.yaw.rad();
_imuTrans[0].z = _imuStart.roll.rad();
_imuTrans[1].x = _imuCur.pitch.rad();
_imuTrans[1].y = _imuCur.yaw.rad();
_imuTrans[1].z = _imuCur.roll.rad();
Vector3 imuShiftFromStart = _imuPositionShift;
rotateYXZ(imuShiftFromStart, -_imuStart.yaw, -_imuStart.pitch, -_imuStart.roll);
_imuTrans[2].x = imuShiftFromStart.x();
_imuTrans[2].y = imuShiftFromStart.y();
_imuTrans[2].z = imuShiftFromStart.z();
Vector3 imuVelocityFromStart = _imuCur.velocity - _imuStart.velocity;
rotateYXZ(imuVelocityFromStart, -_imuStart.yaw, -_imuStart.pitch, -_imuStart.roll);
_imuTrans[3].x = imuVelocityFromStart.x();
_imuTrans[3].y = imuVelocityFromStart.y();
_imuTrans[3].z = imuVelocityFromStart.z();
}9、process函数
1.首先判断有没有新数据hasNewData()，如果没有新数据，该函数直接返回 return;
- 各个数据都已经更新，且 各个数据都由一帧激光数据生成
return _newCornerPointsSharp && _newCornerPointsLessSharp && _newSurfPointsFlat &&
_newSurfPointsLessFlat && _newLaserCloudFullRes && _newImuTrans &&
fabs((_timeCornerPointsSharp - _timeSurfPointsLessFlat).toSec()) < 0.005 &&
fabs((_timeCornerPointsLessSharp - _timeSurfPointsLessFlat).toSec()) < 0.005 &&
fabs((_timeSurfPointsFlat - _timeSurfPointsLessFlat).toSec()) < 0.005 &&
fabs((_timeLaserCloudFullRes - _timeSurfPointsLessFlat).toSec()) < 0.005 &&
fabs((_timeImuTrans - _timeSurfPointsLessFlat).toSec()) < 0.005;2.重置标志位reset()
_newCornerPointsSharp = false;
_newCornerPointsLessSharp = false;
_newSurfPointsFlat = false;
_newSurfPointsLessFlat = false;
_newLaserCloudFullRes = false;
_newImuTrans = false;3.求取帧间匹配的位姿 process 函数
BasicLaserOdometry::process();4.发布结果 publishResult();
1)transformSum有里程计总的转化，包括平移和旋转，首先将旋转转化成4元素
geometry_msgs::Quaternion geoQuat = tf::createQuaternionMsgFromRollPitchYaw(transformSum().rot_z.rad(),
-transformSum().rot_x.rad(),
-transformSum().rot_y.rad());2)从话题和tf中将该数据发布出去
_laserOdometryMsg.header.stamp = _timeSurfPointsLessFlat;
_laserOdometryMsg.pose.pose.orientation.x = -geoQuat.y;
_laserOdometryMsg.pose.pose.orientation.y = -geoQuat.z;
_laserOdometryMsg.pose.pose.orientation.z = geoQuat.x;
_laserOdometryMsg.pose.pose.orientation.w = geoQuat.w;
_laserOdometryMsg.pose.pose.position.x = transformSum().pos.x();
_laserOdometryMsg.pose.pose.position.y = transformSum().pos.y();
_laserOdometryMsg.pose.pose.position.z = transformSum().pos.z();
_pubLaserOdometry.publish(_laserOdometryMsg);
_laserOdometryTrans.stamp_ = _timeSurfPointsLessFlat;
_laserOdometryTrans.setRotation(tf::Quaternion(-geoQuat.y, -geoQuat.z, geoQuat.x, geoQuat.w));
_laserOdometryTrans.setOrigin(tf::Vector3(transformSum().pos.x(), transformSum().pos.y(), transformSum().pos.z()));
_tfBroadcaster.sendTransform(_laserOdometryTrans);3)按照设定比率发布数据
if (_ioRatio < 2 || frameCount() % _ioRatio == 1)
{
ros::Time sweepTime = _timeSurfPointsLessFlat;
publishCloudMsg(_pubLaserCloudCornerLast, *lastCornerCloud(), sweepTime, "/camera");
publishCloudMsg(_pubLaserCloudSurfLast, *lastSurfaceCloud(), sweepTime, "/camera");
transformToEnd(laserCloud()); // transform full resolution cloud to sweep end before sending it
publishCloudMsg(_pubLaserCloudFullRes, *laserCloud(), sweepTime, "/camera");
}三、核心函数 BasicLaserOdometry::process函数
分为三步：初始化，点云处理，坐标转化
1.检测系统是否初始化，如果未初始化时，进行初始化
- 激光的匹配涉及到两组数据，因此初始化相当于首先获取将一组数据。
- 初始化中，角特征 + 平面特征 构建了lastKDTree
if (!_systemInited)
{
_cornerPointsLessSharp.swap(_lastCornerCloud);
_surfPointsLessFlat.swap(_lastSurfaceCloud);
_lastCornerKDTree.setInputCloud(_lastCornerCloud);
_lastSurfaceKDTree.setInputCloud(_lastSurfaceCloud);
_transformSum.rot_x += _imuPitchStart;
_transformSum.rot_z += _imuRollStart;
_systemInited = true;
return;
}2.在点云足够多的条件下，开始正常工作。这里我们设定整个L-M运动估计的迭代次数为25次，以保证运算效率。迭代部分又可分为：对特征边/面上的点进行处理，构建Jaccobian矩阵，L-M运动估计求解。
- 首先得保证这些特征点足够多，否则没有任何特征的点没办法进行匹配， 角特征>10，面特征>100
pcl::PointXYZI coeff;
bool isDegenerate = false;
Eigen::Matrix matP;
_frameCount++;
_transform.pos -= _imuVeloFromStart * _scanPeriod;
size_t lastCornerCloudSize = _lastCornerCloud->points.size();
size_t lastSurfaceCloudSize = _lastSurfaceCloud->points.size();
if (lastCornerCloudSize > 10 && lastSurfaceCloudSize > 100)
{
std::vector pointSearchInd(1);
std::vector pointSearchSqDis(1);
std::vector indices;
pcl::removeNaNFromPointCloud(*_cornerPointsSharp, *_cornerPointsSharp, indices);
size_t cornerPointsSharpNum = _cornerPointsSharp->points.size();
size_t surfPointsFlatNum = _surfPointsFlat->points.size();
_pointSearchCornerInd1.resize(cornerPointsSharpNum);
_pointSearchCornerInd2.resize(cornerPointsSharpNum);
_pointSearchSurfInd1.resize(surfPointsFlatNum);
_pointSearchSurfInd2.resize(surfPointsFlatNum);
_pointSearchSurfInd3.resize(surfPointsFlatNum);
for (size_t iterCount = 0; iterCount < _maxIterations; iterCount++)
{
pcl::PointXYZI pointSel, pointProj, tripod1, tripod2, tripod3;
_laserCloudOri->clear();
_coeffSel->clear();
/***** 1. 特征边上的点配准并构建Jaccobian *****/
/***** 2. 特征面上的点配准并构建Jaccobian *****/
/***** 3. L-M运动估计求解 *****/
}
} - L-M方法其实就是非线性最小二乘，是Gauss-Newton优化的一种改进（增加了一个阻尼因子，代码中的s），所以关键在于如何把点云配准和运动估计的问题转换为L-M优化求解的问题。主要思路就是：构建约束方程 -> 约束方程求偏导构建Jaccobian矩阵 -> L-M求解。
- 下面再一步一步来看：关于构建约束方程的问题就是这节标题中提到的点云配准的问题，其基本思想就是从上一帧点云中找到一些边/面特征点，在当前帧点云中同样找这么一些点，建立他们之间的约束关系。
- 找t+1时刻的某个特征边/面上的点在t时刻下对应的配准点，论文作者给出如上图的思路。特征线：利用KD树找点i在t时刻点云中最近的一点j，并在j周围（上下几条线的范围内）找次近点l，于是我们把（j，l）称为点i在t时刻点云中的对应。特征面：与特征线类似，先找最近点j，在j周围找l，在j周围找m，将（j，l，m）称为点i在t时刻点云中的对应。代码实现如下：
3.特征线上的点配准：
1)将点坐标转换到起始点云坐标系中 pointSel为当前激光的角点
for (int i = 0; i < cornerPointsSharpNum; i++)
{
transformToStart(_cornerPointsSharp->points[i], pointSel);transformToStart 函数的定义为：
void BasicLaserOdometry::transformToStart(const pcl::PointXYZI& pi, pcl::PointXYZI& po)
{
float s = (1.f / _scanPeriod) * (pi.intensity - int(pi.intensity));
po.x = pi.x - s * _transform.pos.x();
po.y = pi.y - s * _transform.pos.y();
po.z = pi.z - s * _transform.pos.z();
po.intensity = pi.intensity;
Angle rx = -s * _transform.rot_x.rad();
Angle ry = -s * _transform.rot_y.rad();
Angle rz = -s * _transform.rot_z.rad();
rotateZXY(po, rz, rx, ry);
}2)每迭代五次,搜索一次最近点和次临近点(降采样)
(计算量的一种平衡吧，每次更新transform后都更新一次最近匹配计算资源消耗大)
if (iterCount % 5 == 0){下面代码}3)找到pointSel(当前时刻边特征中的某一点)在laserCloudCornerLast中的1个最邻近点
pointSearchInd(对应点的索引) , pointSearchSqDis:(pointSel与对应点的欧氏距离)
pcl::removeNaNFromPointCloud(*_lastCornerCloud, *_lastCornerCloud, indices);
_lastCornerKDTree.nearestKSearch(pointSel, 1, pointSearchInd, pointSearchSqDis);template inline
int KdTreeFLANN::nearestKSearch(const PointT &point, int num_closest,
std::vector &k_indices,
std::vector &k_sqr_distances) const
{
k_indices.resize(num_closest);
k_sqr_distances.resize(num_closest);
nanoflann::KNNResultSet resultSet(num_closest);
resultSet.init( k_indices.data(), k_sqr_distances.data());
_kdtree.findNeighbors(resultSet, point.data, nanoflann::SearchParams() );
return resultSet.size();
} 4)如果最临近的点距离小于25时，计算出 搜索到的点 所在线数 （closestPointScan 上一时刻最近点所在的线数）
if (pointSearchSqDis[0] < 25)
{
closestPointInd = pointSearchInd[0];
int closestPointScan = int(_lastSurfaceCloud->points[closestPointInd].intensity);5)从找得到的最邻近点开始，向上遍历3条线特征，找到最近距离以及最近标记 minPointSqDis2 + minPointSqDis2
- 找到与最邻近点相距3条线的特征点时跳出
for (int j = closestPointInd + 1; j < cornerPointsSharpNum; j++)
{
if (int(_lastCornerCloud->points[j].intensity) > closestPointScan + 2.5)
{
break;
}
pointSqDis = calcSquaredDiff(_lastCornerCloud->points[j], pointSel);
if (int(_lastCornerCloud->points[j].intensity) > closestPointScan)
{
if (pointSqDis < minPointSqDis2)
{
minPointSqDis2 = pointSqDis;
minPointInd2 = j;
}
}
}6)向下三条线，找最近点和次临近点
for (int j = closestPointInd - 1; j >= 0; j--)
{
if (int(_lastCornerCloud->points[j].intensity) < closestPointScan - 2.5)
{
break;
}
pointSqDis = calcSquaredDiff(_lastCornerCloud->points[j], pointSel);
if (int(_lastCornerCloud->points[j].intensity) < closestPointScan)
{
if (pointSqDis < minPointSqDis2)
{
minPointSqDis2 = pointSqDis;
minPointInd2 = j;
}
}
}7)获得了临近点和次临近点。 5 + 6 得到了次临近点
当前所有边特征点在上一时刻边特征点云中对应的最邻近点的索引
_pointSearchCornerInd1[i] = closestPointInd;当前所有边特征点在上一时刻边特征点云中对应的次邻近点的索引
_pointSearchCornerInd2[i] = minPointInd2;4.特征线的构建Jaccobian矩阵
1)当前点云坐标pointSel，上一时刻临近点坐标tripod1，上一时刻次次临近点坐标tripod2
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;2）点到直线的距离
中：：
，实际就乘上一个两向量夹角的正弦值 分别作差并叉乘后的向量模长
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)));3)求两点之间的模长
float l12 = sqrt((x1 - x2)*(x1 - x2) + (y1 - y2)*(y1 - y2) + (z1 - z2)*(z1 - z2));
3)向量[la；lb；lc] 为距离l 022分别对x0 y0 z0的偏导 transform的偏导
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;4)计算点到直线的距离，以及偏差
float ld2 = a012 / l12; // Eq. (2)
// TODO: Why writing to a variable that's never read?
pointProj = pointSel;
pointProj.x -= la * ld2;
pointProj.y -= lb * ld2;
pointProj.z -= lc * ld2;5)定义阻尼因子，点到直线距离越小阻尼因子越大 ，计算出偏差 （xyz强度）
float s = 1;
if (iterCount >= 5)
{
s = 1 - 1.8f * fabs(ld2);
}
coeff.x = s * la;
coeff.y = s * lb;
coeff.z = s * lc;
coeff.intensity = s * ld2;6)满足阈值(ld2 < 0.5)，将特征点插入
if (s > 0.1 && ld2 != 0) {
laserCloudOri->push_back(cornerPointsSharp->points[i]);
coeffSel->push_back(coeff);
}5.特征面上点配准
1)遍历平面特征点，pointSel(当前时刻边特征中的某一点)
for (int i = 0; i < surfPointsFlatNum; i++)
{
transformToStart(_surfPointsFlat->points[i], pointSel);
if (iterCount % 5 == 0)
{2)每迭代五次,搜索一次最近点和次临近点(降采样)
if (iterCount % 5 == 0)
{3)找到pointSel(当前时刻边特征中的某一点)在laserCloudCornerLast中的1个最邻近点
pointSearchInd(对应点的索引) , pointSearchSqDis:(pointSel与对应点的欧氏距离)
_lastSurfaceKDTree.nearestKSearch(pointSel, 1, pointSearchInd, pointSearchSqDis);
int closestPointInd = -1, minPointInd2 = -1, minPointInd3 = -1;4)如果最临近的点距离小于25时，计算出 搜索到的点 所在线数 （closestPointScan 上一时刻最近点所在的线数）
if (pointSearchSqDis[0] < 25)
{
closestPointInd = pointSearchInd[0];
int closestPointScan = int(_lastSurfaceCloud->points[closestPointInd].intensity);5)从找得到的最邻近点开始，遍历所有边特征点,找到与最邻近点相距3条线的特征点时跳出
for (int j = closestPointInd + 1; j < surfPointsFlatNum; j++)
{
if (int(_lastSurfaceCloud->points[j].intensity) > closestPointScan + 2.5)
{
break;
}
pointSqDis = calcSquaredDiff(_lastSurfaceCloud->points[j], pointSel);
if (int(_lastSurfaceCloud->points[j].intensity) <= closestPointScan)
{
if (pointSqDis < minPointSqDis2)
{
minPointSqDis2 = pointSqDis;
minPointInd2 = j;
}
}
else
{
if (pointSqDis < minPointSqDis3)
{
minPointSqDis3 = pointSqDis;
minPointInd3 = j;
}
}
}6)向下三条线
for (int j = closestPointInd - 1; j >= 0; j--)
{
if (int(_lastSurfaceCloud->points[j].intensity) < closestPointScan - 2.5)
{
break;
}
pointSqDis = calcSquaredDiff(_lastSurfaceCloud->points[j], pointSel);
if (int(_lastSurfaceCloud->points[j].intensity) >= closestPointScan)
{
if (pointSqDis < minPointSqDis2)
{
minPointSqDis2 = pointSqDis;
minPointInd2 = j;
}
}
else
{
if (pointSqDis < minPointSqDis3)
{
minPointSqDis3 = pointSqDis;
minPointInd3 = j;
}
}
}7)确定目标点
_pointSearchSurfInd1[i] = closestPointInd;
_pointSearchSurfInd2[i] = minPointInd2;
_pointSearchSurfInd3[i] = minPointInd3;6.特征面的构建Jaccobian矩阵 点到面的过程
if (_pointSearchSurfInd2[i] >= 0 && _pointSearchSurfInd3[i] >= 0)
{
tripod1 = _lastSurfaceCloud->points[_pointSearchSurfInd1[i]];
tripod2 = _lastSurfaceCloud->points[_pointSearchSurfInd2[i]];
tripod3 = _lastSurfaceCloud->points[_pointSearchSurfInd3[i]];
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; //Eq. (3)??
// TODO: Why writing to a variable that's never read? Maybe it should be used afterwards?
pointProj = pointSel;
pointProj.x -= pa * pd2;
pointProj.y -= pb * pd2;
pointProj.z -= pc * pd2;
float s = 1;
if (iterCount >= 5)
{
s = 1 - 1.8f * fabs(pd2) / sqrt(calcPointDistance(pointSel));
}
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);
}
}
}至此Jaccobian矩阵就构建完毕了。每个已经匹配的特征_laserCloudOri ；特征点对应的Jaccobian矩阵的三个元素都保存在coeffSel中，后面采用L-M方法解算的时候直接调用就行了。
7.L-M运动估计求解
假设：雷达的运动是连续的。将所有对应到的点求到直线的距离到面的距离之和最短然后按照Levenberg-Marquardt算法迭代计算，得到两帧之间的变换，最后通过累计计算odom。
公式（1,2,3）特征提取与计算误差
我们认为Lidar匀速运动，因此公式(4)实现了Lidar变换矩阵的内插，得到其扫描点i时刻的位姿
公式(5)就是经典的刚体的旋转平移变换。
公式(6)为罗德里格斯公式，将旋转矩阵用旋转矢量表示，这种方法的最大优势在于节约计算所消耗的资源。
公式(7)-(8)分别为旋转矢量公式中的参数计算。搞清楚这些以后，就开始L-M的计算了;
1)匹配到的点的个数(即存在多少个约束)
int pointSelNum = _laserCloudOri->points.size();
if (pointSelNum < 10)
{
continue;
}
Eigen::Matrix matA(pointSelNum, 6);
Eigen::Matrix matAt(6, pointSelNum);
Eigen::Matrix matAtA;
Eigen::VectorXf matB(pointSelNum);
Eigen::Matrix matAtB;
Eigen::Matrix matX; 2)遍历每个对应点，并且构建Jaccobian矩阵
for (int i = 0; i < pointSelNum; i++) {
3)采用Levenberg-Marquardt计算 ，首先建立当前时刻Lidar坐标系下提取到的特征点与点到直线/平面的约束方程。而后对约束方程求对坐标变换(3旋转+3平移)的偏导。公式参见论文(2)-(8)
pointOri 当前时刻点坐标，coeff 该点所对应的偏导数
const pcl::PointXYZI& pointOri = _laserCloudOri->points[i];
coeff = _coeffSel->points[i];
4)具体构建某一点的矩阵分许： arx ary arz 偏导数
float s = 1;
float srx = sin(s * _transform.rot_x.rad());
float crx = cos(s * _transform.rot_x.rad());
float sry = sin(s * _transform.rot_y.rad());
float cry = cos(s * _transform.rot_y.rad());
float srz = sin(s * _transform.rot_z.rad());
float crz = cos(s * _transform.rot_z.rad());
float tx = s * _transform.pos.x();
float ty = s * _transform.pos.y();
float tz = s * _transform.pos.z();
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;
matA(i, 0) = arx;
matA(i, 1) = ary;
matA(i, 2) = arz;
matA(i, 3) = atx;
matA(i, 4) = aty;
matA(i, 5) = atz;
matB(i, 0) = -0.05 * d2;5)最小二乘计算(QR分解法)
matAt = matA.transpose();
matAtA = matAt * matA;
matAtB = matAt * matB;
matX = matAtA.colPivHouseholderQr().solve(matAtB);
if (iterCount == 0)
{
Eigen::Matrix matE;
Eigen::Matrix matV;
Eigen::Matrix matV2;
Eigen::SelfAdjointEigenSolver< Eigen::Matrix > esolver(matAtA);
matE = esolver.eigenvalues().real();
matV = esolver.eigenvectors().real();
matV2 = matV;
isDegenerate = false;
float eignThre[6] = { 10, 10, 10, 10, 10, 10 };
for (int i = 0; i < 6; i++)
{
if (matE(0, i) < eignThre[i])
{
for (int j = 0; j < 6; j++)
{
matV2(i, j) = 0;
}
isDegenerate = true;
}
else
{
break;
}
}
matP = matV.inverse() * matV2;
}
if (isDegenerate)
{
Eigen::Matrix matX2(matX);
matX = matP * matX2;
}
_transform.rot_x = _transform.rot_x.rad() + matX(0, 0);
_transform.rot_y = _transform.rot_y.rad() + matX(1, 0);
_transform.rot_z = _transform.rot_z.rad() + matX(2, 0);
_transform.pos.x() += matX(3, 0);
_transform.pos.y() += matX(4, 0);
_transform.pos.z() += matX(5, 0);
if (!pcl_isfinite(_transform.rot_x.rad())) _transform.rot_x = Angle();
if (!pcl_isfinite(_transform.rot_y.rad())) _transform.rot_y = Angle();
if (!pcl_isfinite(_transform.rot_z.rad())) _transform.rot_z = Angle();
if (!pcl_isfinite(_transform.pos.x())) _transform.pos.x() = 0.0;
if (!pcl_isfinite(_transform.pos.y())) _transform.pos.y() = 0.0;
if (!pcl_isfinite(_transform.pos.z())) _transform.pos.z() = 0.0;
float deltaR = sqrt(pow(rad2deg(matX(0, 0)), 2) +
pow(rad2deg(matX(1, 0)), 2) +
pow(rad2deg(matX(2, 0)), 2));
float deltaT = sqrt(pow(matX(3, 0) * 100, 2) +
pow(matX(4, 0) * 100, 2) +
pow(matX(5, 0) * 100, 2));
if (deltaR < _deltaRAbort && deltaT < _deltaTAbort)
break; 8.坐标转换
算出了点云间的相对运动，但他们是在这两帧点云的局部坐标系下的，我们需要把它转换到世界坐标系下，因此需要进行转换
Angle rx, ry, rz;
accumulateRotation(_transformSum.rot_x,
_transformSum.rot_y,
_transformSum.rot_z,
-_transform.rot_x,
-_transform.rot_y.rad() * 1.05,
-_transform.rot_z,
rx, ry, rz);
Vector3 v(_transform.pos.x() - _imuShiftFromStart.x(),
_transform.pos.y() - _imuShiftFromStart.y(),
_transform.pos.z() * 1.05 - _imuShiftFromStart.z());
rotateZXY(v, rz, rx, ry);
Vector3 trans = _transformSum.pos - v;
pluginIMURotation(rx, ry, rz,
_imuPitchStart, _imuYawStart, _imuRollStart,
_imuPitchEnd, _imuYawEnd, _imuRollEnd,
rx, ry, rz);
_transformSum.rot_x = rx;
_transformSum.rot_y = ry;
_transformSum.rot_z = rz;
_transformSum.pos = trans;
transformToEnd(_cornerPointsLessSharp);
transformToEnd(_surfPointsLessFlat);
_cornerPointsLessSharp.swap(_lastCornerCloud);
_surfPointsLessFlat.swap(_lastSurfaceCloud);
lastCornerCloudSize = _lastCornerCloud->points.size();
lastSurfaceCloudSize = _lastSurfaceCloud->points.size();
if (lastCornerCloudSize > 10 && lastSurfaceCloudSize > 100)
{
_lastCornerKDTree.setInputCloud(_lastCornerCloud);
_lastSurfaceKDTree.setInputCloud(_lastSurfaceCloud);
}计算出总的转换 _transformSum
总结：
线特征
1. 线特征首先基于 KD tree 找到最近的一个点
2. 然后基于 该点找到临近 6条线上的次近点 (不在本条线)
3. 然后计算点到直线 及为 代价函数
4. 基于代价函数求解jacobi
5. LM 引入阻尼因子
面特征：
1. 面特征也是基于 KD tree 找到最近的一个点
2. 基于该点在 本条线上找到一个最近点，同时在 非本条线的最近6条线找到次近点
3. 基于 这3个点构造点到平面的距离 及为 代价函数
4. 基于代价函数求解jacobi
5. LM 引入阻尼因子
LM 求解 相对关系：
1. 默认为 匀速运动，各个特征与转换t 存在线性关系(基于时间成比例)
2. 总体求解Jacobian matrix ，使用 引入 
阻尼因子 非线性求解 odom 。