基於因子圖優化的鐳射IMU融合SLAM學習

SLAM不dunk發表於2020-12-21
優化SLAM

因子圖優化鐳射&IMU融合SLAM


主要學習今年最新開源的LIOSAM.
LIOSAM 點雲預處理與特徵提取的環節的分析見這裡
legoloam系列演算法之點雲處理與特徵提取

這裡主要學習的是在提取了鐳射特徵後, 融合IMU, GPS 實現鐳射里程計的方法, 雖然LIOSAM也是具有迴環的, 但是用的方法基本和legoloam一樣, 所以重點就放在 鐳射里程計上.
梳理一下主要檔案:
關於IMU的融合主要在src/imuPreintegration.cpp中完成.
接收到去除畸變後的鐳射特徵以及GPS資料後, 對鐳射和GPS進行融合的過程在 src/mapOptmization.cpp 中完成.
本次學習的重點就是這兩個檔案了.

1. 鐳射特徵的處理

LIOSAM融合鐳射的方式是, 單獨求取兩幀鐳射的運動, 然後將其作為約束新增到因子圖中.

鐳射的特徵在 回撥函式 laserCloudInfoHandler() 中處理:
核心的幾個函式如下:

  1. 預測當前幀的位姿 updateInitialGuess()
  2. 提取區域性map的特徵 extractSurroundingKeyFrames()
  3. 降取樣本幀的特徵downsampleCurrentScan()
  4. 進行高斯牛頓優化求解運動 scan2MapOptimization()

updateInitialGuess()

extractSurroundingKeyFrames()

scan2MapOptimization()

通過前面的步驟獲取預測位姿以及提取出當前幀與Map幀的特徵, 然後進行高斯牛頓優化.
步驟如下:

  1. 首先對區域性Map的特徵構造kdtree, 用於當前幀的特徵進行搜尋關聯.

      // 設定kdtree 
  kdtreeCornerFromMap->setInputCloud(laserCloudCornerFromMapDS);
  kdtreeSurfFromMap->setInputCloud(laserCloudSurfFromMapDS);

  2. 進行迭代, 求解出位姿

// 30次迭代 
for (int iterCount = 0; iterCount < 30; iterCount++)
{
    laserCloudOri->clear();
    coeffSel->clear();
    // 角點殘差計算
    cornerOptimization();
    // 平面點殘差計算
    surfOptimization();
    // 整理出用於進行優化的點線,點面殘差 
    combineOptimizationCoeffs();
    // 優化的主函式
    if (LMOptimization(iterCount) == true)
        break;              
}
  1. 更新狀態 transformUpdate().

關鍵在於迭代求解的環節, 主要研究下面幾個函式

cornerOptimization()

作用: 對所有的線特徵, 計算點線殘差.
這個過程的幾個重點如下:

  1. 遍歷當前幀全部曲面特徵 , 將其變換到 Map座標系中, 並用kdtree搜尋Map中最近的5個曲面特徵

    kdtreeCornerFromMap->nearestKSearch(pointSel, 5, pointSearchInd, pointSearchSqDis);

    其中kdtreeCornerFromMap的搜尋物件是 laserCloudCornerFromMapDS, 也就是map中全部 Corner

    kdtreeCornerFromMap->setInputCloud(laserCloudCornerFromMapDS);

    這個也是源自legoloam的操作, 這個做法的好處是,由於 特徵點只會和具有同樣標籤的特徵進行匹配, 這樣提高了資料關聯的準確性和效率.

  2. 求這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;
  1. 進行特徵分解 , 並通過特徵值判斷是否是直線
// 進行特徵分解  
cv::eigen(matA1, matD1, matV1);
// 如果 最大的特徵值比第二大特徵值大3倍以上   那麼認為是直線  
if (matD1.at<float>(0, 0) > 3 * matD1.at<float>(0, 1)) 
{
 ......
}

接著取當前點與直線上兩個點

// 當前點
float x0 = pointSel.x;
float y0 = pointSel.y;
float z0 = pointSel.z;
// 取直線的兩個點   matV1 為直線方向向量 
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);

計算當前點到直線的距離 (根據叉乘得到三角形面積, 然後通過三角形面積求解高),
以及該距離的方向向量, 這個用於後面高斯牛頓優化的jacobian矩陣的求解, 實際上也是一個梯度向量

