1. 概述

整理一下LOAM中對鐳射點雲資料、IMU資料的處理以及相關的公式推導，希望能幫助大家，同時供自己以後複習參考。由於相關的東西較多，可能會有疏漏，並且由於時間有限，精力有限，此部落格長期更新，歡迎各位小夥伴批評指正。

2. 前言

LOAM是 bulabula。
LOAM參考原始碼倉庫github地址為LOAM 原始碼連結具體配置可以參照部落格LOAM SLAM安裝與配置
關於LOAM原始論文的解讀可以參考LOAM SLAM之論文原理解讀，
相關連結LOAM SLAM原理之防止非線性優化解退化。
其他參考連結，感謝各位在此之前分享對LOAM原理以及程式碼的分析理解
LOAM細節分析
LOAM:3D鐳射里程計及環境建圖的方法和實現（一）

每個人對程式碼的理解是不一樣，均有可能出現偏差，歡迎探討！

3. 準備工作

在LOAM中，使用的座標系是左上前座標系，也就是X指向左，Y指向上，Z指向前。一定要注意座標系問題，後面的一系列工作全都是基於這一基準座標系開展的。

3.1 鐳射雷達

LOAM原始論文使用的是一個二維鐳射雷達，通過機械機構產生3D點雲資料，但是現在的程式碼中大多使用的是Velodyne美國威力登公司的16線鐳射雷達：vlp16也稱為PUCK。因此本文以程式碼中使用的vlp16雷達開展（程式碼使用的這個，如果想用其他的就自己改改吧，哈哈）。那麼簡單介紹一下vlp16：

• Velodyne PUCK（VLP-16）16通道
• TOF測量距離，最遠100m，精度3cm
• 垂直解析度2°
• 水平解析度600、1200RPM對應0.2°、0.4°
• 旋轉速率5-20Hz
• 最大點雲資料量 360/0.4161200/60=288000（30萬）

我們主要關注點雲資料的結構，根據官方文件，具體結構示意如下圖，需要注意相鄰鐳射束的ID差別較大，也就是鐳射每次掃描時不是依次掃描的，而是隔了半個垂直視場的範圍

vlp16的座標系為右前上，並且掃描時順時針旋轉，通過安裝可以將座標系變換為前左上，因此我們在資料處理時只需要將前左上座標系的點雲資料轉換為左上前座標系即可。

3.2 IMU

根據程式碼可以看到IMU安裝座標系為前左上，因此實際使用時需要將測量的比力訊號轉換到左上前座標系，並且注意對重力加速度的處理之後才是真正的鐳射運動加速度。

3.3 座標變換

前面談到了座標系問題，這裡講一下座標變換的問題，網上相關的部落格很多，為了不引起誤解，對於LOAM，我們統一用下面的方式描述旋轉變換：
左乘，旋轉座標軸，先Z再X後Y得到旋轉矩陣$R_{ZXY}$。
舉例說明：鐳射座標系$L$和世界座標系$W$原點開始時一樣，經過繞Z軸旋轉$rz$角度，繞X軸$rx$角度，繞Y軸$ry$角度（以座標軸正方向為旋轉正方向）之後得到鐳射座標系$L^{\prime }$，那麼根據上面的定義有
從世界座標系$W$到鐳射座標系$L^{\prime }$的變換$R_{ZXY}$為
根據左乘
$$R_{ZXY}=R_{ry}{R}_{rx}{R}_{rz}$$
根據旋轉座標軸矩陣
$$R_{ZXY}=\left[\begin{array}{ccc}cry& 0& -sry\\ 0& 1& 0\\ sry& 0& cry\end{array}\right]\left[\begin{array}{ccc}1& 0& 0\\ 0& crx& srx\\ 0& -srx& crx\end{array}\right]\left[\begin{array}{ccc}crz& srz& 0\\ -srz& crz& 0\\ 0& 0& 1\end{array}\right]$$
進一步得到
$$R_{ZXY}=\left[\begin{array}{ccc}cycz-sxsysz& cysz+sxsycz& -sycx\\ -cxsz& cxcz& sx\\ sycz+sxcysz& sysz-sxcycz& cycx\end{array}\right]$$
上面的公式以及以後的公式中$sy=sry=sin(ry),cy=cry=cos(ry)$
但有的時候，我們需要的是逆運算，也就是從鐳射座標系$L^{\prime }$到世界座標系$W$的變換$(R_{ZXY}{)}^{-1}$
根據變換的關係很容易得到，所有的變換角度取負數，順序取反就可以得到
$$(R_{ZXY}{)}^{-1}=R_{-rz}{R}_{-rx}{R}_{-ry}$$

單獨考慮先Y再X後Z左乘，旋轉座標軸的矩陣$R_{YXZ}$，有
根據左乘旋轉座標軸矩陣
$$R_{YXZ}=\left[\begin{array}{ccc}crz& srz& 0\\ -srz& crz& 0\\ 0& 0& 1\end{array}\right]\left[\begin{array}{ccc}1& 0& 0\\ 0& crx& srx\\ 0& -srx& crx\end{array}\right]\left[\begin{array}{ccc}cry& 0& -sry\\ 0& 1& 0\\ sry& 0& cry\end{array}\right]$$
進一步得到
$$R_{YXZ}=\left[\begin{array}{ccc}cycz+sxsysz& cxsz& -sycz+sxcysz\\ -cysz+sxsycz& cxcz& sysz+sxcycz\\ cxsy& -sx& cxcy\end{array}\right]$$

很容易看到單獨的$R_{YXZ}$和$(R_{ZXY}{)}^{-1}$差了一個負號，這是因為逆運算導致的。

