前言

LIO-SAM的全稱是：Tightly-coupled Lidar Inertial Odometry via Smoothing and Mapping

從全稱上可以看出，該算法是一個(gè)緊耦合的雷達(dá)慣導(dǎo)里程計(jì)（Tightly-coupled Lidar Inertial Odometry），借助的手段就是利用GT-SAM庫(kù)中的方法。

LIO-SAM 提出了一個(gè)利用GT-SAM的緊耦合激光雷達(dá)慣導(dǎo)里程計(jì)的框架。

實(shí)現(xiàn)了高精度、實(shí)時(shí)的移動(dòng)機(jī)器人的軌跡估計(jì)和建圖。

其中點(diǎn)云運(yùn)動(dòng)畸變矯正的代碼在圖像投影的節(jié)點(diǎn)中

可以看到該節(jié)點(diǎn) 訂閱 3種消息：

原始點(diǎn)云數(shù)據(jù)

原始imu數(shù)據(jù)

imu預(yù)積分后預(yù)測(cè)的imu里程計(jì)數(shù)據(jù)其中完成的一個(gè)主要功能就是進(jìn)行畸變矯正。

本篇博客將解讀其畸變矯正處理流程部分。

畸變矯正

將點(diǎn)云投影到一個(gè)矩陣上，并保存每個(gè)點(diǎn)的信息,并在內(nèi)部進(jìn)行畸變矯正

  void projectPointCloud()  {

    int cloudSize = laserCloudIn->points.size();    for (int i = 0; i < cloudSize; ++i)    {

遍歷整個(gè)點(diǎn)云

      PointType thisPoint;       thisPoint.x = laserCloudIn->points[i].x;      thisPoint.y = laserCloudIn->points[i].y;      thisPoint.z = laserCloudIn->points[i].z;      thisPoint.intensity = laserCloudIn->points[i].intensity;

取出對(duì)應(yīng)的某個(gè)點(diǎn)

float range = pointDistance(thisPoint);

計(jì)算這個(gè)點(diǎn)距離lidar中心的距離

      if (range < lidarMinRange || range > lidarMaxRange)        continue;

距離太小或者太遠(yuǎn)都認(rèn)為是異常點(diǎn)

      int rowIdn = laserCloudIn->points[i].ring;      if (rowIdn < 0 || rowIdn >= N_SCAN)        continue;      if (rowIdn % downsampleRate != 0)        continue;

取出對(duì)應(yīng)的在第幾根scan上

scan id 合理判斷

如果需要降采樣，就根據(jù)scan id 適當(dāng)跳過(guò)

      float horizonAngle = atan2(thisPoint.x, thisPoint.y) * 180 / M_PI;       static float ang_res_x = 360.0/float(Horizon_SCAN);      int columnIdn = -round((horizonAngle-90.0)/ang_res_x) + Horizon_SCAN/2;      if (columnIdn >= Horizon_SCAN)        columnIdn -= Horizon_SCAN;      if (columnIdn < 0 || columnIdn >= Horizon_SCAN)        continue;

計(jì)算水平角

計(jì)算水平分辨率

計(jì)算水平線束id ，轉(zhuǎn)換到x負(fù)方向?yàn)槠鹗迹槙r(shí)針為正方向，范圍[0-H]

對(duì)水平角做補(bǔ)償，因?yàn)槔走_(dá)是順時(shí)針旋轉(zhuǎn)，

對(duì)水平id進(jìn)行檢查

      if (rangeMat.at(rowIdn, columnIdn) != FLT_MAX)        continue;

如果這個(gè)位置有填充了就跳過(guò)

點(diǎn)云不是完全的360度，可能會(huì)多一些

      thisPoint = deskewPoint(&thisPoint, laserCloudIn->points[i].time);

對(duì)點(diǎn)做運(yùn)動(dòng)補(bǔ)償

rangeMat.at(rowIdn, columnIdn) = range;

將這個(gè)點(diǎn)的距離數(shù)據(jù)保存進(jìn)這個(gè)range矩陣種

int index = columnIdn + rowIdn * Horizon_SCAN;

算出點(diǎn)的索引

fullCloud->points[index] = thisPoint;

保存這個(gè)點(diǎn)的坐標(biāo)

