PCL - ICP代碼研讀（二七） - TransformationEstimationPointToPlaneLLS實現

最新推荐文章于 2024-05-26 19:45:00 发布

keineahnung2345

最新推荐文章于 2024-05-26 19:45:00 发布

阅读量1.2k

点赞数

分类专栏：三維點雲文章标签： 1024程序员节 c++ pcl icp 点云

本文链接：https://blog.csdn.net/keineahnung2345/article/details/120930292

版权

三維點雲专栏收录该内容

50 篇文章 40 订阅

订阅专栏

PCL - ICP代碼研讀（二七） - TransformationEstimationPointToPlaneLLS實現

前言
TransformationEstimationPointToPlaneLLS

前言

TransformationEstimationPointToPlaneLLS類別中有五個estimateRigidTransformation函數，其中四個是public的，另一個是protected的。前四個public的estimateRigidTransformation都是protected的estimateRigidTransformation的wrapper。

其中protected的estimateRigidTransformation函數部分的代碼，建議與ICP變種Point-To-Plane算法推導交互參看。

本篇對應到transformation_estimation_point_to_plane_lls.hpp這個檔案。

TransformationEstimationPointToPlaneLLS

estimateRigidTransformation的wrapper

以下這四個函數都是estimateRigidTransformation(ConstCloudIterator<PointSource>& source_it, ConstCloudIterator<PointTarget>& target_it, Matrix4& transformation_matrix)這個protected函數的wrapper。它們會將各自的輸入都轉為ConstCloudIterator後，再調用protected版本的estimateRigidTransformation。

#ifndef PCL_REGISTRATION_TRANSFORMATION_ESTIMATION_POINT_TO_PLANE_LLS_HPP_
#define PCL_REGISTRATION_TRANSFORMATION_ESTIMATION_POINT_TO_PLANE_LLS_HPP_

#include <pcl/cloud_iterator.h>