上面的左乘，對應右乘，對右乘有$(R_{ZXY})=R_{rz}{R}_{rx}{R}_{ry}$，這裡不考慮。
上面的旋轉座標軸，對應旋轉向量，對旋轉向量有
$$R_{ry}=\left[\begin{array}{ccc}cry& 0& sry\\ 0& 1& 0\\ -sry& 0& cry\end{array}\right]$$
其實就是旋轉座標軸的角度取反，可以認為是一組逆運算。相對運動，哈哈。這個在程式碼裡體現為所有的rotateZXY和rotateYXZ函式都使用的是旋轉向量矩陣，因此需要注意實際傳入的角度資訊。

同樣的，對於變換
$$X_t=R{X}_0+T$$的逆運算為
$$X_0=R^{-1}(X_t-T)$$
這個後面具體用到的時候再說吧。

4. 插話

現在就開始進行我們的原始碼閱讀之旅，注意分析內容有可能較多，後面視情況再將內容劃分。
總體概覽和框架可以參考原始論文分析LOAM SLAM之論文原理解讀，這裡主要對程式碼進行詳細分析。

5. 之MultiScanRegistration

MultiScanRegistration節點利用插值得到的IMU資料對鐳射運動補償，得到近似勻速運動模型下的點雲資料，同時對點雲資料提取邊緣特徵，平面特徵，供LaserOdometry節點使用。

5.1 引數配置

可設定引數以及預設引數有，第一列表示引數名稱，第二列表示預設值或者可選值

      scanPeriod                 const float& scanPeriod_ = 0.1,//一次完整的鐳射資料，對應掃描頻率10hz
      imuHistorySize             const int& imuHistorySize_ = 200,//IMU資料緩衝區大小，後面使用緩衝區資料進行插值
      featureRegions             const int& nFeatureRegions_ = 6,//同一鐳射束點雲資料劃分的子區域數量
      curvatureRegion            const int& curvatureRegion_ = 5,//計算每個點彎曲度時在當前點一邊選取的附近點數，總點數為 region*2
      maxCornerSharp             const int& maxCornerSharp_ = 2,//子區域中選取的最大邊緣點數
      maxCornerLessSharp         maxCornerLessSharp(10 * maxCornerSharp_)//子區域中選取的最大次邊緣點數(彎曲度大於閾值的部分點)
      maxSurfaceFlat             const int& maxSurfaceFlat_ = 4,//子區域中選取的最大平面點數
      lessFlatFilterSize         const float& lessFlatFilterSize_ = 0.2,//次平面點濾波器使用的體素大小
      surfaceCurvatureThreshold  const float& surfaceCurvatureThreshold_ = 0.1//區分平面、邊緣時使用的彎曲度閾值

      //鐳射設定可以選擇下面的一種方式
      1.          lidar          VLP-16，HDL-32，HDL-64E       //velodyne具體產品，對應16線，32線，64線
      2.          minVerticalAngle || maxVerticalAngle || nScanRings  //非velodyne產品，根據具體的引數進行設定，並且至少需要2線鐳射雷達

鐳射束數量不同的鐳射雷達（16，32，64線等）在處理資料時稍有不同：需要將不同鐳射束點雲資料區分開，供後續里程計，建圖時使用。
因此做了下面工作：

• 預設支援VLP16，HDL32，HDL64E，建立scanMapper物件。
• 或者根據最大、最小垂直角度，鐳射線束構建建立scanMapper物件，這種可以用於非Velodyne品牌的鐳射雷達。

  if (privateNode.getParam("lidar", lidarName)) {
    if (lidarName == "VLP-16") {
      _scanMapper = MultiScanMapper::Velodyne_VLP_16();
    } else if (lidarName == "HDL-32") {
      _scanMapper = MultiScanMapper::Velodyne_HDL_32();
    } else if (lidarName == "HDL-64E") {
      _scanMapper = MultiScanMapper::Velodyne_HDL_64E();
    } else {
      ROS_ERROR("Invalid lidar parameter: %s (only \"VLP-16\", \"HDL-32\" and \"HDL-64E\" are supported)", lidarName.c_str());
      return false;
    }

    ROS_INFO("Set  %s  scan mapper.", lidarName.c_str());
    if (!privateNode.hasParam("scanPeriod")) {
      config_out.scanPeriod = 0.1;
      ROS_INFO("Set scanPeriod: %f", config_out.scanPeriod);
    }
  } else {
    float vAngleMin, vAngleMax;
    int nScanRings;

    if (privateNode.getParam("minVerticalAngle", vAngleMin) &&
        privateNode.getParam("maxVerticalAngle", vAngleMax) &&
        privateNode.getParam("nScanRings", nScanRings)) {
      if (vAngleMin >= vAngleMax) {
        ROS_ERROR("Invalid vertical range (min >= max)");
        return false;
      } else if (nScanRings < 2) {
        ROS_ERROR("Invalid number of scan rings (n < 2)");
        return false;
      }

      _scanMapper.set(vAngleMin, vAngleMax, nScanRings);
      ROS_INFO("Set linear scan mapper from %g to %g degrees with %d scan rings.", vAngleMin, vAngleMax, nScanRings);
    }
  }

scanMapper物件的作用使用一句話概括：根據角度獲取鐳射束ID，後續用於分開不同鐳射束資料。

• 以vlp16為例

  /** \brief Construct a new multi scan mapper instance.
   *
   * @param lowerBound - the lower vertical bound (degrees)
   * @param upperBound - the upper vertical bound (degrees)
   * @param nScanRings - the number of scan rings
   */
  MultiScanMapper(const float& lowerBound = -15,
                  const float& upperBound = 15,
                  const uint16_t& nScanRings = 16);
  MultiScanMapper::MultiScanMapper(const float& lowerBound,
                                 const float& upperBound,
                                 const uint16_t& nScanRings)
    : _lowerBound(lowerBound),
      _upperBound(upperBound),
      _nScanRings(nScanRings),
      _factor((nScanRings - 1) / (upperBound - lowerBound))
{}
// 根據垂直角度獲取鐳射束ID：0,1,2、、、15
int MultiScanMapper::getRingForAngle(const float& angle) {
  return int(((angle * 180 / M_PI) - _lowerBound) * _factor + 0.5);
}

