车载激光雷达标定到组合导航

  下图显示了3个对应点（需要求解的最小点）之间的旋转变换。

  假设Dataset A为在激光雷达坐标系下测得的标定板中心坐标集，Dataset B为在组合导航坐标系下测得的标定板中心的坐标。相应的点具有相同的颜色，$R$表示旋转，$t$表示平移。我们希望找到将数据集A中的点与数据集B中的点对齐的最佳旋转和平移。这里，“最优”或“最佳”是指最小平方误差。这种变换有时称为欧几里德变换或刚性变换，因为它保留了形状和大小。这与仿射变换不同，仿射变换包括缩放和剪切。

  根据上述描述，假设两个数据集没有噪声，则可以得到如下数学模型：
$$B=R\ast A+t\text{\hspace{1em}(1)}$$
  如果两个数据集带有噪声，则需要最小化下列最小二乘误差：
$$err=\sum _{i=1}^n{\Vert R\ast A^i+t-B^i\Vert }^2\text{\hspace{1em}(2)}$$

  为了寻找$R$和$t$，通常需要一下三个步骤：
  $\bullet$ 计算点集合的中心点
  $\bullet$ 将点集合移动到原点，计算最优旋转矩阵$R$
  $\bullet$ 计算转移矩阵$T$

  计算点集合的中心点
  点集合的中心点计算非常简单，即只需要计算所有点每个维度坐标的平均值，可按如下公式进行计算：
$$centroi{d}_A=\frac{1}{N}\sum _{i=1}^N{P}_A^i\text{\hspace{1em}(3)}$$
$$centroi{d}_B=\frac{1}{N}\sum _{i=1}^N{P}_B^i\text{\hspace{1em}(4)}$$
其中$P_A^i$和$P_B^i$是$3\times 1$的矢量$\left[\begin{array}{c}x\\ y\\ z\end{array}\right]$。

  计算最优旋转矩阵
  可以使用奇异值分解（SVD）来求解得到最优旋转矩阵，首先将两个数据集重新居中，使两个中心点都位于原点，如下图所示。

  通过将两个数据集居中之后，消除了两个数据集之间的平移成分，从而只剩下旋转需要进行处理。接下来需要计算居中之后两个数据集之间的协方差矩阵，然后利用SVD求解得到旋转矩阵：
$$H=\sum _{i=1}^N(P_A^i-centroi{d}_A)(P_B^i-centroi{d}_B{)}^T\text{\hspace{1em}(5)}$$
$$[U,S,V]=SVD(H)\text{\hspace{1em}(6)}$$
$$R=V{U}^T\text{\hspace{1em}(7)}$$
  在求解旋转矩阵时，有一种特殊情况需要注意。有时SVD会返回一个“反射”矩阵，这在数值上是正确的，但在现实生活中实际上是无意义的。因此需要通过检查R的行列式（来自上面的SVD）并查看它是否为负（-1）来解决这种特殊情况。如果是，则V的第三列乘以-1。

if determinant(R) < 0
    [U,S,V] = svd(R)
    multiply 3rd column of V by -1
    R = V * transpose(U)
end if

  计算平移矩阵
  对两个数据集中的每个点运用公式（1），可以得到：
$$\sum _{i=1}^N(R\times P_A^i+T)=P_B^i\text{\hspace{1em}(8)}$$
上式等式两边分别除以数据集的个数$N$可以得到：
$$T=centroi{d}_B-R\times centroi{d}_A\text{\hspace{1em}(9)}$$

  C++代码实现

/**
 * @brief 对激光雷达进行标定，输出激光雷达相对于东北天的旋转矩阵和平移矩阵
 * @param pts1  输入--根据组合导航数据计算得到的标定板中心东北天坐标，相对于选定的东北天坐标系原点
 * @param pts2  输入--根据对激光雷达原始点云数据进行处理提取出来的标定板在激光雷达坐标系下的中心坐标
 * @param R     输出--旋转矩阵
 * @param T     输出--平移矩阵
 */
void LidarCalib::pose_estimation_3d3d(const vector<Eigen::Vector3d> &pts1, const vector<Eigen::Vector3d> &pts2,
                              Eigen::Matrix3d &R, Eigen::Vector3d &T){
    // pts1 = R * pts2 + T;
    Eigen::Vector3d p1(0,0,0), p2(0,0,0); // center of mass
    int N = pts1.size();
    for (int i = 0; i < N; i++){
        p1 += pts1[i];
        p2 += pts2[i];
    }
    p1 = p1 / N;
    p2 = p2 / N;

    vector<Eigen::Vector3d> q1, q2; // remove the center
    for (int i = 0; i < N; i++){
        Eigen::Vector3d q1_temp, q2_temp;
        q1_temp= pts1[i] - p1;
        q2_temp = pts2[i] - p2;
        q1.push_back(q1_temp);
        q2.push_back(q2_temp);
    }

    // compute q1*q2^T
    Eigen::Matrix3d W = Eigen::Matrix3d::Zero();
    for(int i = 0; i < N; i++){
        W += q2[i] * q1[i].transpose();
    }

    // SVD on W
    Eigen::JacobiSVD<Eigen::Matrix3d> svd(W, Eigen::ComputeFullU | Eigen::ComputeFullV);
    Eigen::Matrix3d U = svd.matrixU();
    Eigen::Matrix3d V = svd.matrixV();

    Eigen::Matrix3d E;
    E << 1,0,0,0,1,0,0,0,(V * (U.transpose())).determinant();

    // pts1 = R * pts2 + T;
    // ENU = R * Lidar + T;
    R = V * E * (U.transpose());
    T = p1 - R * p2;
}