// | (p0 - p1) x (p0 - p2) |
//向量OA = (x0 - x1, y0 - y1, z0 - z1), 向量OB = (x0 - x2, y0 - y2, z0 - z2)，向量AB = （x1 - x2, y1 - y2, z1 - z2）
//向量OA OB的向量積(即叉乘)為：
//|  i      j      k  |
//|x0-x1  y0-y1  z0-z1|
//|x0-x2  y0-y2  z0-z2|
//模為：
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)));
// |(p1 - p2)|
float l12 = sqrt((x1 - x2)*(x1 - x2) + (y1 - y2)*(y1 - y2) + (z1 - z2)*(z1 - z2));
// 下面求d的方向向量  nd = {la, lb, lc}
// 這個也是當前距離函式的梯度向量 
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;

參考下圖:
在這裡插入圖片描述

最後就是賦予一個權重, 然後將距離向量(梯度), 和距離殘差值放入 coeff

// 權重係數  
float s = 1 - 0.9 * fabs(ld2);
// 點到直線的距離向量
coeff.x = s * la;
coeff.y = s * lb;
coeff.z = s * lc;
coeff.intensity = s * ld2;    // 代價值為距離 x 權重
surfOptimization()

作用: 對所有的面特徵, 計算點面殘差.
同樣是遍歷全部平面特徵, 然後在Map的平面特徵集中尋找5個最近點,
然後是採用最小二乘, 把平面的方程擬合出來 這裡是構造 AX + 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= B ( matA0*matX0 = -1 )  對A進行QR分解  =>  QRX = B  令  RX = y  =>  y = Q^t*B =>求解 X
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;    // 1 / ps 也需要同時縮小 
...........
// 如果平面的精度足夠
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;
    }
}

這裡思考一個問題, 為什麼 在構造點到直線的距離的時候, 使用的是距離的絕對值,
而構造點面的距離的時候用得卻是一個有正負的值呢?? 那麼可不可以直接把點到面的距離改成絕對值呢?

這樣是不可以的, 原因簡單來說, 是因為 , 構造點到線距離的時候, 其實不僅僅是求了一個點到直線的距離這麼簡單,它還求了一個距離函式的梯度,也就是一個朝向使距離函式值增大速度最快方向的向量, 而 構造點面距離殘差的部分 , 並沒有求一個梯度而是一個固定的平面法向量, 這時 我們直接將距離函式取絕對值, 那麼在高斯牛頓優化求解時, 系統並不能往正確的方向上去迭代, 如下圖, 點1和點2 距離平面距離都為h, 但是上面的點需要往下走, 而下面的點需要往上走, 而高斯牛頓求解時這兩種情況的更新相同, 那麼自然是錯誤的.
在這裡插入圖片描述
那麼如果要將點到平面的距離構造為絕對值的形式應該怎麼做呢? 參考下面程式碼

if (planeValid) 
{
    float pd2 = pa * pointSel.x + pb * pointSel.y + pc * pointSel.z + pd;

    // 後面部分相除求的是[pa,pb,pc,pd]與pointSel的夾角餘弦值(兩個sqrt，其實並不是餘弦值)
    // 這個夾角餘弦值越小越好，越小證明所求的[pa,pb,pc,pd]與平面越垂直
    float s = 1 - 0.9 * fabs(pd2) / sqrt(sqrt(pointSel.x * pointSel.x
            + pointSel.y * pointSel.y + pointSel.z * pointSel.z));

    if(pd2<0)
    {
        coeff.x = -s * pa;
        coeff.y = -s * pb;
        coeff.z = -s * pc;
    }        
    else
    {
        coeff.x = s * pa;
        coeff.y = s * pb;
        coeff.z = s * pc;
    }

    // 改用距離的絕對值   
    coeff.intensity = s * fabs(pd2);

    // 判斷是否是合格平面，是就加入laserCloudOri
    if (s > 0.1) 
    {
        laserCloudOri->push_back(pointOri);
        coeffSel->push_back(coeff);
    }
}
combineOptimizationCoeffs()

作用: 遍歷當前幀所有的特徵 將用於構成殘差的點放入 laserCloudOri, 殘差資訊放入 coeffSel

bool LMOptimization(int iterCount)

進行高斯牛頓優化, 這個函式是重點.

----------------------------------------------從下面開始 需要學習GTSAM的相關知識 ----------------------------------

2.IMU的處理

3.GPS的處理

相關文章