5.2 話題訂閱

bool ScanRegistration::setupROS(ros::NodeHandle& node, ros::NodeHandle& privateNode, RegistrationParams& config_out)
{
  if (!parseParams(privateNode, config_out))
    return false;

  // subscribe to IMU topic
  _subImu = node.subscribe<sensor_msgs::Imu>("/imu/data", 50, &ScanRegistration::handleIMUMessage, this);

  // advertise scan registration topics
  _pubLaserCloud            = node.advertise<sensor_msgs::PointCloud2>("/velodyne_cloud_2", 2);
  _pubCornerPointsSharp     = node.advertise<sensor_msgs::PointCloud2>("/laser_cloud_sharp", 2);
  _pubCornerPointsLessSharp = node.advertise<sensor_msgs::PointCloud2>("/laser_cloud_less_sharp", 2);
  _pubSurfPointsFlat        = node.advertise<sensor_msgs::PointCloud2>("/laser_cloud_flat", 2);
  _pubSurfPointsLessFlat    = node.advertise<sensor_msgs::PointCloud2>("/laser_cloud_less_flat", 2);
  _pubImuTrans              = node.advertise<sensor_msgs::PointCloud2>("/imu_trans", 5);

  return true;
}

以及點雲資料

  // subscribe to input cloud topic
  _subLaserCloud = node.subscribe<sensor_msgs::PointCloud2>
      ("/multi_scan_points", 2, &MultiScanRegistration::handleCloudMessage, this);

5.3 IMU資料處理

IMU資料的處理如下，IMU的座標系為前左上，對測量的比力進行處理得到真正的加速度訊號，可以自己推導一下，有需要的話我再把這部分內容新增上，轉換座標系為左上前座標系。這裡的偏航俯仰滾轉角三個尤拉角都不會變化，可以考慮一下為什麼。

void ScanRegistration::handleIMUMessage(const sensor_msgs::Imu::ConstPtr& imuIn)
{
  tf::Quaternion orientation;
  tf::quaternionMsgToTF(imuIn->orientation, orientation);
  double roll, pitch, yaw;
  tf::Matrix3x3(orientation).getRPY(roll, pitch, yaw);

  Vector3 acc;
  acc.x() = float(imuIn->linear_acceleration.y - sin(roll) * cos(pitch) * 9.81);
  acc.y() = float(imuIn->linear_acceleration.z - cos(roll) * cos(pitch) * 9.81);
  acc.z() = float(imuIn->linear_acceleration.x + sin(pitch)             * 9.81);

  IMUState newState;
  newState.stamp = fromROSTime( imuIn->header.stamp);
  newState.roll = roll;
  newState.pitch = pitch;
  newState.yaw = yaw;
  newState.acceleration = acc;

  updateIMUData(acc, newState);
}

對於陀螺儀而言，測量得到加速度資訊是需要進行積分的，因此有
加速度訊號轉換到世界座標系然後積分得到速度velocity，再積分得到位置position。
對於姿態角，不必積分，直接使用即可。
將這些資料儲存到IMU環形容器_imuHistory中。用於後面的插值以及偏移計算。

void BasicScanRegistration::updateIMUData(Vector3& acc, IMUState& newState)
{
  if (_imuHistory.size() > 0) {
    // accumulate IMU position and velocity over time
    //這裡將資料轉換到世界座標系中，理論上應該取負數，但是這裡程式碼中使用了旋轉向量矩陣，因此就不需要取負數了
    rotateZXY(acc, newState.roll, newState.pitch, newState.yaw);

    const IMUState& prevState = _imuHistory.last();
    float timeDiff = toSec(newState.stamp - prevState.stamp);
    newState.position = prevState.position
                        + (prevState.velocity * timeDiff)
                        + (0.5 * acc * timeDiff * timeDiff);
    newState.velocity = prevState.velocity
                        + acc * timeDiff;
  }

  _imuHistory.push(newState);
}

覺得太複雜直接記住下面的總結就行，我就不多講了，容易混淆。簡單總結就是對於IMU測量的姿態角有：

   　　　　　rotateYXZ(-yaw,-pitch,-roll)
   世界座標系－－－－－－－－－－－－>>>>>IMU座標系
   世界座標系<<<<<－－－－－－－－－－－－IMU座標系
   　　　　　rotateZXY(roll,pitch,yaw)

5.4 點雲資料處理

5.4.1 點雲資料

整個點雲註冊環節核心部分就在這裡，主要是對點雲資料進一步處理，根據IMU資料校正得到近似勻速運動的點雲資料，以供鐳射里程計使用。對，勻速運動假設，這個很重要，需要構造這樣的條件，不然後面的處理會出現很大的誤差。

具體的處理環節在下面的函式中

void MultiScanRegistration::process(const pcl::PointCloud<pcl::PointXYZ>& laserCloudIn, const Time& scanTime)

原始的鐳射座標系是右前上，順時針旋轉，點雲資料的水平角度遞減，但是可以通過安裝將鐳射座標系變換為前左上，依舊是順時針旋轉，點雲資料水平角度遞減，很簡單對不對。這樣就可以很方便的轉換為論文中提到的統一座標系左上前。

判斷點的起始方位和終止方位,原始角度是順時針變化，因此需要對角度進行取反，並對角度判斷週期性。注意負號，因為原始是順時針旋轉的，這裡取負號，後面也取負號，就統一變為逆時針旋轉。

