基本转自别人
/**
* 当前帧位姿初始化
* 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, 0) = a13; matA1.at<float>(2, 1) = a23; matA1.at<float>(2, 2) = a33;
// 特征值分解
cv::eigen(matA1, matD1, matV1);
// 如果最大的特征值相比次大特征值，大很多，认为构成了线，角点是合格的
if (matD1.at<float>(0, 0) > 3 * matD1.at<float>(0, 1)) {
// 当前帧角点坐标（map系下）
float x0 = pointSel.x;
float y0 = pointSel.y;
float z0 = pointSel.z;
// 局部map对应中心角点，沿着特征向量（直线方向）方向，前后各取一个点
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);
// area_012，也就是三个点组成的三角形面积*2，叉积的模|axb|=a*b*sin(theta)
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)));
// line_12，底边边长
float l12 = sqrt((x1 - x2)*(x1 - x2) + (y1 - y2)*(y1 - y2) + (z1 - z2)*(z1 - z2));
// 两次叉积，得到点到直线的垂线段单位向量，x分量，下面同理
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;
// 距离越大，s越小，是个距离惩罚因子（权重）
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) {
// 当前激光帧角点，加入匹配集合中
laserCloudOriCornerVec[i] = pointOri;
// 角点的参数
coeffSelCornerVec[i] = coeff;
laserCloudOriCornerFlag[i] = true;
}
}
}
}
}/**
* 当前激光帧平面点寻找局部map匹配点
* 1、更新当前帧位姿，将当前帧平面点坐标变换到map系下，在局部map中查找5个最近点，距离小于1m，且5个点构成平面（最小二乘拟合平面），则认为匹配上了
* 2、计算当前帧平面点到平面的距离、垂线的单位向量，存储为平面点参数
*/void surfOptimization()
{
// 更新当前帧位姿
updatePointAssociateToMap();
// 遍历当前帧平面点集合
#pragma omp parallel for num_threads(numberOfCores)
for (int i = 0; i < laserCloudSurfLastDSNum; i++)
{
PointType pointOri, pointSel, coeff;
std::vector<int> pointSearchInd;
std::vector<float> pointSearchSqDis;
// 平面点（坐标还是lidar系）
pointOri = laserCloudSurfLastDS->points[i];
// 根据当前帧位姿，变换到世界坐标系（map系）下
pointAssociateToMap(&pointOri, &pointSel);
// 在局部平面点map中查找当前平面点相邻的5个平面点
kdtreeSurfFromMap->nearestKSearch(pointSel, 5, pointSearchInd, pointSearchSqDis);
Eigen::Matrix<float, 5, 3> matA0;
Eigen::Matrix<float, 5, 1> matB0;
Eigen::Vector3f matX0;
matA0.setZero();
matB0.fill(-1);
matX0.setZero();
// 要求距离都小于1m
if (pointSearchSqDis[4] < 1.0) {
for (int j = 0; j < 5; j++) {
matA0(j, 0) = laserCloudSurfFromMapDS->points[pointSearchInd[j]].x;
matA0(j, 1) = laserCloudSurfFromMapDS->points[pointSearchInd[j]].y;
matA0(j, 2) = laserCloudSurfFromMapDS->points[pointSearchInd[j]].z;
}
// 假设平面方程为ax+by+cz+1=0，这里就是求方程的系数abc，d=1
matX0 = matA0.colPivHouseholderQr().solve(matB0);
// 平面方程的系数，也是法向量的分量
float pa = matX0(0, 0);
float pb = matX0(1, 0);
float pc = matX0(2, 0);
float pd = 1;
// 单位法向量
float ps = sqrt(pa * pa + pb * pb + pc * pc);
pa /= ps; pb /= ps; pc /= ps; pd /= ps;
// 检查平面是否合格，如果5个点中有点到平面的距离超过0.2m，那么认为这些点太分散了，不构成平面
bool planeValid = true;
for (int j = 0; j < 5; j++) {
if (fabs(pa * laserCloudSurfFromMapDS->points[pointSearchInd[j]].x +
pb * laserCloudSurfFromMapDS->points[pointSearchInd[j]].y +
pc * laserCloudSurfFromMapDS->points[pointSearchInd[j]].z + pd) > 0.2) {
planeValid = false;
break;
}
}
// 平面合格
if (planeValid) {
// 当前激光帧点到平面距离
float pd2 = pa * pointSel.x + pb * pointSel.y + pc * pointSel.z + pd;
float s = 1 - 0.9 * 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) {
// 当前激光帧平面点，加入匹配集合中
laserCloudOriSurfVec[i] = pointOri;
// 参数
coeffSelSurfVec[i] = coeff;
laserCloudOriSurfFlag[i] = true;
}
}
}
}
}/**
* scan-to-map优化
* 对匹配特征点计算Jacobian矩阵，观测值为特征点到直线、平面的距离，构建高斯牛顿方程，迭代优化当前位姿，存transformTobeMapped
* 公式推导：todo
*/bool LMOptimization(int iterCount)
{
// This optimization is from the original loam_velodyne by Ji Zhang, need to cope with coordinate transformation
// lidar <- camera --- camera <- lidar
// x = z --- x = y
// y = x --- y = z
// z = y --- z = x
// roll = yaw --- roll = pitch
// pitch = roll --- pitch = yaw
// yaw = pitch --- yaw = roll
// lidar -> camera
float srx = sin(transformTobeMapped[1]);
float crx = cos(transformTobeMapped[1]);
float sry = sin(transformTobeMapped[2]);
float cry = cos(transformTobeMapped[2]);
float srz = sin(transformTobeMapped[0]);
float crz = cos(transformTobeMapped[0]);
// 当前帧匹配特征点数太少
int laserCloudSelNum = laserCloudOri->size();
if (laserCloudSelNum < 50) {
return false;
}
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));
PointType pointOri, coeff;
// 遍历匹配特征点，构建Jacobian矩阵
for (int i = 0; i < laserCloudSelNum; i++) {
// lidar -> camera todo
pointOri.x = laserCloudOri->points[i].y;
pointOri.y = laserCloudOri->points[i].z;
pointOri.z = laserCloudOri->points[i].x;
// lidar -> camera
coeff.x = coeffSel->points[i].y;
coeff.y = coeffSel->points[i].z;
coeff.z = coeffSel->points[i].x;
coeff.intensity = coeffSel->points[i].intensity;
// in camera
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;
// lidar -> camera
matA.at<float>(i, 0) = arz;
matA.at<float>(i, 1) = arx;
matA.at<float>(i, 2) = ary;
matA.at<float>(i, 3) = coeff.z;
matA.at<float>(i, 4) = coeff.x;
matA.at<float>(i, 5) = coeff.y;
// 点到直线距离、平面距离，作为观测值
matB.at<float>(i, 0) = -coeff.intensity;
}
cv::transpose(matA, matAt);
matAtA = matAt * matA;
matAtB = matAt * matB;
// J^T·J·delta_x = -J^T·f 高斯牛顿
cv::solve(matAtA, matAtB, matX, cv::DECOMP_QR);
// 首次迭代，检查近似Hessian矩阵（J^T·J）是否退化，或者称为奇异，行列式值=0 todo
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<float>(0, i) < eignThre[i]) {
for (int j = 0; j < 6; j++) {
matV2.at<float>(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;
}
// 更新当前位姿 x = x + delta_x
transformTobeMapped[0] += matX.at<float>(0, 0);
transformTobeMapped[1] += matX.at<float>(1, 0);
transformTobeMapped[2] += matX.at<float>(2, 0);
transformTobeMapped[3] += matX.at<float>(3, 0);
transformTobeMapped[4] += matX.at<float>(4, 0);
transformTobeMapped[5] += matX.at<float>(5, 0);
float deltaR = sqrt(
pow(pcl::rad2deg(matX.at<float>(0, 0)), 2) +
pow(pcl::rad2deg(matX.at<float>(1, 0)), 2) +
pow(pcl::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));
// delta_x很小，认为收敛
if (deltaR < 0.05 && deltaT < 0.05) {
return true;
}
return false;
}/**
* 设置当前帧为关键帧并执行因子图优化
* 1、计算当前帧与前一帧位姿变换，如果变化太小，不设为关键帧，反之设为关键帧
* 2、添加激光里程计因子、GPS因子、闭环因子
* 3、执行因子图优化
* 4、得到当前帧优化后位姿，位姿协方差
* 5、添加cloudKeyPoses3D，cloudKeyPoses6D，更新transformTobeMapped，添加当前关键帧的角点、平面点集合
*/void saveKeyFramesAndFactor()
{
// 计算当前帧与前一帧位姿变换，如果变化太小，不设为关键帧，反之设为关键帧
if (saveFrame() == false)
return;
// 激光里程计因子
addOdomFactor();
// GPS因子
addGPSFactor();
// 闭环因子
addLoopFactor();
// cout << "****************************************************" << endl;
// gtSAMgraph.print("GTSAM Graph:\n");
// 执行优化
isam->update(gtSAMgraph, initialEstimate);
isam->update();
if (aLoopIsClosed == true)
{
isam->update();
isam->update();
isam->update();
isam->update();
isam->update();
}
// update之后要清空一下保存的因子图，注：历史数据不会清掉，ISAM保存起来了
gtSAMgraph.resize(0);
initialEstimate.clear();
PointType thisPose3D;
PointTypePose thisPose6D;
Pose3 latestEstimate;
// 优化结果
isamCurrentEstimate = isam->calculateEstimate();
// 当前帧位姿结果
latestEstimate = isamCurrentEstimate.at<Pose3>(isamCurrentEstimate.size()-1);
// cout << "****************************************************" << endl;
// isamCurrentEstimate.print("Current estimate: ");
// cloudKeyPoses3D加入当前帧位姿
thisPose3D.x = latestEstimate.translation().x();
thisPose3D.y = latestEstimate.translation().y();
thisPose3D.z = latestEstimate.translation().z();
// 索引
thisPose3D.intensity = cloudKeyPoses3D->size();
cloudKeyPoses3D->push_back(thisPose3D);
// cloudKeyPoses6D加入当前帧位姿
thisPose6D.x = thisPose3D.x;
thisPose6D.y = thisPose3D.y;
thisPose6D.z = thisPose3D.z;
thisPose6D.intensity = thisPose3D.intensity ;
thisPose6D.roll = latestEstimate.rotation().roll();
thisPose6D.pitch = latestEstimate.rotation().pitch();
thisPose6D.yaw = latestEstimate.rotation().yaw();
thisPose6D.time = timeLaserInfoCur;
cloudKeyPoses6D->push_back(thisPose6D);
// cout << "****************************************************" << endl;
// cout << "Pose covariance:" << endl;
// cout << isam->marginalCovariance(isamCurrentEstimate.size()-1) << endl << endl;
// 位姿协方差
poseCovariance = isam->marginalCovariance(isamCurrentEstimate.size()-1);
// transformTobeMapped更新当前帧位姿
transformTobeMapped[0] = latestEstimate.rotation().roll();
transformTobeMapped[1] = latestEstimate.rotation().pitch();
transformTobeMapped[2] = latestEstimate.rotation().yaw();
transformTobeMapped[3] = latestEstimate.translation().x();
transformTobeMapped[4] = latestEstimate.translation().y();
transformTobeMapped[5] = latestEstimate.translation().z();
// 当前帧激光角点、平面点，降采样集合
pcl::PointCloud<PointType>::Ptr thisCornerKeyFrame(new pcl::PointCloud<PointType>());
pcl::PointCloud<PointType>::Ptr thisSurfKeyFrame(new pcl::PointCloud<PointType>());
pcl::copyPointCloud(*laserCloudCornerLastDS, *thisCornerKeyFrame);
pcl::copyPointCloud(*laserCloudSurfLastDS, *thisSurfKeyFrame);
// 保存特征点降采样集合
cornerCloudKeyFrames.push_back(thisCornerKeyFrame);
surfCloudKeyFrames.push_back(thisSurfKeyFrame);
// 更新里程计轨迹
updatePath(thisPose6D);
}/**
* 更新因子图中所有变量节点的位姿，也就是所有历史关键帧的位姿，更新里程计轨迹
*/void correctPoses()
{
if (cloudKeyPoses3D->points.empty())
return;
if (aLoopIsClosed == true)
{
// 清空局部map
laserCloudMapContainer.clear();
// 清空里程计轨迹
globalPath.poses.clear();
// 更新因子图中所有变量节点的位姿，也就是所有历史关键帧的位姿
int numPoses = isamCurrentEstimate.size();
for (int i = 0; i < numPoses; ++i)
{
cloudKeyPoses3D->points[i].x = isamCurrentEstimate.at<Pose3>(i).translation().x();
cloudKeyPoses3D->points[i].y = isamCurrentEstimate.at<Pose3>(i).translation().y();
cloudKeyPoses3D->points[i].z = isamCurrentEstimate.at<Pose3>(i).translation().z();
cloudKeyPoses6D->points[i].x = cloudKeyPoses3D->points[i].x;
cloudKeyPoses6D->points[i].y = cloudKeyPoses3D->points[i].y;
cloudKeyPoses6D->points[i].z = cloudKeyPoses3D->points[i].z;
cloudKeyPoses6D->points[i].roll = isamCurrentEstimate.at<Pose3>(i).rotation().roll();
cloudKeyPoses6D->points[i].pitch = isamCurrentEstimate.at<Pose3>(i).rotation().pitch();
cloudKeyPoses6D->points[i].yaw = isamCurrentEstimate.at<Pose3>(i).rotation().yaw();
// 更新里程计轨迹
updatePath(cloudKeyPoses6D->points[i]);
}
aLoopIsClosed = false;
}
}/**
* 发布激光里程计
*/void publishOdometry()
{
// 发布激光里程计，odom等价map
nav_msgs::Odometry laserOdometryROS;
laserOdometryROS.header.stamp = timeLaserInfoStamp;
laserOdometryROS.header.frame_id = odometryFrame;
laserOdometryROS.child_frame_id = "odom_mapping";
laserOdometryROS.pose.pose.position.x = transformTobeMapped[3];
laserOdometryROS.pose.pose.position.y = transformTobeMapped[4];
laserOdometryROS.pose.pose.position.z = transformTobeMapped[5];
laserOdometryROS.pose.pose.orientation = tf::createQuaternionMsgFromRollPitchYaw(transformTobeMapped[0], transformTobeMapped[1], transformTobeMapped[2]);
pubLaserOdometryGlobal.publish(laserOdometryROS);
// 发布TF，odom->lidar
static tf::TransformBroadcaster br;
tf::Transform t_odom_to_lidar = tf::Transform(tf::createQuaternionFromRPY(transformTobeMapped[0], transformTobeMapped[1], transformTobeMapped[2]),
tf::Vector3(transformTobeMapped[3], transformTobeMapped[4], transformTobeMapped[5]));
tf::StampedTransform trans_odom_to_lidar = tf::StampedTransform(t_odom_to_lidar, timeLaserInfoStamp, odometryFrame, "lidar_link");
br.sendTransform(trans_odom_to_lidar);
// Publish odometry for ROS (incremental)
static bool lastIncreOdomPubFlag = false;
static nav_msgs::Odometry laserOdomIncremental;
static Eigen::Affine3f increOdomAffine;
if (lastIncreOdomPubFlag == false)
{
lastIncreOdomPubFlag = true;
laserOdomIncremental = laserOdometryROS;
increOdomAffine = trans2Affine3f(transformTobeMapped);
} else {
// 当前帧与前一帧之间的位姿变换
Eigen::Affine3f affineIncre = incrementalOdometryAffineFront.inverse() * incrementalOdometryAffineBack;
// 还是当前帧位姿（打印一下数据，可以看到与激光里程计的位姿transformTobeMapped是一样的）
increOdomAffine = increOdomAffine * affineIncre;
float x, y, z, roll, pitch, yaw;
pcl::getTranslationAndEulerAngles (increOdomAffine, x, y, z, roll, pitch, yaw);
if (cloudInfo.imuAvailable == true)
{
if (std::abs(cloudInfo.imuPitchInit) < 1.4)
{
double imuWeight = 0.1;
tf::Quaternion imuQuaternion;
tf::Quaternion transformQuaternion;
double rollMid, pitchMid, yawMid;
// roll姿态角加权平均
transformQuaternion.setRPY(roll, 0, 0);
imuQuaternion.setRPY(cloudInfo.imuRollInit, 0, 0);
tf::Matrix3x3(transformQuaternion.slerp(imuQuaternion, imuWeight)).getRPY(rollMid, pitchMid, yawMid);
roll = rollMid;
// pitch姿态角加权平均
transformQuaternion.setRPY(0, pitch, 0);
imuQuaternion.setRPY(0, cloudInfo.imuPitchInit, 0);
tf::Matrix3x3(transformQuaternion.slerp(imuQuaternion, imuWeight)).getRPY(rollMid, pitchMid, yawMid);
pitch = pitchMid;
}
}
laserOdomIncremental.header.stamp = timeLaserInfoStamp;
laserOdomIncremental.header.frame_id = odometryFrame;
laserOdomIncremental.child_frame_id = "odom_mapping";
laserOdomIncremental.pose.pose.position.x = x;
laserOdomIncremental.pose.pose.position.y = y;
laserOdomIncremental.pose.pose.position.z = z;
laserOdomIncremental.pose.pose.orientation = tf::createQuaternionMsgFromRollPitchYaw(roll, pitch, yaw);
if (isDegenerate)
laserOdomIncremental.pose.covariance[0] = 1;
else
laserOdomIncremental.pose.covariance[0] = 0;
}
pubLaserOdometryIncremental.publish(laserOdomIncremental);
}/**
* 发布里程计、点云、轨迹
* 1、发布历史关键帧位姿集合
* 2、发布局部map的降采样平面点集合
* 3、发布历史帧（累加的）的角点、平面点降采样集合
* 4、发布里程计轨迹
*/void publishFrames()
{
if (cloudKeyPoses3D->points.empty())
return;
// 发布历史关键帧位姿集合
publishCloud(&pubKeyPoses, cloudKeyPoses3D, timeLaserInfoStamp, odometryFrame);
// 发布局部map的降采样平面点集合
publishCloud(&pubRecentKeyFrames, laserCloudSurfFromMapDS, timeLaserInfoStamp, odometryFrame);
// 发布历史帧（累加的）的角点、平面点降采样集合
if (pubRecentKeyFrame.getNumSubscribers() != 0)
{
pcl::PointCloud<PointType>::Ptr cloudOut(new pcl::PointCloud<PointType>());
PointTypePose thisPose6D = trans2PointTypePose(transformTobeMapped);
*cloudOut += *transformPointCloud(laserCloudCornerLastDS, &thisPose6D);
*cloudOut += *transformPointCloud(laserCloudSurfLastDS, &thisPose6D);
publishCloud(&pubRecentKeyFrame, cloudOut, timeLaserInfoStamp, odometryFrame);
}
// 发布当前帧原始点云配准之后的点云
if (pubCloudRegisteredRaw.getNumSubscribers() != 0)
{
pcl::PointCloud<PointType>::Ptr cloudOut(new pcl::PointCloud<PointType>());
pcl::fromROSMsg(cloudInfo.cloud_deskewed, *cloudOut);
PointTypePose thisPose6D = trans2PointTypePose(transformTobeMapped);
*cloudOut = *transformPointCloud(cloudOut, &thisPose6D);
publishCloud(&pubCloudRegisteredRaw, cloudOut, timeLaserInfoStamp, odometryFrame);
}
// 发布里程计轨迹
if (pubPath.getNumSubscribers() != 0)
{
globalPath.header.stamp = timeLaserInfoStamp;
globalPath.header.frame_id = odometryFrame;
pubPath.publish(globalPath);
}
}/**
* 闭环scan-to-map，icp优化位姿
* 1、在历史关键帧中查找与当前关键帧距离最近的关键帧集合，选择时间相隔较远的一帧作为候选闭环帧
* 2、提取当前关键帧特征点集合，降采样；提取闭环匹配关键帧前后相邻若干帧的关键帧特征点集合，降采样
* 3、执行scan-to-map优化，调用icp方法，得到优化后位姿，构造闭环因子需要的数据，在因子图优化中一并加入更新位姿
* 注：闭环的时候没有立即更新当前帧的位姿，而是添加闭环因子，让图优化去更新位姿
*/void performLoopClosure()
{
if (cloudKeyPoses3D->points.empty() == true)
return;
mtx.lock();
*copy_cloudKeyPoses3D = *cloudKeyPoses3D;
*copy_cloudKeyPoses6D = *cloudKeyPoses6D;
mtx.unlock();
// 当前关键帧索引，候选闭环匹配帧索引
int loopKeyCur;
int loopKeyPre;
// not-used
if (detectLoopClosureExternal(&loopKeyCur, &loopKeyPre) == false)
// 在历史关键帧中查找与当前关键帧距离最近的关键帧集合，选择时间相隔较远的一帧作为候选闭环帧
if (detectLoopClosureDistance(&loopKeyCur, &loopKeyPre) == false) return;
// 提取
pcl::PointCloud<PointType>::Ptr cureKeyframeCloud(new pcl::PointCloud<PointType>());
pcl::PointCloud<PointType>::Ptr prevKeyframeCloud(new pcl::PointCloud<PointType>());
{
// 提取当前关键帧特征点集合，降采样
loopFindNearKeyframes(cureKeyframeCloud, loopKeyCur, 0);
// 提取闭环匹配关键帧前后相邻若干帧的关键帧特征点集合，降采样
loopFindNearKeyframes(prevKeyframeCloud, loopKeyPre, historyKeyframeSearchNum);
// 如果特征点较少，返回
if (cureKeyframeCloud->size() < 300 || prevKeyframeCloud->size() < 1000)
return;
// 发布闭环匹配关键帧局部map
if (pubHistoryKeyFrames.getNumSubscribers() != 0)
publishCloud(&pubHistoryKeyFrames, prevKeyframeCloud, timeLaserInfoStamp, odometryFrame);
}
// ICP参数设置
static pcl::IterativeClosestPoint<PointType, PointType> icp;
icp.setMaxCorrespondenceDistance(historyKeyframeSearchRadius*2);
icp.setMaximumIterations(100);
icp.setTransformationEpsilon(1e-6);
icp.setEuclideanFitnessEpsilon(1e-6);
icp.setRANSACIterations(0);
// scan-to-map，调用icp匹配
icp.setInputSource(cureKeyframeCloud);
icp.setInputTarget(prevKeyframeCloud);
pcl::PointCloud<PointType>::Ptr unused_result(new pcl::PointCloud<PointType>());
icp.align(*unused_result);
// 未收敛，或者匹配不够好
// historyKeyframeFitnessScore = 0.3
if (icp.hasConverged() == false || icp.getFitnessScore() > historyKeyframeFitnessScore)
return;
// 发布当前关键帧经过闭环优化后的位姿变换之后的特征点云
if (pubIcpKeyFrames.getNumSubscribers() != 0)
{
pcl::PointCloud<PointType>::Ptr closed_cloud(new pcl::PointCloud<PointType>());
pcl::transformPointCloud(*cureKeyframeCloud, *closed_cloud, icp.getFinalTransformation());
publishCloud(&pubIcpKeyFrames, closed_cloud, timeLaserInfoStamp, odometryFrame);
}
// 闭环优化得到的当前关键帧与闭环关键帧之间的位姿变换
float x, y, z, roll, pitch, yaw;
Eigen::Affine3f correctionLidarFrame;
correctionLidarFrame = icp.getFinalTransformation();
// 闭环优化前当前帧位姿
Eigen::Affine3f tWrong = pclPointToAffine3f(copy_cloudKeyPoses6D->points[loopKeyCur]);
// 闭环优化后当前帧位姿
Eigen::Affine3f tCorrect = correctionLidarFrame * tWrong;
pcl::getTranslationAndEulerAngles (tCorrect, x, y, z, roll, pitch, yaw);
gtsam::Pose3 poseFrom = Pose3(Rot3::RzRyRx(roll, pitch, yaw), Point3(x, y, z));
// 闭环匹配帧的位姿
gtsam::Pose3 poseTo = pclPointTogtsamPose3(copy_cloudKeyPoses6D->points[loopKeyPre]);
gtsam::Vector Vector6(6);
float noiseScore = icp.getFitnessScore();
Vector6 << noiseScore, noiseScore, noiseScore, noiseScore, noiseScore, noiseScore;
noiseModel::Diagonal::shared_ptr constraintNoise = noiseModel::Diagonal::Variances(Vector6);
// 添加闭环因子需要的数据
mtx.lock();
loopIndexQueue.push_back(make_pair(loopKeyCur, loopKeyPre));
loopPoseQueue.push_back(poseFrom.between(poseTo));
loopNoiseQueue.push_back(constraintNoise);
mtx.unlock();
loopIndexContainer[loopKeyCur] = loopKeyPre;
}
本文章为转载内容，我们尊重原作者对文章享有的著作权。如有内容错误或侵权问题，欢迎原作者联系我们进行内容更正或删除文章。
