PCL - ICP代碼研讀（八 ） - transformCloud函數

前言

如PCL - ICP代碼研讀（六 ） - IterativeClosestPoint架構中所介紹的，transformCloud函數用於對輸入點雲的XYZ坐標及法向量做剛體變換。

本篇中介紹的是IterativeClosestPoint::transformCloud，icp.hpp中還另外定義了一個IterativeClosestPointWithNormals::transformCloud函數，在此略過不述。

transformCloud

transformCloud函數會對輸入點雲input使用transform的4*4矩陣做轉換，然後輸出到output點雲。這個函數在computeTransform函數中多次被用到。

template <typename PointSource, typename PointTarget, typename Scalar>
void
IterativeClosestPoint<PointSource, PointTarget, Scalar>::transformCloud(
    const PointCloudSource& input, PointCloudSource& output, const Matrix4& transform)
{

pt表示一個三維點，pt_t表示轉換後的三維點，兩者都是使用齊次表示法。

  Eigen::Vector4f pt(0.0f, 0.0f, 0.0f, 1.0f), pt_t;

4*4變換矩陣：

  // 4*4的變換矩陣
  Eigen::Matrix4f tr = transform.template cast<float>();

關於template cast可參考.template cast＜＞中template的作用。

在source點雲帶有法向量的情況下，執行if分支：

  //?
  // XYZ is ALWAYS present due to the templatization, so we only have to check for
  // normals
  if (source_has_normals_) {

事先宣告nt為法向量，nt_t為轉換後的法向量：

    Eigen::Vector3f nt, nt_t;

變換矩陣的左上角為旋轉矩陣：

    Eigen::Matrix3f rot = tr.block<3, 3>(0, 0);

看不太懂這邊轉換為std::uint8_t的用意：

    for (std::size_t i = 0; i < input.size(); ++i) {
      // 把input點雲的第i個元素的"位址"轉成std::uint8_t*的data_in
      // 這裡為什麼cast成uint8_t?
      // std::uint8_t為8bit
      const std::uint8_t* data_in = reinterpret_cast<const std::uint8_t*>(&input[i]);
      std::uint8_t* data_out = reinterpret_cast<std::uint8_t*>(&output[i]);

把data_in（也就是input[i]）中的XYZ坐標複製到pt：

      // 然後再把data_in複製到pt -> 為什麼要用memcpy不直接用=?
      /**
       * 點雲底層的資料型別一定是float?
       * 查看Registration類別的模板:
       * template <typename PointSource, typename PointTarget, typename Scalar = float>
       * 底層資料型別可以是float或double,
       * 感覺這邊要改成sizeof(Scalar)然後轉成float才對?
       **/
      memcpy(&pt[0], data_in + x_idx_offset_, sizeof(float));
      memcpy(&pt[1], data_in + y_idx_offset_, sizeof(float));
      memcpy(&pt[2], data_in + z_idx_offset_, sizeof(float));

      if (!std::isfinite(pt[0]) || !std::isfinite(pt[1]) || !std::isfinite(pt[2]))
        continue;

使用變換矩陣tr對pt做旋轉及平移，得到旋轉平移後的法向量pt_t：

      pt_t = tr * pt;

把pt_t的內容複製到（也就是output[i]）的XYZ坐標欄位：

      // 由pt_t複製到data_out
      memcpy(data_out + x_idx_offset_, &pt_t[0], sizeof(float));
      memcpy(data_out + y_idx_offset_, &pt_t[1], sizeof(float));
      memcpy(data_out + z_idx_offset_, &pt_t[2], sizeof(float));

把data_in（也就是input[i]）中的法向量複製到nt：

      // 將法向量由data_in複製到nt
      memcpy(&nt[0], data_in + nx_idx_offset_, sizeof(float));
      memcpy(&nt[1], data_in + ny_idx_offset_, sizeof(float));
      memcpy(&nt[2], data_in + nz_idx_offset_, sizeof(float));

      if (!std::isfinite(nt[0]) || !std::isfinite(nt[1]) || !std::isfinite(nt[2]))
        continue;

使用旋轉矩陣rot對nt做旋轉，得到旋轉後的法向量nt_t（注意：不需要對法向量做平移）：

      // 對法向量只做旋轉,不做平移(是用rot左乘而非tr)
      nt_t = rot * nt;

把nt_t的內容複製到data_out（也就是output[i]）的法向量欄位：

      // 將法向量由nt_t複製到data_out
      memcpy(data_out + nx_idx_offset_, &nt_t[0], sizeof(float));
      memcpy(data_out + ny_idx_offset_, &nt_t[1], sizeof(float));
      memcpy(data_out + nz_idx_offset_, &nt_t[2], sizeof(float));
    }
  }

else分支的代碼與if分支的代碼大同小異：

  else {
    // 這段代碼與上面的重複了，應該可以只寫一次
    for (std::size_t i = 0; i < input.size(); ++i) {
      const std::uint8_t* data_in = reinterpret_cast<const std::uint8_t*>(&input[i]);
      std::uint8_t* data_out = reinterpret_cast<std::uint8_t*>(&output[i]);
      memcpy(&pt[0], data_in + x_idx_offset_, sizeof(float));
      memcpy(&pt[1], data_in + y_idx_offset_, sizeof(float));
      memcpy(&pt[2], data_in + z_idx_offset_, sizeof(float));

      if (!std::isfinite(pt[0]) || !std::isfinite(pt[1]) || !std::isfinite(pt[2]))
        continue;

      pt_t = tr * pt;

      memcpy(data_out + x_idx_offset_, &pt_t[0], sizeof(float));
      memcpy(data_out + y_idx_offset_, &pt_t[1], sizeof(float));
      memcpy(data_out + z_idx_offset_, &pt_t[2], sizeof(float));
    }
  }
}