• atan2的值域是$-\pi$到$\pi$。

  float startOri = -std::atan2(laserCloudIn[0].y, laserCloudIn[0].x);
  float endOri = -std::atan2(laserCloudIn[cloudSize - 1].y,
                             laserCloudIn[cloudSize - 1].x) + 2 * float(M_PI);
  // 必須滿足終止角度 > 初始角度並且，近似一個週期2pi
  if (endOri - startOri > 3 * M_PI) {
    endOri -= 2 * M_PI;
  } else if (endOri - startOri < M_PI) {
    endOri += 2 * M_PI;
  }

需要轉換座標系：前左上轉換為左上前座標系，不處理無效點資料。

    // 前左上 轉換 為左上前座標系
    point.x = laserCloudIn[i].y;
    point.y = laserCloudIn[i].z;
    point.z = laserCloudIn[i].x;

    // skip NaN and INF valued points
    // 去除無效點資料
    if (!pcl_isfinite(point.x) ||
        !pcl_isfinite(point.y) ||
        !pcl_isfinite(point.z)) {
      continue;
    }
    // skip zero valued points
    if (point.x * point.x + point.y * point.y + point.z * point.z < 0.0001) {
      continue;
    }

根據垂直角度找到對應的鐳射束ID，這個時候座標系已經發生了改變，注意使用的座標軸資料

    // calculate vertical point angle and scan ID
    float angle = std::atan(point.y / std::sqrt(point.x * point.x + point.z * point.z));
    int scanID = _scanMapper.getRingForAngle(angle);
    if (scanID >= _scanMapper.getNumberOfScanRings() || scanID < 0 ){
      continue;
    }

接下來是求解每個點的水平角度，根據水平角度可以得到獲取每個點時相對與開始點的時間，因為鐳射雷達是先垂直掃描再水平掃描的。但是角度函式atan2的值域是$-\pi$到$\pi$，因此必須要進行變換。這裡將整個水平方向(360度)掃描的點分為兩部分，第一部分與開始點起始角度startOri相差角度在$\pi$以內，第二部分與結束點終止角度endOri相差角度在$\pi$以內。這裡預設使用的角度是逆時針增加。

求解每個點的水平角度，對於atan2的結果需要取負數，以和前面對應，將順時針遞減的角度轉變為逆時針遞增的順序。
注意實際中的鐳射雷達資料不一定從角度0開始的，因此需要考慮到atan2解算的角度在$-\pi$和$\pi$變換時的突變會出問題，需要對角度進一步補償。

例如相鄰點雲資料點的角度由atan2解算並取負數後為$\pi -0.1$，$-\pi +0.1$。在空間上是相鄰的，由於三角函式的關係它們在數值上並不相鄰，因此需要進行補償，主要是對$-\pi +0.1$補償。

• 考慮起始角度startOri為$\pi /2$，現在角度是逆時針增加，那麼當前角度為$-\pi +0.1$時會出現$(-\pi +0.1)-(\pi /2)=-3\ast \pi /2+0.1$。因為$-3\ast \pi /2+0.1$小於0，這也就意味著角度不是增加了，根據角度週期性$2\pi$，實際角度應該為$-\pi +0.1-->\pi +0.1$。這樣才有$(\pi +0.1)-(\pi /2)=1/2\ast \pi +0.1$，這表示角度增加。因此考慮到半個掃描週期並且包含有$\pi$到$-\pi$轉換後有當$\theta -startOri<-1/2\ast \pi$時需要進行補償$\theta +2\pi$可以參考圖解。
  

• 上圖，綠色線表示起始方位，黑色表示當前點的解算方位，由於atan2的原因，角度會出現突變，導致角度需要進行補償，可以看到突變的角度為角度減少$2\pi$，但是根據半個週期的約束，如果包含該突變，起始角度必須位於$[0,\pi ]$之間，也就是說當前解算方位和起始方位最小相差$-\pi$，同時考慮不與順時針旋轉衝突，起始方位角有偏差。因此實際設定只要相差$-\pi /2$就認為當前解算方位和起始方位還沒有相差半個週期角度$\pi$，需要補償當前方位角。而當前解算方位和起始方位相差半個週期角度$\pi$以上時，預設會讓解算角度增加 。

• 同樣的，考慮當角度變換順時針減少時，需要補償條件為($\theta$) - startOri > 3/2*pi。

其實可以看到應該有兩個判斷半週期的條件，一個是ori - startOri > M_PI，一個是ori - startOri < -M_PI。但是velodyne只需要使用ori - startOri > M_PI，估計也就沒有新增了。
或者說ori > startOri + M_PI * 3 / 2是多餘的，沒有用處，因為已經保證了角度增加。

同樣的可以得到endOri和ori的關係，其實就是一一對應罷了。可以參考下面的不等式：

• ori < startOri - M_PI / 2
• ori > endOri + M_PI / 2 --> ori - M_PI / 2> endOri

    // calculate horizontal point angle
    // atan2的角度，值位於-pi到pi之間
    float ori = -std::atan2(point.x, point.z);
    if (!halfPassed) {
      if (ori < startOri - M_PI / 2) {
        ori += 2 * M_PI;
      } else if (ori > startOri + M_PI * 3 / 2) {
        ori -= 2 * M_PI;
      }

      if (ori - startOri > M_PI) {
        halfPassed = true;
      }
    } else {
      ori += 2 * M_PI;

      if (ori < endOri - M_PI * 3 / 2) {
        ori += 2 * M_PI;
      } else if (ori > endOri + M_PI / 2) {
        ori -= 2 * M_PI;
      }
    }

因此根據水平角度可以得到每個點相對於開始點的相對時間relTime，並儲存到intensit中。
對點雲資料格式資訊的利用達到最大化：把pcl::PointXYZI中的intensity設定為相對開始點的時間＋對應線束id，一個資料包含了兩個資訊！！後面還有類似的操作。

// calculate relative scan time based on point orientation
    float relTime = config().scanPeriod * (ori - startOri) / (endOri - startOri);
    point.intensity = scanID + relTime;

