lio_sam代码阅读之mapOptimization
Posted COCO_PEAK_NOODLE
tags:
篇首语：本文由小常识网(cha138.com)小编为大家整理，主要介绍了lio_sam代码阅读之mapOptimization相关的知识，希望对你有一定的参考价值。
基本转自别人
/**
* 当前帧位姿初始化
* 1、如果是第一帧，用原始imu数据的RPY初始化当前帧位姿（旋转部分）
* 2、后续帧，用imu里程计计算两帧之间的增量位姿变换，作用于前一帧的激光位姿，得到当前帧激光位姿
*/
void updateInitialGuess()
{
// 前一帧的位姿，注：这里指lidar的位姿，后面都简写成位姿
//transformTobeMapped 初始值为0;
incrementalOdometryAffineFront = trans2Affine3f(transformTobeMapped);
// 前一帧的初始化姿态角（来自原始imu数据），用于估计第一帧的位姿（旋转部分）
static Eigen::Affine3f lastImuTransformation;
// 如果关键帧集合为空，继续进行初始化
if (cloudKeyPoses3D->points.empty())
{
// 当前帧位姿的旋转部分，用激光帧信息中的RPY（来自imu原始数据）初始化
transformTobeMapped[0] = cloudInfo.imuRollInit;
transformTobeMapped[1] = cloudInfo.imuPitchInit;
transformTobeMapped[2] = cloudInfo.imuYawInit;
if (!useImuHeadingInitialization)
transformTobeMapped[2] = 0;
lastImuTransformation = pcl::getTransformation(0, 0, 0, cloudInfo.imuRollInit, cloudInfo.imuPitchInit, cloudInfo.imuYawInit);
return;
}
// 用当前帧和前一帧对应的imu里程计计算相对位姿变换，再用前一帧的位姿与相对变换，计算当前帧的位姿，存transformTobeMapped
static bool lastImuPreTransAvailable = false;
static Eigen::Affine3f lastImuPreTransformation;
if (cloudInfo.odomAvailable == true)
{
// 当前帧的初始估计位姿（来自imu里程计），后面用来计算增量位姿变换
Eigen::Affine3f transBack = pcl::getTransformation(cloudInfo.initialGuessX, cloudInfo.initialGuessY, cloudInfo.initialGuessZ,
cloudInfo.initialGuessRoll, cloudInfo.initialGuessPitch, cloudInfo.initialGuessYaw);
if (lastImuPreTransAvailable == false)
{
// 赋值给前一帧
lastImuPreTransformation = transBack;
lastImuPreTransAvailable = true;
} else {
// 当前帧相对于前一帧的位姿变换，imu里程计计算得到
Eigen::Affine3f transIncre = lastImuPreTransformation.inverse() * transBack;
// 前一帧的位姿
Eigen::Affine3f transTobe = trans2Affine3f(transformTobeMapped);
// 当前帧的位姿
Eigen::Affine3f transFinal = transTobe * transIncre;
// 更新当前帧位姿transformTobeMapped
pcl::getTranslationAndEulerAngles(transFinal, transformTobeMapped[3], transformTobeMapped[4], transformTobeMapped[5],
transformTobeMapped[0], transformTobeMapped[1], transformTobeMapped[2]);
// 赋值给前一帧
lastImuPreTransformation = transBack;
lastImuTransformation = pcl::getTransformation(0, 0, 0, cloudInfo.imuRollInit, cloudInfo.imuPitchInit, cloudInfo.imuYawInit); // save imu before return;
return;
}
}
// 只在第一帧调用（注意上面的return），用imu数据初始化当前帧位姿，仅初始化旋转部分
if (cloudInfo.imuAvailable == true)
{
// 当前帧的姿态角（来自原始imu数据）
Eigen::Affine3f transBack = pcl::getTransformation(0, 0, 0, cloudInfo.imuRollInit, cloudInfo.imuPitchInit, cloudInfo.imuYawInit);
// 当前帧相对于前一帧的姿态变换
Eigen::Affine3f transIncre = lastImuTransformation.inverse() * transBack;
// 前一帧的位姿
Eigen::Affine3f transTobe = trans2Affine3f(transformTobeMapped);
// 当前帧的位姿
Eigen::Affine3f transFinal = transTobe * transIncre;
// 更新当前帧位姿transformTobeMapped
pcl::getTranslationAndEulerAngles(transFinal, transformTobeMapped[3], transformTobeMapped[4], transformTobeMapped[5],
transformTobeMapped[0], transformTobeMapped[1], transformTobeMapped[2]);
lastImuTransformation = pcl::getTransformation(0, 0, 0, cloudInfo.imuRollInit, cloudInfo.imuPitchInit, cloudInfo.imuYawInit); // save imu before return;
return;
}
}
/**
* 提取局部角点、平面点云集合，加入局部map
* 1、对最近的一帧关键帧，搜索时空维度上相邻的关键帧集合，降采样一下
* 2、对关键帧集合中的每一帧，提取对应的角点、平面点，加入局部map中
*/
void extractSurroundingKeyFrames()
{
if (cloudKeyPoses3D->points.empty() == true)
return;
// if (loopClosureEnableFlag == true)
// {
// extractForLoopClosure();
// } else {
// extractNearby();
// }
// 提取局部角点、平面点云集合，加入局部map
// 1、对最近的一帧关键帧，搜索时空维度上相邻的关键帧集合，降采样一下
// 2、对关键帧集合中的每一帧，提取对应的角点、平面点，加入局部map中
extractNearby();
}
void extractNearby()
{
//这里面存放的是关键帧的位姿点，用一个3d点表示
pcl::PointCloud<PointType>::Ptr surroundingKeyPoses(new pcl::PointCloud<PointType>());
//这里是降采样的3d点
pcl::PointCloud<PointType>::Ptr surroundingKeyPosesDS(new pcl::PointCloud<PointType>());
//下面这些参数是pcl中保存结果的变量
std::vector<int> pointSearchInd;
std::vector<float> pointSearchSqDis;
// kdtree的输入，全局关键帧位姿集合（历史所有关键帧集合）
kdtreeSurroundingKeyPoses->setInputCloud(cloudKeyPoses3D);
// 对最近的一帧关键帧，在半径区域内搜索空间区域上相邻的关键帧集合
// surroundingKeyframeSearchRadius = 50；
kdtreeSurroundingKeyPoses->radiusSearch(cloudKeyPoses3D->back(), (double)surroundingKeyframeSearchRadius, pointSearchInd, pointSearchSqDis);
// 遍历搜索结果，pointSearchInd存的是结果在cloudKeyPoses3D下面的索引
for (int i = 0; i < (int)pointSearchInd.size(); ++i)
{
int id = pointSearchInd[i];
// 加入相邻关键帧位姿集合中
surroundingKeyPoses->push_back(cloudKeyPoses3D->points[id]);
}
// 降采样一下
downSizeFilterSurroundingKeyPoses.setInputCloud(surroundingKeyPoses);
downSizeFilterSurroundingKeyPoses.filter(*surroundingKeyPosesDS);
// 加入时间上相邻的一些关键帧，比如当载体在原地转圈，这些帧加进来是合理的
int numPoses = cloudKeyPoses3D->size();
for (int i = numPoses-1; i >= 0; --i)
{
if (timeLaserInfoCur - cloudKeyPoses6D->points[i].time < 10.0)
surroundingKeyPosesDS->push_back(cloudKeyPoses3D->points[i]);
else
break;
}
// 将相邻关键帧集合对应的角点、平面点，加入到局部map中，作为scan-to-map匹配的局部点云地图
extractCloud(surroundingKeyPosesDS);
}
void extractCloud(pcl::PointCloud<PointType>::Ptr cloudToExtract)
{
// 相邻关键帧集合对应的角点、平面点，加入到局部map中；注：称之为局部map，后面进行scan-to-map匹配，所用到的map就是这里的相邻关键帧对应点云集合
laserCloudCornerFromMap->clear();
laserCloudSurfFromMap->clear();
// 遍历当前帧（实际是取最近的一个关键帧来找它相邻的关键帧集合）时空维度上相邻的关键帧集合
for (int i = 0; i < (int)cloudToExtract->size(); ++i)
{
// 距离超过阈值，丢弃
if (pointDistance(cloudToExtract->points[i], cloudKeyPoses3D->back()) > surroundingKeyframeSearchRadius)
continue;
// 相邻关键帧索引,之所以保存到laserCloudMapContainer变量中，是为了牺牲空间节省查找时间
int thisKeyInd = (int)cloudToExtract->points[i].intensity;
if (laserCloudMapContainer.find(thisKeyInd) != laserCloudMapContainer.end())
{
*laserCloudCornerFromMap += laserCloudMapContainer[thisKeyInd].first;
*laserCloudSurfFromMap += laserCloudMapContainer[thisKeyInd].second;
} else {
// 相邻关键帧对应的角点、平面点云，通过6D位姿变换到世界坐标系下
pcl::PointCloud<PointType> laserCloudCornerTemp = *transformPointCloud(cornerCloudKeyFrames[thisKeyInd], &cloudKeyPoses6D->points[thisKeyInd]);
pcl::PointCloud<PointType> laserCloudSurfTemp = *transformPointCloud(surfCloudKeyFrames[thisKeyInd], &cloudKeyPoses6D->points[thisKeyInd]);
// 加入局部map
*laserCloudCornerFromMap += laserCloudCornerTemp;
*laserCloudSurfFromMap += laserCloudSurfTemp;
laserCloudMapContainer[thisKeyInd] = make_pair(laserCloudCornerTemp, laserCloudSurfTemp);
}
}
// 降采样局部角点map
downSizeFilterCorner.setInputCloud(laserCloudCornerFromMap);
downSizeFilterCorner.filter(*laserCloudCornerFromMapDS);
laserCloudCornerFromMapDSNum = laserCloudCornerFromMapDS->size();
// 降采样局部平面点map
downSizeFilterSurf.setInputCloud(laserCloudSurfFromMap);
downSizeFilterSurf.filter(*laserCloudSurfFromMapDS);
laserCloudSurfFromMapDSNum = laserCloudSurfFromMapDS->size();
// 太大了，清空一下内存
if (laserCloudMapContainer.size() > 1000)
laserCloudMapContainer.clear();
}
/**
* 当前激光帧角点、平面点集合降采样
*/
void downsampleCurrentScan()
{
// 当前激光帧角点集合降采样
laserCloudCornerLastDS->clear();
downSizeFilterCorner.setInputCloud(laserCloudCornerLast);
downSizeFilterCorner.filter(*laserCloudCornerLastDS);
laserCloudCornerLastDSNum = laserCloudCornerLastDS->size();
// 当前激光帧平面点集合降采样
laserCloudSurfLastDS->clear();
downSizeFilterSurf.setInputCloud(laserCloudSurfLast);
downSizeFilterSurf.filter(*laserCloudSurfLastDS);
laserCloudSurfLastDSNum = laserCloudSurfLastDS->size();
}
/**
* scan-to-map优化当前帧位姿
* 1、要求当前帧特征点数量足够多，且匹配的点数够多，才执行优化
* 2、迭代30次（上限）优化
* 1) 当前激光帧角点寻找局部map匹配点
* a.更新当前帧位姿，将当前帧角点坐标变换到map系下，在局部map中查找5个最近点，距离小于1m，且5个点构成直线（用距离中心点的协方差矩阵，特征值进行判断），则认为匹配上了
* b.计算当前帧角点到直线的距离、垂线的单位向量，存储为角点参数
* 2) 当前激光帧平面点寻找局部map匹配点
* a.更新当前帧位姿，将当前帧平面点坐标变换到map系下，在局部map中查找5个最近点，距离小于1m，且5个点构成平面（最小二乘拟合平面），则认为匹配上了
* b.计算当前帧平面点到平面的距离、垂线的单位向量，存储为平面点参数
* 3) 提取当前帧中与局部map匹配上了的角点、平面点，加入同一集合
* 4) 对匹配特征点计算Jacobian矩阵，观测值为特征点到直线、平面的距离，构建高斯牛顿方程，迭代优化当前位姿，存transformTobeMapped
* 3、用imu原始RPY数据与scan-to-map优化后的位姿进行加权融合，更新当前帧位姿的roll、pitch，约束z坐标
*/
void scan2MapOptimization()
{
// 要求有关键帧
if (cloudKeyPoses3D->points.empty())
return;
// 当前激光帧的角点、平面点数量足够多
// edgeFeatureMinValidNum = 10；
// surfFeatureMinValidNum = 100；
if (laserCloudCornerLastDSNum > edgeFeatureMinValidNum && laserCloudSurfLastDSNum > surfFeatureMinValidNum)
{
// kdtree输入为局部map点云
kdtreeCornerFromMap->setInputCloud(laserCloudCornerFromMapDS);
kdtreeSurfFromMap->setInputCloud(laserCloudSurfFromMapDS);
// 迭代30次
for (int iterCount = 0; iterCount < 30; iterCount++)
{
// 每次迭代清空特征点集合
laserCloudOri->clear();
coeffSel->clear();
// 当前激光帧角点寻找局部map匹配点
// 1、更新当前帧位姿，将当前帧角点坐标变换到map系下，在局部map中查找5个最近点，距离小于1m，且5个点构成直线（用距离中心点的协方差矩阵，特征值进行判断），则认为匹配上了
// 2、计算当前帧角点到直线的距离、垂线的单位向量，存储为角点参数
cornerOptimization();
// 当前激光帧平面点寻找局部map匹配点
// 1、更新当前帧位姿，将当前帧平面点坐标变换到map系下，在局部map中查找5个最近点，距离小于1m，且5个点构成平面（最小二乘拟合平面），则认为匹配上了
// 2、计算当前帧平面点到平面的距离、垂线的单位向量，存储为平面点参数
surfOptimization();
// 提取当前帧中与局部map匹配上了的角点、平面点，加入同一集合
combineOptimizationCoeffs();
// scan-to-map优化
// 对匹配特征点计算Jacobian矩阵，观测值为特征点到直线、平面的距离，构建高斯牛顿方程，迭代优化当前位姿，存transformTobeMapped
if (LMOptimization(iterCount) == true)
break;
}
// 用imu原始RPY数据与scan-to-map优化后的位姿进行加权融合，更新当前帧位姿的roll、pitch，约束z坐标
transformUpdate();
} else {
ROS_WARN("Not enough features! Only %d edge and %d planar features available.", laserCloudCornerLastDSNum, laserCloudSurfLastDSNum);
}
}
/**
* 当前激光帧角点寻找局部map匹配点
* 1、更新当前帧位姿，将当前帧角点坐标变换到map系下，在局部map中查找5个最近点，距离小于1m，且5个点构成直线（用距离中心点的协方差矩阵，特征值进行判断），则认为匹配上了
* 2、计算当前帧角点到直线的距离、垂线的单位向量，存储为角点参数
*/
void cornerOptimization()
{
// 更新当前帧位姿
updatePointAssociateToMap();
// 遍历当前帧角点集合
// numberOfCores = 4;
#pragma omp parallel for num_threads(numberOfCores)
for (int i = 0; i < laserCloudCornerLastDSNum; i++)
{
PointType pointOri, pointSel, coeff;
std::vector<int> pointSearchInd;
std::vector<float> pointSearchSqDis;
// 角点（坐标还是lidar系）
pointOri = laserCloudCornerLastDS->points[i];
// 根据当前帧位姿，变换到世界坐标系（map系）下
pointAssociateToMap(&pointOri, &pointSel);
// 在局部角点map中查找当前角点相邻的5个角点
kdtreeCornerFromMap->nearestKSearch(pointSel, 5, pointSearchInd, pointSearchSqDis);
cv::Mat matA1(3, 3, CV_32F, cv::Scalar::all(0));
cv::Mat matD1(1, 3, CV_32F, cv::Scalar::all(0));
cv::Mat matV1(3, 3, CV_32F, cv::Scalar::all(0));
// 要求距离都小于1m
if (pointSearchSqDis[4] < 1.0) {
// 计算5个点的均值坐标，记为中心点
float cx = 0, cy = 0, cz = 0;
for (int j = 0; j < 5; j++) {
cx += laserCloudCornerFromMapDS->points[pointSearchInd[j]].x;
cy += laserCloudCornerFromMapDS->points[pointSearchInd[j]].y;
cz += laserCloudCornerFromMapDS->points[pointSearchInd[j]].z;
}
cx /= 5; cy /= 5; cz /= 5;
// 计算协方差
float a11 = 0, a12 = 0, a13 = 0, a22 = 0, a23 = 0, a33 = 0;
for (int j = 0; j < 5; j++) {
// 计算点与中心点之间的距离
float ax = laserCloudCornerFromMapDS->points[pointSearchInd[j]].x - cx;
float ay = laserCloudCornerFromMapDS->points[pointSearchInd[j]].y - cy;
float az = laserCloudCornerFromMapDS->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;
// 构建协方差矩阵
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, 0lio_sam代码阅读之imuPreintegration