namespace pcl {

namespace registration {

// 將輸入轉為ConstCloudIterator後調用protected函數estimateRigidTransformation
template <typename PointSource, typename PointTarget, typename Scalar>
inline void
TransformationEstimationPointToPlaneLLS<PointSource, PointTarget, Scalar>::
    estimateRigidTransformation(const pcl::PointCloud<PointSource>& cloud_src,
                                const pcl::PointCloud<PointTarget>& cloud_tgt,
                                Matrix4& transformation_matrix) const
{
  const auto nr_points = cloud_src.size();
  if (cloud_tgt.size() != nr_points) {
    PCL_ERROR(
        "[pcl::TransformationEstimationPointToPlaneLLS::estimateRigidTransformation] "
        "Number or points in source (%zu) differs than target (%zu)!\n",
        static_cast<std::size_t>(nr_points),
        static_cast<std::size_t>(cloud_tgt.size()));
    return;
  }

  ConstCloudIterator<PointSource> source_it(cloud_src);
  ConstCloudIterator<PointTarget> target_it(cloud_tgt);
  estimateRigidTransformation(source_it, target_it, transformation_matrix);
}

// 將輸入轉為ConstCloudIterator後調用protected函數estimateRigidTransformation
template <typename PointSource, typename PointTarget, typename Scalar>
void
TransformationEstimationPointToPlaneLLS<PointSource, PointTarget, Scalar>::
    estimateRigidTransformation(const pcl::PointCloud<PointSource>& cloud_src,
                                const pcl::Indices& indices_src,
                                const pcl::PointCloud<PointTarget>& cloud_tgt,
                                Matrix4& transformation_matrix) const
{
  const auto nr_points = indices_src.size();
  if (cloud_tgt.size() != nr_points) {
    PCL_ERROR(
        "[pcl::TransformationEstimationPointToPlaneLLS::estimateRigidTransformation] "
        "Number or points in source (%zu) differs than target (%zu)!\n",
        indices_src.size(),
        static_cast<std::size_t>(cloud_tgt.size()));
    return;
  }

  ConstCloudIterator<PointSource> source_it(cloud_src, indices_src);
  ConstCloudIterator<PointTarget> target_it(cloud_tgt);
  estimateRigidTransformation(source_it, target_it, transformation_matrix);
}

template <typename PointSource, typename PointTarget, typename Scalar>
inline void
TransformationEstimationPointToPlaneLLS<PointSource, PointTarget, Scalar>::
    estimateRigidTransformation(const pcl::PointCloud<PointSource>& cloud_src,
                                const pcl::Indices& indices_src,
                                const pcl::PointCloud<PointTarget>& cloud_tgt,
                                const pcl::Indices& indices_tgt,
                                Matrix4& transformation_matrix) const
{
  const auto nr_points = indices_src.size();
  if (indices_tgt.size() != nr_points) {
    PCL_ERROR(
        "[pcl::TransformationEstimationPointToPlaneLLS::estimateRigidTransformation] "
        "Number or points in source (%zu) differs than target (%zu)!\n",
        indices_src.size(),
        indices_tgt.size());
    return;
  }

  ConstCloudIterator<PointSource> source_it(cloud_src, indices_src);
  ConstCloudIterator<PointTarget> target_it(cloud_tgt, indices_tgt);
  estimateRigidTransformation(source_it, target_it, transformation_matrix);
}

template <typename PointSource, typename PointTarget, typename Scalar>
inline void
TransformationEstimationPointToPlaneLLS<PointSource, PointTarget, Scalar>::
    estimateRigidTransformation(const pcl::PointCloud<PointSource>& cloud_src,
                                const pcl::PointCloud<PointTarget>& cloud_tgt,
                                const pcl::Correspondences& correspondences,
                                Matrix4& transformation_matrix) const
{
  ConstCloudIterator<PointSource> source_it(cloud_src, correspondences, true);
  ConstCloudIterator<PointTarget> target_it(cloud_tgt, correspondences, false);
  estimateRigidTransformation(source_it, target_it, transformation_matrix);
}

constructTransformationMatrix

template <typename PointSource, typename PointTarget, typename Scalar>
inline void
TransformationEstimationPointToPlaneLLS<PointSource, PointTarget, Scalar>::
    constructTransformationMatrix(const double& alpha,
                                  const double& beta,
                                  const double& gamma,
                                  const double& tx,
                                  const double& ty,
                                  const double& tz,
                                  Matrix4& transformation_matrix) const
{

左上角3*3的矩陣是為旋轉矩陣，推導詳見R,t參數化：

  // Construct the transformation matrix from rotation and translation
  // 旋轉矩陣:Rz(gamma)*Ry(beta)*Rx(alpha)的結果
  // 推導詳見https://blog.csdn.net/keineahnung2345/article/details/120880598?spm=1001.2014.3001.5501#t3
  transformation_matrix = Eigen::Matrix<Scalar, 4, 4>::Zero();
  transformation_matrix(0, 0) = static_cast<Scalar>(std::cos(gamma) * std::cos(beta));
  transformation_matrix(0, 1) = static_cast<Scalar>(
      -sin(gamma) * std::cos(alpha) + std::cos(gamma) * sin(beta) * sin(alpha));
  transformation_matrix(0, 2) = static_cast<Scalar>(
      sin(gamma) * sin(alpha) + std::cos(gamma) * sin(beta) * std::cos(alpha));
  transformation_matrix(1, 0) = static_cast<Scalar>(sin(gamma) * std::cos(beta));
  transformation_matrix(1, 1) = static_cast<Scalar>(
      std::cos(gamma) * std::cos(alpha) + sin(gamma) * sin(beta) * sin(alpha));
  transformation_matrix(1, 2) = static_cast<Scalar>(
      -std::cos(gamma) * sin(alpha) + sin(gamma) * sin(beta) * std::cos(alpha));
  transformation_matrix(2, 0) = static_cast<Scalar>(-sin(beta));
  transformation_matrix(2, 1) = static_cast<Scalar>(std::cos(beta) * sin(alpha));
  transformation_matrix(2, 2) = static_cast<Scalar>(std::cos(beta) * std::cos(alpha));

右上角3*1的向量是為平移向量：

  transformation_matrix(0, 3) = static_cast<Scalar>(tx);
  transformation_matrix(1, 3) = static_cast<Scalar>(ty);
  transformation_matrix(2, 3) = static_cast<Scalar>(tz);
  transformation_matrix(3, 3) = static_cast<Scalar>(1);
}

estimateRigidTransformation

參考ICP變種Point-To-Plane算法推導，estimateRigidTransformation函數使用Moore–Penrose偽逆，即取 $\hat{x} = A^+b = (A^TA)^{-1}A^Tb$ 來使得損失函數最小化。其中 $A^TA$ 是一個6*6矩陣， $A^Tb$ 是一個6*1向量，此處要求的解 $x$ 亦是一個6*1向量，即 $\begin{bmatrix}\alpha \\ \beta \\ \gamma \\ t_x \\ t_y \\ t_z\end{bmatrix}$ 。

template <typename PointSource, typename PointTarget, typename Scalar>
inline void
TransformationEstimationPointToPlaneLLS<PointSource, PointTarget, Scalar>::
    estimateRigidTransformation(ConstCloudIterator<PointSource>& source_it,
                                ConstCloudIterator<PointTarget>& target_it,
                                Matrix4& transformation_matrix) const
{
  using Vector6d = Eigen::Matrix<double, 6, 1>;
  using Matrix6d = Eigen::Matrix<double, 6, 6>;

對 $A^TA$ 矩陣和 $A^Tb$ 向量做初始化：

  Matrix6d ATA;
  Vector6d ATb;
  ATA.setZero();
  ATb.setZero();

遍歷source_it及target_it這兩個迭代器：

  // Approximate as a linear least squares problem
  while (source_it.isValid() && target_it.isValid()) {

忽略坐標是無限遠的點和法向量是無限大的點：

    // 如果source點雲中的某點或target點雲中的某點或其法向量是無效值,就跳過
    if (!std::isfinite(source_it->x) || !std::isfinite(source_it->y) ||
        !std::isfinite(source_it->z) || !std::isfinite(target_it->x) ||
        !std::isfinite(target_it->y) || !std::isfinite(target_it->z) ||
        !std::isfinite(target_it->normal_x) || !std::isfinite(target_it->normal_y) ||
        !std::isfinite(target_it->normal_z)) {
      ++target_it;
      ++source_it;
      continue;
    }

使用s?表示source點，d?表示target點，n?表示target點法向量：

    // source
    const float& sx = source_it->x;
    const float& sy = source_it->y;
    const float& sz = source_it->z;
    // destination
    const float& dx = target_it->x;
    const float& dy = target_it->y;
    const float& dz = target_it->z;
    const float& nx = target_it->normal[0];
    const float& ny = target_it->normal[1];
    const float& nz = target_it->normal[2];

代碼中的a,b,c對應到最小二乘問題中的 $a_{i1},a_{i2},a_{i3}$ ，其中下標 $i$ 代表當前點的索引。

    // alpha,beta,gamma的multiplier
    // https://blog.csdn.net/keineahnung2345/article/details/120880598?spm=1001.2014.3001.5501#t4中的a1,a2,a3
    double a = nz * sy - ny * sz;
    double b = nx * sz - nz * sx;
    double c = ny * sx - nx * sy;

ATA是一個6*6的矩陣，它的計算方式詳見Moore–Penrose偽逆。

    //    0  1  2  3  4  5
    //    6  7  8  9 10 11
    //   12 13 14 15 16 17
    //   18 19 20 21 22 23
    //   24 25 26 27 28 29
    //   30 31 32 33 34 35

    // https://blog.csdn.net/keineahnung2345/article/details/120880598?spm=1001.2014.3001.5501#t4中的A^T*A
    ATA.coeffRef(0) += a * a;
    ATA.coeffRef(1) += a * b;
    ATA.coeffRef(2) += a * c;
    ATA.coeffRef(3) += a * nx;
    ATA.coeffRef(4) += a * ny;
    ATA.coeffRef(5) += a * nz;
    ATA.coeffRef(7) += b * b;
    ATA.coeffRef(8) += b * c;
    ATA.coeffRef(9) += b * nx;
    ATA.coeffRef(10) += b * ny;
    ATA.coeffRef(11) += b * nz;
    ATA.coeffRef(14) += c * c;
    ATA.coeffRef(15) += c * nx;
    ATA.coeffRef(16) += c * ny;
    ATA.coeffRef(17) += c * nz;
    ATA.coeffRef(21) += nx * nx;
    ATA.coeffRef(22) += nx * ny;
    ATA.coeffRef(23) += nx * nz;
    ATA.coeffRef(28) += ny * ny;
    ATA.coeffRef(29) += ny * nz;
    ATA.coeffRef(35) += nz * nz;

ATb是一個6*1的向量，它的計算方式詳見Moore–Penrose偽逆。

    // https://blog.csdn.net/keineahnung2345/article/details/120880598?spm=1001.2014.3001.5501#t4中的bi
    double d = nx * dx + ny * dy + nz * dz - nx * sx - ny * sy - nz * sz;
    // https://blog.csdn.net/keineahnung2345/article/details/120880598?spm=1001.2014.3001.5501#t6中的A^T*b
    ATb.coeffRef(0) += a * d;
    ATb.coeffRef(1) += b * d;
    ATb.coeffRef(2) += c * d;
    ATb.coeffRef(3) += nx * d;
    ATb.coeffRef(4) += ny * d;
    ATb.coeffRef(5) += nz * d;

繼續看下一個點對：

    ++target_it;
    ++source_it;
  }

因為 $A^TA$ 是對稱矩陣，所以可以拿對角線另外一側的元素來填補：

  // A^T*A是對稱矩陣
  ATA.coeffRef(6) = ATA.coeff(1);
  ATA.coeffRef(12) = ATA.coeff(2);
  ATA.coeffRef(13) = ATA.coeff(8);
  ATA.coeffRef(18) = ATA.coeff(3);
  ATA.coeffRef(19) = ATA.coeff(9);
  ATA.coeffRef(20) = ATA.coeff(15);
  ATA.coeffRef(24) = ATA.coeff(4);
  ATA.coeffRef(25) = ATA.coeff(10);
  ATA.coeffRef(26) = ATA.coeff(16);
  ATA.coeffRef(27) = ATA.coeff(22);
  ATA.coeffRef(30) = ATA.coeff(5);
  ATA.coeffRef(31) = ATA.coeff(11);
  ATA.coeffRef(32) = ATA.coeff(17);
  ATA.coeffRef(33) = ATA.coeff(23);
  ATA.coeffRef(34) = ATA.coeff(29);

使用Moore–Penrose偽逆公式求解使得損失函數最小化的x：

  // Solve A*x = b
  // x = (A^TA)^(-1)A^Tb
  Vector6d x = static_cast<Vector6d>(ATA.inverse() * ATb);

將6*1向量x轉換成4*4的轉換矩陣transformation_matrix（出參）：

  // Construct the transformation matrix from x
  constructTransformationMatrix(
      x(0), x(1), x(2), x(3), x(4), x(5), transformation_matrix);
}

} // namespace registration
} // namespace pcl

#endif /* PCL_REGISTRATION_TRANSFORMATION_ESTIMATION_POINT_TO_PLANE_LLS_HPP_ */