那這個相對時間relTime有什麼用呢？馬上就會用到：和IMU資料一起近似去除鐳射的非勻速運動，構建勻速運動模型。

5.4.2 近似勻速運動模型

當IMU資料存在時，使用IMU資料積分得到速度位置對點雲資料進行補償，得到鐳射勻速運動模型下的座標。
首先是獲取IMU資料，使用插值得到精確時間_scanTime + relTime的IMU資料(包含速度，位置，三個姿態角)

// 根據relTime進行IMU資料插值，得到速度，位置資訊。
void BasicScanRegistration::interpolateIMUStateFor(const float &relTime, IMUState &outputState)
{
  // IMU資料和點雲點的時間差：點雲點時間 - IMU時間
  double timeDiff = toSec(_scanTime - _imuHistory[_imuIdx].stamp) + relTime;
  while (_imuIdx < _imuHistory.size() - 1 && timeDiff > 0) {
    _imuIdx++;
  // IMU資料和點雲點的時間差：點雲點時間 - IMU時間
    timeDiff = toSec(_scanTime - _imuHistory[_imuIdx].stamp) + relTime;
  }

  if (_imuIdx == 0 || timeDiff > 0) {
    // 不能插值，1._imuIdx == 0：使用最舊的資料  2. timeDiff > 0：使用最新的資料
    outputState = _imuHistory[_imuIdx];
  } else {
    // 如果IMU緩衝區中可以插值，就直接插值
    float ratio = -timeDiff / toSec(_imuHistory[_imuIdx].stamp - _imuHistory[_imuIdx - 1].stamp);
    IMUState::interpolate(_imuHistory[_imuIdx], _imuHistory[_imuIdx - 1], ratio, outputState);
  }
}

進行插值時特別注意偏航角，這裡和剛才有點類似，$-\pi$和$\pi$的轉換，這是因為一般偏航角度可以360度，必須考慮角度的轉換，而俯仰、滾轉角度變換較小，不會存在$-\pi$和$\pi$的轉換。

    /** \brief Interpolate between two IMU states.
    *
    * @param start the first IMUState
    * @param end the second IMUState
    * @param ratio the interpolation ratio
    * @param result the target IMUState for storing the interpolation result
    */
    static void interpolate(const IMUState& start,
      const IMUState& end,
      const float& ratio,
      IMUState& result)
    {
      float invRatio = 1 - ratio;

      result.roll = start.roll.rad() * invRatio + end.roll.rad() * ratio;
      result.pitch = start.pitch.rad() * invRatio + end.pitch.rad() * ratio;
      if (start.yaw.rad() - end.yaw.rad() > M_PI)
      {
        result.yaw = start.yaw.rad() * invRatio + (end.yaw.rad() + 2 * M_PI) * ratio;
      }
      else if (start.yaw.rad() - end.yaw.rad() < -M_PI)
      {
        result.yaw = start.yaw.rad() * invRatio + (end.yaw.rad() - 2 * M_PI) * ratio;
      }
      else
      {
        result.yaw = start.yaw.rad() * invRatio + end.yaw.rad() * ratio;
      }

      result.velocity = start.velocity * invRatio + end.velocity * ratio;
      result.position = start.position * invRatio + end.position * ratio;
    };

經過上面處理，因此我們擁有了當前點時間(_scanTime + relTime)對應的鐳射位置和速度，
進一步和開始點(_scanTime + 0)的鐳射位置、速度進行處理有
Startposition + PositionShift + 勻速模型(開始點對應的速度) = Curposition。可以考慮鐳射加速運動下，PositionShift表示了開始點時刻到當前時刻鐳射由於加速移動的距離。

• relSweepTime其實等於relTime，表示當前點相對於開始點的時間差。

  // 得到非勻速運動下的鐳射位置偏移量。 Startposition + PositionShift + 勻速模型(開始點對應的速度) = Curposition
  float relSweepTime = toSec(_scanTime - _sweepStart) + relTime;// _scanTime - _sweepStart = 0
  _imuPositionShift = _imuCur.position - _imuStart.position - _imuStart.velocity * relSweepTime;

對於位置偏移，需要在世界座標系下對點雲資料進行處理，這裡使用了之前提到的IMU區域性座標系和世界座標系的轉換，並且由於點雲測量座標和鐳射運動趨勢相反，因此這裡需要加上位置偏移量，特別注意不是減！然後進一步得到IMU區域性座標系下的點雲座標。

void BasicScanRegistration::transformToStartIMU(pcl::PointXYZI& point)
{
  // rotate point to global IMU system
  // IMU區域性座標系到世界座標系轉換
  rotateZXY(point, _imuCur.roll, _imuCur.pitch, _imuCur.yaw);

  // add global IMU position shift
  // 加上位置偏移量，這裡 加 是因為鐳射運動和點雲運動是相反的。
  // 可以考慮加速運動下，PositionShift為正，並且點雲測量座標系變小，因此需要讓點雲座標變大。
  point.x += _imuPositionShift.x();
  point.y += _imuPositionShift.y();
  point.z += _imuPositionShift.z();

  // rotate point back to local IMU system relative to the start IMU state
  // 世界座標系到IMU區域性座標系轉換
  rotateYXZ(point, -_imuStart.yaw, -_imuStart.pitch, -_imuStart.roll);
}

然後將近似勻速運動模型下IMU區域性座標系下的點雲資料進行儲存得到

_laserCloudScans[scanID].push_back(point);

5.4.3 特徵提取

對所有點雲資料進行運動補償之後就是進行特徵點提取了。首先變換點雲資料儲存格式，得到一維狀態下的點雲資料。使用_scanIndices來確定每束資料的開始位置，結束位置。

 pcl::PointCloud<pcl::PointXYZI> _laserCloud;
 std::vector<pcl::PointCloud<pcl::PointXYZI> > _laserCloudScans;