之后來(lái)看下運(yùn)動(dòng)補(bǔ)償?shù)煤瘮?shù)deskewPoint

  PointType deskewPoint(PointType *point, double relTime)  {

    if (deskewFlag == -1 || cloudInfo.imuAvailable == false)      return *point;

判斷是否可以進(jìn)行運(yùn)動(dòng)補(bǔ)償，不能得話則之間返回原點(diǎn)

判斷依據(jù)：

deskewFlag 是原始點(diǎn)云 沒(méi)有 time得標(biāo)簽 則為-1

cloudInfo.imuAvailable 的原始imu里面的數(shù)據(jù)判斷

    double pointTime = timeScanCur + relTime;

relTime 是相對(duì)時(shí)間，加上起始時(shí)間就是絕對(duì)時(shí)間

    float rotXCur, rotYCur, rotZCur;    findRotation(pointTime, &rotXCur, &rotYCur, &rotZCur);

通過(guò)findRotation函數(shù) 計(jì)算當(dāng)前點(diǎn) 相對(duì)起始點(diǎn)的相對(duì)旋轉(zhuǎn)

其內(nèi)部為：

  void findRotation(double pointTime, float *rotXCur, float *rotYCur, float *rotZCur)  {    *rotXCur = 0; *rotYCur = 0; *rotZCur = 0;

先將相對(duì)旋轉(zhuǎn)至0

    int imuPointerFront = 0;    while (imuPointerFront < imuPointerCur)    {      if (pointTime < imuTime[imuPointerFront])        break;      ++imuPointerFront;    }

找到距離該點(diǎn)云時(shí)間最近的 大于該點(diǎn)云時(shí)間的點(diǎn)

    if (pointTime > imuTime[imuPointerFront] || imuPointerFront == 0)    {      *rotXCur = imuRotX[imuPointerFront];      *rotYCur = imuRotY[imuPointerFront];      *rotZCur = imuRotZ[imuPointerFront];    }

如果時(shí)間戳不在兩個(gè)imu的旋轉(zhuǎn)之間，就直接賦值了

    } else {       int imuPointerBack = imuPointerFront - 1;      double ratioFront = (pointTime - imuTime[imuPointerBack]) / (imuTime[imuPointerFront] - imuTime[imuPointerBack]);      double ratioBack = (imuTime[imuPointerFront] - pointTime) / (imuTime[imuPointerFront] - imuTime[imuPointerBack]);      *rotXCur = imuRotX[imuPointerFront] * ratioFront + imuRotX[imuPointerBack] * ratioBack;      *rotYCur = imuRotY[imuPointerFront] * ratioFront + imuRotY[imuPointerBack] * ratioBack;      *rotZCur = imuRotZ[imuPointerFront] * ratioFront + imuRotZ[imuPointerBack] * ratioBack;    }

否則 作一個(gè)線性插值，得到相對(duì)旋轉(zhuǎn)

算兩個(gè)權(quán)重 進(jìn)行 插值

    float posXCur, posYCur, posZCur;    findPosition(relTime, &posXCur, &posYCur, &posZCur);

這里沒(méi)有計(jì)算平移補(bǔ)償 如果運(yùn)動(dòng)不快的話

    if (firstPointFlag == true)    {      transStartInverse = (pcl::getTransformation(posXCur, posYCur, posZCur, rotXCur, rotYCur, rotZCur)).inverse();      firstPointFlag = false;    }

計(jì)算第一個(gè)點(diǎn)的相對(duì)位姿

    Eigen::Affine3f transFinal = pcl::getTransformation(posXCur, posYCur, posZCur, rotXCur, rotYCur, rotZCur);    Eigen::Affine3f transBt = transStartInverse * transFinal;

計(jì)算當(dāng)前點(diǎn)和第一點(diǎn)的相對(duì)位姿

    newPoint.x = transBt(0,0) * point->x + transBt(0,1) * point->y + transBt(0,2) * point->z + transBt(0,3);    newPoint.y = transBt(1,0) * point->x + transBt(1,1) * point->y + transBt(1,2) * point->z + transBt(1,3);    newPoint.z = transBt(2,0) * point->x + transBt(2,1) * point->y + transBt(2,2) * point->z + transBt(2,3);    newPoint.intensity = point->intensity;    return newPoint;

就是R*p+t ，把點(diǎn)補(bǔ)償?shù)降谝粋€(gè)點(diǎn)對(duì)應(yīng)的時(shí)刻的位姿

然后看提取出有效的點(diǎn)的信息 函數(shù)cloudExtraction

  void cloudExtraction()  {

    for (int i = 0; i < N_SCAN; ++i)    {

遍歷每一根scan

cloudInfo.startRingIndex[i] = count - 1 + 5;

這個(gè)scan可以計(jì)算曲率的起始點(diǎn)（計(jì)算曲率需要左右各五個(gè)點(diǎn)）

      for (int j = 0; j < Horizon_SCAN; ++j)      {

遍歷該 scan上的每 個(gè)點(diǎn)

        if (rangeMat.at(i,j) != FLT_MAX)//FLT_MAX就是最大的浮點(diǎn)數(shù)        {

判斷該點(diǎn) 是否 是一個(gè) 有效的點(diǎn)

rangeMat的每個(gè)點(diǎn)初始化為FLT_MAX ，如果點(diǎn)有效，則會(huì)賦值為 range

cloudInfo.pointColInd[count] = j;

點(diǎn)云信息里面 這個(gè)點(diǎn)對(duì)應(yīng)著哪一個(gè)垂直線

cloudInfo.pointRange[count] = rangeMat.at(i,j);

點(diǎn)云信息里面 保存它的距離信息

 extractedCloud->push_back(fullCloud->points[j + i*Horizon_SCAN]);

他的3d坐標(biāo)信息

cloudInfo.endRingIndex[i] = count -1 - 5;

這個(gè)scan可以計(jì)算曲率的終端

在上面處理完后

即可發(fā)布點(diǎn)云

  void publishClouds()  {    cloudInfo.header = cloudHeader;    cloudInfo.cloud_deskewed = publishCloud(&pubExtractedCloud, extractedCloud, cloudHeader.stamp, lidarFrame);    pubLaserCloudInfo.publish(cloudInfo);  }

最后將處理后的點(diǎn)云發(fā)布出去

result

審核編輯：劉清