-
LIO-SAM
发表于IROS2020
,是一个效果非常好的惯性-激光紧耦合里程计
- 我打算给我们的机器人搞一个激光里程计,于是打算把
LIO-SAM
改一改搞过来
- 修改过程中发现一个问题:在里程计求解(
mapOptimization
的LMOptimization
函数)过程中,作者是用欧拉角来表示位姿的旋转部分的
- 这对于移动机器人来说还算可行,因为一般情况下地面移动机器人的
pitch
不会到达90°
,也就不会引起死锁
- 但我们的机器人是攀爬机器人,经常出现翻转的动作,因此不能使用欧拉角来表示旋转;因此,需要对这部分做一定修改
- 修改思路:“用
SO3
加平移”的方式来表示位姿
- 先看看原代码整体思路:
void laserCloudInfoHandler(const lio_sam::cloud_infoConstPtr& msgIn)
{
// extract time stamp
timeLaserInfoStamp = msgIn->header.stamp;
timeLaserInfoCur = msgIn->header.stamp.toSec();
// extract info and feature cloud
cloudInfo = *msgIn;
pcl::fromROSMsg(msgIn->cloud_corner, *laserCloudCornerLast);
pcl::fromROSMsg(msgIn->cloud_surface, *laserCloudSurfLast);
std::lock_guard<std::mutex> lock(mtx);
static double timeLastProcessing = -1;
if (timeLaserInfoCur - timeLastProcessing >= mappingProcessInterval)
{
timeLastProcessing = timeLaserInfoCur;
updateInitialGuess();
extractSurroundingKeyFrames();
downsampleCurrentScan();
scan2MapOptimization();
saveKeyFramesAndFactor();
correctPoses();
publishOdometry();
publishFrames();
}
}
- 翻译下,整个
mapOptimization
的大致流程就是:
-
updateInitialGuess
:将IMU的预测结果设为优化初值
-
extractSurroundingKeyFrames
:提取与当前帧相邻的关键帧点云,组成一个局部地图
-
downsampleCurrentScan
:降采样咯,分别对corner
和surface
特征集合降采样
-
scan2MapOptimization
:重点!!利用高斯牛顿法求解当前帧的位姿
-
saveKeyFramesAndFactor
:判断是否需要插入新的关键帧;给iSAM
加入新的因子,进行进一步优化(这个优化的效果主要体现在长距离、长时间、有回环情况下的误差抑制)
-
correctPoses
:与IMU
预测的pitch
,roll
进行融合
- 那么,我要改的主要是
scan2MapOptimization
这个部分,接下来看一看scan2MapOptimization
里面的主要流程:
void scan2MapOptimization()
{
if (cloudKeyPoses3D->points.empty())
return;
if (laserCloudCornerLastDSNum > edgeFeatureMinValidNum && laserCloudSurfLastDSNum > surfFeatureMinValidNum)
{
kdtreeCornerFromMap->setInputCloud(laserCloudCornerFromMapDS);
kdtreeSurfFromMap->setInputCloud(laserCloudSurfFromMapDS);
for (int iterCount = 0; iterCount < 30; iterCount++)
{
laserCloudOri->clear();
coeffSel->clear();
cornerOptimization();
surfOptimization();
combineOptimizationCoeffs();
if (LMOptimization(iterCount) == true)
break;
}
transformUpdate();
} else {
ROS_WARN("Not enough features! Only %d edge and %d planar features available.", laserCloudCornerLastDSNum, laserCloudSurfLastDSNum);
}
}
- 翻译下:
-
cornerOptimization, surfOptimization
里进行了特征匹配、残差计算以及对平移部分的雅克比计算(旋转部分的雅克比可由平移部分的雅克比进一步计算得到)
-
combineOptimizationCoeffs
是将上面两个函数计算的残差与雅克比的部分计算合并在一起,以便在LMOptimization
里面用同一个for
来计算迭代步长
-
LMOptimization
里面计算对旋转部分的雅克比,并用高斯牛顿法计算一次迭代步长、更新位姿
- 整个
scan2MapOptimization
的内容就是迭代优化30次,每次迭代都会重新匹配特征
- 好了,那么我具体要改哪些函数?
- 首要明确:我要改的是在优化过程中,状态变量的表示方式
- 优化迭代公式如下:
J^T*J*δx = - J^T * f(x)
,状态变量的表示方式会影响雅克比的计算;本文修改的是旋转的表示方式,因此涉及旋转雅克比计算的函数都要进行修改,状态更新的地方也要修改
- 因此,主要修改这三个函数:
cornerOptimization, surfOptimization,LMOptimization
- 先看
surfOptimization
:
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;
}
}
}
}
}
- 总结下:
- 先把当前特征点转到
map
坐标系下,用kd
树找map
中 与此点相邻的5个平面点
- 用这五个平面点拟合一个平面,并检查这个平面质量是否足够好
- 如果平面质量够好,就计算点到面的残差,然后根据这个点到面的距离,给这个残差项分配一个可信度因子(距离被匹配平面远的点可信度没那么高);如果可信度因子足够大,则将这个点放到匹配集合里去
- 这里要注意一下
coeff
变量,它的xyz
保存的是(乘了一个可信度因子的)这个面的法向量,intensity
保存的是(乘了一个可信度因子的)点到这个面的距离
- 这里要注意一下:按照点面特征的雅克比计算公式("SO3加平移"版本),点面残差对平移部分的雅克比就是这个面的单位法向量转置,而对旋转部分的雅克比也只是在这个基础上右乘一个
-Rp
(当前旋转估计乘以当前点在雷达坐标系下的表示)的反对称矩阵而已
- 对旋转部分的雅克比计算是在
LMOptimization
中完成的,在LMOptimization
之前我们就要完成-Rp
的计算了
- 修改后的代码:
Eigen::Vector3d CulRp(PointType const * const pi) {
return Eigen::Vector3d(transPointAssociateToMap(0,0) * pi->x + transPointAssociateToMap(0,1) * pi->y + transPointAssociateToMap(0,2) * pi->z,
transPointAssociateToMap(1,0) * pi->x + transPointAssociateToMap(1,1) * pi->y + transPointAssociateToMap(1,2) * pi->z,
transPointAssociateToMap(2,0) * pi->x + transPointAssociateToMap(2,1) * pi->y + transPointAssociateToMap(2,2) * pi->z);
}
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;
pointOri = laserCloudSurfLastDS->points[i];
pointAssociateToMap(&pointOri, &pointSel);
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();
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;
}
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;
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;
surfRp[i] = (CulRp(&pointOri)); // 新增的类成员变量, std::vector<Eigen::Vector3d> surfRp; // for jacobian orientation
}
}
}
}
}
- 其实就加了一行
surfRp[i] = (CulRp(&pointOri)); // 新增的类成员变量, std::vector<Eigen::Vector3d> surfRp; // for jacobian orientation
。。。
-
cornerOptimization
的修改思路与上面差不多,但线特征的雅克比计算有所不同:
+ 放一放原码:
void cornerOptimization()
{
updatePointAssociateToMap();
#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;
pointOri = laserCloudCornerLastDS->points[i];
pointAssociateToMap(&pointOri, &pointSel);
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));
if (pointSearchSqDis[4] < 1.0) {
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)) {
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);
// a012其实就是((p_i - p_a) x (p_i - p_b)).norm
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)); // (pa - pb).norm
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;
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;
cornerRp[i] = CulRp(&pointOri);
}
}
}
}
}
- 对于这个函数的coeff变量,它的xyz保存的直接就是是(乘了一个可信度因子的)对平移部分的雅克比,intensity保存的是(乘了一个可信度因子的)点到这个线特征的距离
- 注意:线特征对旋转部分的雅克比矩阵中,
Rp
是不带负号的
- 修改完
surfOptimization
和cornerOptimization
后,由于多出了Rp
的计算,因此要把Rp
也合并到一起去,因此要改combineOptimizationCoeffs
:
void combineOptimizationCoeffs()
{
// combine corner coeffs
for (int i = 0; i < laserCloudCornerLastDSNum; ++i){
if (laserCloudOriCornerFlag[i] == true){
laserCloudOri->push_back(laserCloudOriCornerVec[i]);
coeffSel->push_back(coeffSelCornerVec[i]);
pcl::PointXYZ p;
p.x = cornerRp[i].x(); p.y = cornerRp[i].y(); p.z = cornerRp[i].z();
Rp->push_back(p); // pcl::PointCloud<pcl::PointXYZ>::Ptr Rp; // for jacobian orientation
}
}
// combine surf coeffs
for (int i = 0; i < laserCloudSurfLastDSNum; ++i){
if (laserCloudOriSurfFlag[i] == true){
laserCloudOri->push_back(laserCloudOriSurfVec[i]);
coeffSel->push_back(coeffSelSurfVec[i]);
pcl::PointXYZ p;
p.x = -1 * surfRp[i].x(); p.y = -1 * surfRp[i].y(); p.z = -1 * surfRp[i].z();
Rp->push_back(p);
}
}
// reset flag for next iteration
std::fill(laserCloudOriCornerFlag.begin(), laserCloudOriCornerFlag.end(), false);
std::fill(laserCloudOriSurfFlag.begin(), laserCloudOriSurfFlag.end(), false);
}
- 如果是线特征,则给
Rp
加负号
- 最后,要修改
LMOptimization
,先上一波源码:
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;
}
- 要改的地方:
- 状态变量(状态变量定义为
[roll, pitch, yaw, x, y, z]
,优化过程中会临时修改这个顺序)
- 雅克比矩阵(
matA
就是雅克比矩阵,matAt
是其转置,matB
是残差),matX
是迭代步长
- 状态变量:
- 从
[roll, pitch, yaw, x, y, z]
修改为[so3, x, y, z]
- 只有优化过程这样改,其他部分还是保持原来的表示;因此这里有一个欧拉角到SO3的转换过程
- 对旋转进行更新时,需要将迭代步长的旋转部分从李代数转成李群,然后再左乘到原状态上
- 计算完迭代之后,要将SO3转回到欧拉角
- 雅克比矩阵:
- 懒得写了…看码吧
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;
for (int i = 0; i < laserCloudSelNum; i++) {
// // lidar -> camera
// 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;
// lidar -> camera
pointOri.x = laserCloudOri->points[i].x;
pointOri.y = laserCloudOri->points[i].y;
pointOri.z = laserCloudOri->points[i].z;
// lidar -> camera
coeff.x = coeffSel->points[i].x;
coeff.y = coeffSel->points[i].y;
coeff.z = coeffSel->points[i].z;
coeff.intensity = coeffSel->points[i].intensity;
// skew of Rp
auto p_pcl = Rp->points[i];
Eigen::Vector3f p(p_pcl.x, p_pcl.y, p_pcl.z);
Eigen::Matrix3f RpSkew = mapOptimization::skewSymmetric(p);
Eigen::Vector3f tmp(coeff.x, coeff.y, coeff.z);
Eigen::Matrix<float, 1, 3> jacobian_theta = tmp.transpose() * RpSkew;
// // 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, 0) = jacobian_theta(0, 0);
matA.at<float>(i, 1) = jacobian_theta(0, 1);
matA.at<float>(i, 2) = jacobian_theta(0, 2);
// matA.at<float>(i, 3) = coeff.z;
// matA.at<float>(i, 4) = coeff.x;
// matA.at<float>(i, 5) = coeff.y;
matA.at<float>(i, 3) = coeff.x;
matA.at<float>(i, 4) = coeff.y;
matA.at<float>(i, 5) = coeff.z;
matB.at<float>(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<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;
}
Eigen::Quaternionf quat(transPointAssociateToMap.matrix().block<3, 3>(0, 0));
Sophus::SO3f so3_orientation;
so3_orientation.setQuaternion(quat.normalized());
// 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);
Eigen::Vector3f update_so3(matX.at<float>(0, 0), matX.at<float>(1, 0), matX.at<float>(2, 0));
so3_orientation = Sophus::SO3f::exp(update_so3) * so3_orientation;
Eigen::Vector3f euler_angles = so3_orientation.matrix().eulerAngles(2, 1, 0);
transformTobeMapped[0] = euler_angles[2];
transformTobeMapped[1] = euler_angles[1];
transformTobeMapped[2] = euler_angles[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));
auto tmp = Sophus::SO3f::exp(update_so3);
float deltaR = sqrt(
pow(pcl::rad2deg(tmp.angleX()), 2) +
pow(pcl::rad2deg(tmp.angleY()), 2) +
pow(pcl::rad2deg(tmp.angleZ()), 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.05 && deltaT < 0.05) {
return true; // converged
}
return false; // keep optimizing
}
- 这里的SO3转回欧拉角后,再与IMU做融合会出问题(可能涉及到欧拉角的插值问题),因此我把
scan2MapOptimization
中的transformUpdate
换成incrementalOdometryAffineBack = trans2Affine3f(transformTobeMapped);
,这样就不会与IMU的pitch,roll融合了