void BasicScanRegistration::processScanlines(const Time& scanTime, std::vector<pcl::PointCloud<pcl::PointXYZI>> const& laserCloudScans)
{
  // reset internal buffers and set IMU start state based on current scan time
  // 為下一次處理做準備，同時清空當前特徵點容器
  reset(scanTime);  

  // construct sorted full resolution cloud
  // 將不同鐳射束點雲資料合併為一維形式，使用_scanIndices來確定每束資料的開始位置，結束位置。
  size_t cloudSize = 0;
  for (int i = 0; i < laserCloudScans.size(); i++) {
    _laserCloud += laserCloudScans[i];

    IndexRange range(cloudSize, 0);
    cloudSize += laserCloudScans[i].size();
    range.second = cloudSize > 0 ? cloudSize - 1 : 0;
    _scanIndices.push_back(range);
  }

  extractFeatures();
  updateIMUTransform();
}

現在最重要的環節就是提取每個鐳射束的特徵點，包含平面特徵點和邊緣特徵點，具體可以參考下面
特徵點的選擇以及注意事項可以參考論文理解部分，這裡不過多講解。在

void BasicScanRegistration::extractFeatures(const uint16_t& beginIdx)

中進行特徵點提取。
提取每個鐳射束點雲資料在一維容器中的的開始索引，結束索引，並且保證點雲資料的數量足夠能夠進行彎曲度計算。

    // 每個鐳射束點雲資料在一維容器中的的開始索引，結束索引
    size_t scanStartIdx = _scanIndices[i].first;
    size_t scanEndIdx = _scanIndices[i].second;

    // skip empty scans
    // 鐳射束中資料不夠
    if (scanEndIdx <= scanStartIdx + 2 * _config.curvatureRegion) {
      continue;
    }

對不不可靠點進行標記，0表示未被選擇，可以作為特徵點，1表示不能被選擇為特徵點。

• 不考慮每次鐳射束的前curvatureRegion，最後curvatureRegion個點，因為不能計算彎曲度。
• 相鄰點距離過大，說明可能不在同一個平面上。加權後的點近似相鄰，但是空間中不相鄰，因此存在被遮擋的可能，平面過於傾斜的可能，屬於不可靠點，需要標記，不能作為特徵點。注意標記是對應標記的區域是根據點的遠近來決定的。
• 考慮A，B，C相鄰三個點，AB距離，BC距離均大於B到鐳射距離一定比例，表明B點可能為離群點，位於過於傾斜的平面，不可靠的測量資料。

void BasicScanRegistration::setScanBuffersFor(const size_t& startIdx, const size_t& endIdx)
{
  // resize buffers
  size_t scanSize = endIdx - startIdx + 1;
  // 0表示未被選擇，可以作為特徵點
  _scanNeighborPicked.assign(scanSize, 0);

  // mark unreliable points as picked
  // 不考慮每次鐳射束的前curvatureRegion，最後curvatureRegion個點，因為不能計算彎曲度
  for (size_t i = startIdx + _config.curvatureRegion; i < endIdx - _config.curvatureRegion; i++) {
    const pcl::PointXYZI& previousPoint = (_laserCloud[i - 1]);
    const pcl::PointXYZI& point = (_laserCloud[i]);
    const pcl::PointXYZI& nextPoint = (_laserCloud[i + 1]);

    float diffNext = calcSquaredDiff(nextPoint, point);
    // 相鄰點距離過大，說明可能不在同一個平面上，需要考慮
    if (diffNext > 0.1) {
      float depth1 = calcPointDistance(point);
      float depth2 = calcPointDistance(nextPoint);
      // 將距離遠的點 座標比例縮放重投影 到 距離近的點平面
      if (depth1 > depth2) {
        // 加權下的相鄰點距離  point點較遠
        float weighted_distance = std::sqrt(calcSquaredDiff(nextPoint, point, depth2 / depth1)) / depth2;
        // 這裡加權距離小說明，加權後的點近似相鄰，但是空間中不相鄰，因此存在被遮擋的可能，平面過於傾斜的可能，
        // 屬於不可靠點，需要標記，不能作為特徵點
        // point遠，那麼point左邊這一部分割槽域點都不能作為特徵點
        if (weighted_distance < 0.1) {
          std::fill_n(&_scanNeighborPicked[i - startIdx - _config.curvatureRegion], _config.curvatureRegion + 1, 1);

          continue;
        }
      } else {
        // 和上面的分析類似
        float weighted_distance = std::sqrt(calcSquaredDiff(point, nextPoint, depth1 / depth2)) / depth1;
        // nextPoint遠，那麼nextPoint右邊這一部分割槽域點都不能作為特徵點
        if (weighted_distance < 0.1) {
          std::fill_n(&_scanNeighborPicked[i - startIdx + 1], _config.curvatureRegion + 1, 1);
        }
      }
    }

    float diffPrevious = calcSquaredDiff(point, previousPoint);
    float dis = calcSquaredPointDistance(point);
    // 考慮A，B，C相鄰三個點，AB距離，BC距離均大於B到鐳射距離一定比例，表明B點可能為離群點，位於過於傾斜的平面，不可靠的測量資料，
    if (diffNext > 0.0002 * dis && diffPrevious > 0.0002 * dis) {
      _scanNeighborPicked[i - startIdx] = 1;
    }
  }
}

將同一鐳射束索引劃分子區域，得到子區域索引sp，ep，

      // 將同一鐳射束索引劃分子區域，得到子區域索引sp ep
      size_t sp = ((scanStartIdx + _config.curvatureRegion) * (_config.nFeatureRegions - j)
                   + (scanEndIdx - _config.curvatureRegion) * j) / _config.nFeatureRegions;
      size_t ep = ((scanStartIdx + _config.curvatureRegion) * (_config.nFeatureRegions - 1 - j)
                   + (scanEndIdx - _config.curvatureRegion) * (j + 1)) / _config.nFeatureRegions - 1;

      // skip empty regions
      // 跳過空的子區域
      if (ep <= sp) {
        continue;
      }

根據論文中向量的歸一化模值公式計運算元區域中每個點的彎曲度並進行排序，彎曲度越大，對應索引儲存位置越靠後。初始所有點的特徵標誌為次平面點。特徵點的種類存放在_regionLabel中(邊緣點，次邊緣點，平面點，次平面點)

void BasicScanRegistration::setRegionBuffersFor(const size_t& startIdx, const size_t& endIdx)
{
  // resize buffers
  size_t regionSize = endIdx - startIdx + 1;
  _regionCurvature.resize(regionSize);
  _regionSortIndices.resize(regionSize);
  // 初始所有點的標誌為 次平面點
  _regionLabel.assign(regionSize, SURFACE_LESS_FLAT);

  // calculate point curvatures and reset sort indices
  float pointWeight = -2 * _config.curvatureRegion;
  // 根據論文中 向量的歸一化模值 計算每個點的彎曲度並儲存
  for (size_t i = startIdx, regionIdx = 0; i <= endIdx; i++, regionIdx++) {
    float diffX = pointWeight * _laserCloud[i].x;
    float diffY = pointWeight * _laserCloud[i].y;
    float diffZ = pointWeight * _laserCloud[i].z;

    for (int j = 1; j <= _config.curvatureRegion; j++) {
      diffX += _laserCloud[i + j].x + _laserCloud[i - j].x;
      diffY += _laserCloud[i + j].y + _laserCloud[i - j].y;
      diffZ += _laserCloud[i + j].z + _laserCloud[i - j].z;
    }

    _regionCurvature[regionIdx] = diffX * diffX + diffY * diffY + diffZ * diffZ;
    _regionSortIndices[regionIdx] = i;
  }

  // sort point curvatures
  // 根據彎曲度將索引排序，彎曲度越大，對應索引儲存位置越靠後
  for (size_t i = 1; i < regionSize; i++) {
    for (size_t j = i; j >= 1; j--) {
      if (_regionCurvature[_regionSortIndices[j] - startIdx] < _regionCurvature[_regionSortIndices[j - 1] - startIdx]) {
        std::swap(_regionSortIndices[j], _regionSortIndices[j - 1]);
      }
    }
  }
}

然後根據彎曲度選取邊緣點，具體操作如下

• 根據彎曲度大小提取邊緣點，彎曲度越大越可能為邊緣點，因此從後面開始尋找
• 未被標記 + 彎曲度滿足閾值 + 邊緣點最大數量未滿足 --> 可被選為邊緣點
• 邊緣點，次邊緣點附近左右curvatureRegion區域點進行標記

      // extract corner features
      int largestPickedNum = 0;
      // 根據彎曲度大小提取邊緣點，彎曲度越大越可能為邊緣點，因此從後面開始尋找
      for (size_t k = regionSize; k > 0 && largestPickedNum < _config.maxCornerLessSharp;) {
        // idx為_laserCloud中的索引  用於提取點雲資料
        // scanIdx為_scanNeighborPicked中的索引   用於標記能夠作為特徵點
        // regionIdx為_regionCurvature，_regionLabel中的索引  用於提取彎曲度，標識特徵型別
        size_t idx = _regionSortIndices[--k];
        size_t scanIdx = idx - scanStartIdx;
        size_t regionIdx = idx - sp;
        // 未被標記 + 彎曲度滿足閾值 + 邊緣點最大數量未滿足 --> 可被選為邊緣點
        // 邊緣點，次邊緣點附近左右curvatureRegion區域點進行標記
        if (_scanNeighborPicked[scanIdx] == 0 &&
            _regionCurvature[regionIdx] > _config.surfaceCurvatureThreshold) {

          largestPickedNum++;
          // 數量為達到則為邊緣點CORNER_SHARP，否則為次邊緣點CORNER_LESS_SHARP
          if (largestPickedNum <= _config.maxCornerSharp) {
            _regionLabel[regionIdx] = CORNER_SHARP;
            // 只存放邊緣點
            _cornerPointsSharp.push_back(_laserCloud[idx]);
          } else {
            _regionLabel[regionIdx] = CORNER_LESS_SHARP;
          }
          // 存放邊緣點和次邊緣點
          _cornerPointsLessSharp.push_back(_laserCloud[idx]);
          // 對選取點附近左右curvatureRegion區域點進行標記
          markAsPicked(idx, scanIdx);
        }
      }

對平面特徵點同樣有

• 根據彎曲度大小提取平面點，彎曲度越小越可能為邊緣點，因此從前面開始尋找
• 未被標記 + 彎曲度滿足閾值 + 邊緣點最大值未滿足 --> 可被選為平面點
• 平面點附近左右curvatureRegion區域點進行標記

     // extract flat surface features
      int smallestPickedNum = 0;
      // 根據彎曲度大小提取平面點，彎曲度越小越可能為邊緣點，因此從前面開始尋找
      for (int k = 0; k < regionSize && smallestPickedNum < _config.maxSurfaceFlat; k++) {
        // idx為_laserCloud中的索引  用於提取點雲資料
        // scanIdx為_scanNeighborPicked中的索引   用於標記能夠作為特徵點
        // regionIdx為_regionCurvature，_regionLabel中的索引  用於提取彎曲度，標識特徵型別
        size_t idx = _regionSortIndices[k];
        size_t scanIdx = idx - scanStartIdx;
        size_t regionIdx = idx - sp;
        // 未被標記 + 彎曲度滿足閾值 + 邊緣點最大值未滿足 --> 可被選為平面點
        // 平面點點附近左右curvatureRegion區域點進行標記
        if (_scanNeighborPicked[scanIdx] == 0 &&
            _regionCurvature[regionIdx] < _config.surfaceCurvatureThreshold) {

          smallestPickedNum++;
          _regionLabel[regionIdx] = SURFACE_FLAT;
          // 只存放平面點
          _surfacePointsFlat.push_back(_laserCloud[idx]);

          markAsPicked(idx, scanIdx);
        }
      }
      // extract less flat surface features
      // 存放所有平面點，次平面點。也就是非邊緣點，次邊緣點的所有點
      for (int k = 0; k < regionSize; k++) {
        if (_regionLabel[k] <= SURFACE_LESS_FLAT) {
          surfPointsLessFlatScan->push_back(_laserCloud[sp + k]);
        }
      }

最後對所有的次平面點進行降取樣

    // down size less flat surface point cloud of current scan
    // 對次平面點進行降取樣得到_surfacePointsLessFlat
    pcl::PointCloud<pcl::PointXYZI> surfPointsLessFlatScanDS;
    pcl::VoxelGrid<pcl::PointXYZI> downSizeFilter;
    downSizeFilter.setInputCloud(surfPointsLessFlatScan);
    downSizeFilter.setLeafSize(_config.lessFlatFilterSize, _config.lessFlatFilterSize, _config.lessFlatFilterSize);
    downSizeFilter.filter(surfPointsLessFlatScanDS);
    _surfacePointsLessFlat += surfPointsLessFlatScanDS;

到此，所有的特徵點存放在_cornerPointsSharp、_cornerPointsLessSharp、_surfacePointsFlat、_surfacePointsLessFlat(經過濾波)中。

5.4.4 打包IMU資料

這個時候需要對IMU變換資料進行儲存打包，把imu資訊儲存在點雲格式的資料中，在鐳射里程計中還會加以使用。

• 獲取點雲中第一個點時對應的鐳射姿態角_imuStart，獲取點雲中最後一個點時對應的鐳射姿態角_imuCur
• 獲取點雲中最後一個點時鐳射相對於獲取開始點時的鐳射的 非勻速運動 位置偏移 開始點對應的IMU區域性座標系imuShiftFromStart
• 獲取點雲中最後一個點時鐳射相對於獲取開始點時的鐳射的 非勻速運動 速度偏移 開始點對應的IMU區域性座標系imuVelocityFromStart
• 點雲已經去除了非勻速運動，為什麼還需要呢？因為後面估計的是鐳射運動，不是點雲運動，因此必須要在鐳射里程計中恢復原來的非勻速運動偏移

void BasicScanRegistration::updateIMUTransform()
{
  // 更新IMU變換的資料，可以供鐳射里程計中使用
  // 獲取點雲中第一個點時對應的鐳射姿態角
  _imuTrans[0].x = _imuStart.pitch.rad();
  _imuTrans[0].y = _imuStart.yaw.rad();
  _imuTrans[0].z = _imuStart.roll.rad();
  // 獲取點雲中最後一個點時對應的鐳射姿態角
  _imuTrans[1].x = _imuCur.pitch.rad();
  _imuTrans[1].y = _imuCur.yaw.rad();
  _imuTrans[1].z = _imuCur.roll.rad();

  Vector3 imuShiftFromStart = _imuPositionShift;
  // 世界座標系到IMU區域性座標系轉換
  rotateYXZ(imuShiftFromStart, -_imuStart.yaw, -_imuStart.pitch, -_imuStart.roll);
  // 獲取點雲中最後一個點時鐳射相對於獲取開始點時的鐳射的 非勻速運動 位置偏移 開始點對應的IMU區域性座標系
  _imuTrans[2].x = imuShiftFromStart.x();
  _imuTrans[2].y = imuShiftFromStart.y();
  _imuTrans[2].z = imuShiftFromStart.z();

  Vector3 imuVelocityFromStart = _imuCur.velocity - _imuStart.velocity;
  // 世界座標系到IMU區域性座標系轉換
  rotateYXZ(imuVelocityFromStart, -_imuStart.yaw, -_imuStart.pitch, -_imuStart.roll);
  // 獲取點雲中最後一個點時鐳射相對於獲取開始點時的鐳射的 非勻速運動 速度偏移 開始點對應的IMU區域性座標系
  _imuTrans[3].x = imuVelocityFromStart.x();
  _imuTrans[3].y = imuVelocityFromStart.y();
  _imuTrans[3].z = imuVelocityFromStart.z();
}

5.5釋出資料

最後將得到點雲資料以及IMU資料進行釋出。_pubLaserCloud是近似勻速模型下的所有點雲資料。其他的前面已經涉及到了，就不再贅述。

void ScanRegistration::publishResult()
{
  auto sweepStartTime = toROSTime(sweepStart());
  // publish full resolution and feature point clouds
  // 釋出點雲資料，特徵點雲資料
  publishCloudMsg(_pubLaserCloud, laserCloud(), sweepStartTime, "/camera");
  publishCloudMsg(_pubCornerPointsSharp, cornerPointsSharp(), sweepStartTime, "/camera");
  publishCloudMsg(_pubCornerPointsLessSharp, cornerPointsLessSharp(), sweepStartTime, "/camera");
  publishCloudMsg(_pubSurfPointsFlat, surfacePointsFlat(), sweepStartTime, "/camera");
  publishCloudMsg(_pubSurfPointsLessFlat, surfacePointsLessFlat(), sweepStartTime, "/camera");

  // publish corresponding IMU transformation information
  // 釋出IMU測量的鐳射位姿變換資料
  publishCloudMsg(_pubImuTrans, imuTransform(), sweepStartTime, "/camera");
}

更新日誌

• 2020-10-11
  整理完成MultiScanRegistration節點內容
• 2020-10-09
  第一次編輯，初版