点云 PCA 坐标系与目标坐标系转换

点云 PCA 坐标系对齐到指定坐标系

1. 问题描述

对点云进行 PCA 后，可以得到：

• 点云质心 \(c\)
• 三个正交单位特征向量 \(e_1,e_2,e_3\)

这三个特征向量可以作为点云自身坐标系的三个轴。

现在给定一个目标坐标系：

• 目标坐标系原点为 \(o\)
• 三个目标轴为 \(t_1,t_2,t_3\)

目标是将整个点云作为刚体移动，使：

• 点云质心移动到目标原点 \(o\)
• PCA 第一轴对齐目标第一轴
• PCA 第二轴对齐目标第二轴
• PCA 第三轴对齐目标第三轴
• 点云形状和尺寸保持不变

---

2. 符号约定

本文统一使用列向量。

点的世界坐标为：

\[p= \left[ \begin{array}{c} x\\ y\\ z \end{array} \right] \]

PCA 三个轴按列组成矩阵：

\[E= \left[ e_1\ e_2\ e_3 \right] \]

目标坐标系三个轴按列组成矩阵：

\[T= \left[ t_1\ t_2\ t_3 \right] \]

展开后：

\[T= \left[ \begin{array}{ccc} x_1 & x_2 & x_3\\ y_1 & y_2 & y_3\\ z_1 & z_2 & z_3 \end{array} \right] \]

其中：

\[t_1= \left[ \begin{array}{c} x_1\\ y_1\\ z_1 \end{array} \right] ,\quad t_2= \left[ \begin{array}{c} x_2\\ y_2\\ z_2 \end{array} \right] ,\quad t_3= \left[ \begin{array}{c} x_3\\ y_3\\ z_3 \end{array} \right] \]

---

3. PCA 基底的性质

点云协方差矩阵是实对称矩阵，因此其特征向量可以取为相互正交的单位向量。

所以：

\[E^TE=I \]

因此：

\[E^{-1}=E^T \]

如果目标坐标系的三个轴也是正交单位向量，则：

\[T^TT=I \]

并且：

\[T^{-1}=T^T \]

如果还满足：

\[\det(E)=1,\qquad \det(T)=1 \]

那么 \(E\) 和 \(T\) 都表示右手旋转基底。

---

4. 计算点在 PCA 坐标系中的局部坐标

设点云中的一个点为 \(p\)，点云质心为 \(c\)。

先计算点相对于质心的向量：

\[q=p-c \]

这里的 \(q\) 表示：

从点云质心指向点 \(p\) 的向量。

接着，将 \(q\) 投影到三个 PCA 轴上：

\[a_1=e_1^Tq \]

\[a_2=e_2^Tq \]

\[a_3=e_3^Tq \]

写成矩阵形式：

\[a=E^T(p-c) \]

其中：

\[a= \left[ \begin{array}{c} a_1\\ a_2\\ a_3 \end{array} \right] \]

这组坐标表示：

• 沿 PCA 第一轴走 \(a_1\)
• 沿 PCA 第二轴走 \(a_2\)
• 沿 PCA 第三轴走 \(a_3\)

例如：

\[a= \left[ \begin{array}{c} 2\\ 1\\ 0 \end{array} \right] \]

表示这个点相对于物体质心：

• 沿物体自身第一轴走 2
• 沿物体自身第二轴走 1
• 沿物体自身第三轴走 0

---

5. 为什么乘 \(E^T\) 可以得到 PCA 坐标

因为：

\[E= \left[ e_1\ e_2\ e_3 \right] \]

所以：

\[E^T= \left[ \begin{array}{c} e_1^T\\ e_2^T\\ e_3^T \end{array} \right] \]

因此：

\[E^Tq= \left[ \begin{array}{c} e_1^Tq\\ e_2^Tq\\ e_3^Tq \end{array} \right] \]

结果中的三个元素分别是 \(q\) 在三个 PCA 轴上的有符号投影长度。

所以：

\[E^Tq \]

完成的是：

\[\text{空间向量} \rightarrow \text{PCA 坐标} \]

---

6. PCA 坐标与原空间向量的关系

既然：

\[a=E^Tq \]

又因为：

\[E^{-1}=E^T \]

所以：

\[q=Ea \]

展开：

\[q=a_1e_1+a_2e_2+a_3e_3 \]

也就是说，原来的相对向量可以看作三个 PCA 轴的线性组合。

这两种写法互为逆过程：

\[a=E^Tq \]

表示从空间向量中提取 PCA 坐标。

\[q=Ea \]

表示根据 PCA 坐标重新构造空间向量。

---

7. 对齐坐标轴时真正要求什么

现在希望将 PCA 三个轴分别对齐到目标三个轴：

\[e_1\rightarrow t_1 \]

\[e_2\rightarrow t_2 \]

\[e_3\rightarrow t_3 \]

设对齐旋转矩阵为 \(R\)。

那么需要满足：

\[Re_1=t_1 \]

\[Re_2=t_2 \]

\[Re_3=t_3 \]

合并成矩阵形式：

\[RE=T \]

因为：

\[E^{-1}=E^T \]

所以：

\[R=TE^{-1} \]

即：

\[\boxed{ R=TE^T } \]

这就是坐标轴对齐旋转矩阵的来源。

---

8. 为什么 PCA 局部坐标可以直接用于目标坐标系

PCA 得到的局部坐标为：

\[a= \left[ \begin{array}{c} a_1\\ a_2\\ a_3 \end{array} \right] \]

原始相对向量是：

\[q=a_1e_1+a_2e_2+a_3e_3 \]

现在要将物体的三个 PCA 轴对齐到目标三个轴。

也就是将：

\[e_1\rightarrow t_1 \]

\[e_2\rightarrow t_2 \]

\[e_3\rightarrow t_3 \]

物体发生刚体运动时，点在物体内部的相对位置不能改变。

因此，原来的系数：

\[a_1,\quad a_2,\quad a_3 \]

保持不变，只替换它们对应的基底。

原来的相对向量：

\[a_1e_1+a_2e_2+a_3e_3 \]

变成：

\[a_1t_1+a_2t_2+a_3t_3 \]

写成矩阵形式：

\[q'=Ta \]

因此，PCA 得到的局部坐标可以直接作为目标坐标系中的局部坐标。

但这里的“直接使用”是人为定义的对齐关系：

\[e_1\leftrightarrow t_1 \]

\[e_2\leftrightarrow t_2 \]

\[e_3\leftrightarrow t_3 \]

核心含义是：

\[\boxed{ \text{系数不变，基底发生改变} } \]

即：

\[\underbrace{ a_1e_1+a_2e_2+a_3e_3 }_{\text{物体原姿态}} \quad\longrightarrow\quad \underbrace{ a_1t_1+a_2t_2+a_3t_3 }_{\text{物体目标姿态}} \]

---

9. 为什么这里直接乘 \(T\)

目标坐标系的三个轴按列组成：

\[T= \left[ t_1\ t_2\ t_3 \right] \]

已知目标局部坐标：

\[a= \left[ \begin{array}{c} a_1\\ a_2\\ a_3 \end{array} \right] \]

那么：

\[Ta= \left[ t_1\ t_2\ t_3 \right] \left[ \begin{array}{c} a_1\\ a_2\\ a_3 \end{array} \right] \]

等价于：

\[Ta=a_1t_1+a_2t_2+a_3t_3 \]

所以乘 \(T\) 的作用不是求投影，而是：

根据目标坐标值，使用目标坐标轴重新构造空间向量。

因此：

\[Ta \]

表示：

\[\text{目标坐标} \rightarrow \text{世界空间向量} \]

---

10. 为什么不是乘 \(T^T\)

如果已知世界空间中的一个向量 \(q'\)，希望求它在目标坐标系中的坐标，应使用：

\[a=T^Tq' \]

因为这是将空间向量投影到目标三个轴上。

但是当前问题正好相反。

现在已经知道局部坐标 \(a\)，需要构造目标姿态下的世界空间向量 \(q'\)。

因此要求：

\[T^Tq'=a \]

因为：

\[T^{-1}=T^T \]

所以：

\[q'=Ta \]

两者是互逆过程：

\[\boxed{ a=T^Tq' } \]

表示从世界向量中提取目标坐标。

\[\boxed{ q'=Ta } \]

表示根据目标坐标构造世界向量。

---

11. 为什么还需要加目标原点

公式：

\[q'=Ta \]

得到的是点相对于目标坐标系原点的向量。

它还不是最终世界坐标。

设目标坐标系原点在世界坐标中的位置为 \(o\)。

则最终世界坐标为：

\[p'=q'+o \]

代入：

\[p'=Ta+o \]

其中：

• \(a\)：点在目标坐标系中的局部坐标
• \(Ta\)：点相对于目标原点的世界空间向量
• \(o\)：目标坐标系原点的世界坐标
• \(p'\)：点移动后的最终世界坐标

例如：

\[Ta= \left[ \begin{array}{c} -1\\ 2 \end{array} \right] \]

目标原点为：

\[o= \left[ \begin{array}{c} 10\\ 20 \end{array} \right] \]

则：

\[p'= \left[ \begin{array}{c} -1\\ 2 \end{array} \right] + \left[ \begin{array}{c} 10\\ 20 \end{array} \right] = \left[ \begin{array}{c} 9\\ 22 \end{array} \right] \]

---

12. 完整点云对齐公式

PCA 局部坐标为：

\[a=E^T(p-c) \]

目标姿态下的相对向量为：

\[q'=Ta \]

最终世界坐标为：

\[p'=q'+o \]

代入后：

\[p'=TE^T(p-c)+o \]

因此完整公式为：

\[\boxed{ p'=TE^T(p-c)+o } \]

令：

\[R=TE^T \]

则：

\[\boxed{ p'=R(p-c)+o } \]

---

13. 整个过程分别做了什么

完整公式：

\[p'=TE^T(p-c)+o \]

可以拆成四步。

第一步：减去点云质心

\[p-c \]

得到点相对于物体中心的向量。

第二步：转换为 PCA 局部坐标

\[a=E^T(p-c) \]

得到点沿三个 PCA 轴分别走了多少。

第三步：使用目标坐标轴重新构造向量

\[q'=Ta \]

保持系数 \(a_1,a_2,a_3\) 不变，将原 PCA 基底替换为目标基底。

第四步：加目标原点

\[p'=q'+o \]

将点云放到目标坐标系在世界空间中的实际位置。

---

14. 移动后的实际效果

对齐完成后：

• 点云质心从 \(c\) 移动到 \(o\)
• PCA 第一轴从 \(e_1\) 旋转到 \(t_1\)
• PCA 第二轴从 \(e_2\) 旋转到 \(t_2\)
• PCA 第三轴从 \(e_3\) 旋转到 \(t_3\)
• 点之间的距离不变
• 点云形状和尺寸不变

验证质心：

令：

\[p=c \]

则：

\[p'=TE^T(c-c)+o=o \]

因此原质心会移动到目标原点。

验证第一轴：

\[Re_1=TE^Te_1 \]

因为：

\[E^Te_1= \left[ \begin{array}{c} 1\\ 0\\ 0 \end{array} \right] \]

所以：

\[Re_1= T \left[ \begin{array}{c} 1\\ 0\\ 0 \end{array} \right] =t_1 \]

同理：

\[Re_2=t_2 \]

\[Re_3=t_3 \]

---

15. 坐标表达转换与实际移动点云的区别

这两个问题需要区分。

15.1 只改变坐标表达

已知世界坐标中的点 \(p\)，目标坐标系原点为 \(o\)，轴矩阵为 \(T\)。

点在目标坐标系中的坐标为：

\[a=T^T(p-o) \]

这里空间中的点没有移动，只是使用另一套坐标数字描述。

15.2 实际移动点云

将点云 PCA 坐标系对齐到目标坐标系：

\[p'=TE^T(p-c)+o \]

这里生成了新的世界坐标 \(p'\)。

点云发生了实际旋转和平移。

因此：

\[T^T(p-o) \]

回答的是：

当前点在目标坐标系中的坐标是多少。

而：

\[TE^T(p-c)+o \]

回答的是：

将点云整体移动到目标坐标系后，该点的新世界坐标是多少。

---

16. 平移过程

公式为：

\[p'=R(p-c)+o \]

平移过程可以理解为：

\[p \rightarrow p-c \rightarrow R(p-c) \rightarrow R(p-c)+o \]

即：

1. 所有点减去质心 \(c\)，将点云质心移动到世界原点。
2. 对点云执行旋转。
3. 所有点加上 \(o\)，将质心移动到目标原点。

质心的变化为：

\[c \rightarrow 0 \rightarrow 0 \rightarrow o \]

旋转本身不会移动位于原点的质心。

---

17. 标准刚体变换形式

完整公式：

\[p'=R(p-c)+o \]

展开：

\[p'=Rp-Rc+o \]

整理为：

\[p'=Rp+t \]

其中：

\[t=o-Rc \]

所以：

\[\boxed{ R=TE^T } \]

\[\boxed{ t=o-Rc } \]

标准刚体变换为：

\[\boxed{ p'=Rp+t } \]

---

18. 齐次变换矩阵

三维点的齐次坐标为：

\[\widetilde p= \left[ \begin{array}{c} x\\ y\\ z\\ 1 \end{array} \right] \]

齐次变换矩阵为：

\[H= \left[ \begin{array}{cccc} r_{11} & r_{12} & r_{13} & t_x\\ r_{21} & r_{22} & r_{23} & t_y\\ r_{31} & r_{32} & r_{33} & t_z\\ 0 & 0 & 0 & 1 \end{array} \right] \]

其中：

\[R=TE^T \]

\[t=o-Rc \]

变换写成：

\[\widetilde p'=H\widetilde p \]

---

19. Eigen 实现

#include <Eigen/Core>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>

void alignPointCloud(
    pcl::PointCloud<pcl::PointXYZ>& cloud,
    const Eigen::Vector3d& pcaCentroid,
    const Eigen::Matrix3d& pcaAxes,
    const Eigen::Vector3d& targetOrigin,
    const Eigen::Matrix3d& targetAxes)
{
    const Eigen::Matrix3d rotation =
        targetAxes * pcaAxes.transpose();

    for (auto& point : cloud.points)
    {
        const Eigen::Vector3d source(
            point.x,
            point.y,
            point.z);

        const Eigen::Vector3d local =
            pcaAxes.transpose()
            * (source - pcaCentroid);

        const Eigen::Vector3d transformed =
            targetAxes * local
            + targetOrigin;

        point.x = static_cast<float>(transformed.x());
        point.y = static_cast<float>(transformed.y());
        point.z = static_cast<float>(transformed.z());
    }
}

也可以合并为：

const Eigen::Matrix3d rotation =
    targetAxes * pcaAxes.transpose();

const Eigen::Vector3d transformed =
    rotation * (source - pcaCentroid)
    + targetOrigin;

对应公式：

\[p'=TE^T(p-c)+o \]

---

20. 齐次矩阵实现

Eigen::Matrix3d R =
    targetAxes * pcaAxes.transpose();

Eigen::Vector3d t =
    targetOrigin - R * pcaCentroid;

Eigen::Matrix4d H =
    Eigen::Matrix4d::Identity();

H.block<3, 3>(0, 0) = R;
H.block<3, 1>(0, 3) = t;

变换点：

Eigen::Vector4d pointH(
    point.x,
    point.y,
    point.z,
    1.0);

Eigen::Vector4d transformedH =
    H * pointH;

也可以使用：

Eigen::Affine3d transform =
    Eigen::Affine3d::Identity();

transform.linear() = R;
transform.translation() = t;

Eigen::Vector3d transformed =
    transform * source;

---

21. PCA 轴顺序问题

使用 Eigen：

Eigen::SelfAdjointEigenSolver<Eigen::Matrix3d> solver(covariance);

Eigen 默认：

• 特征值从小到大排列
• 对应特征向量按列存储

因此：

Eigen::Vector3d minAxis =
    solver.eigenvectors().col(0);

Eigen::Vector3d middleAxis =
    solver.eigenvectors().col(1);

Eigen::Vector3d maxAxis =
    solver.eigenvectors().col(2);

构造 PCA 坐标系时，需要明确轴的对应关系。

例如：

Eigen::Matrix3d pcaAxes;

pcaAxes.col(0) = maxAxis;
pcaAxes.col(1) = middleAxis;
pcaAxes.col(2) = minAxis;

表示：

• 最大特征值方向对应 PCA X 轴
• 中间特征值方向对应 PCA Y 轴
• 最小特征值方向对应 PCA Z 轴

只有当对应关系明确时，PCA 局部坐标才能直接用于目标坐标系。

---

22. PCA 轴方向问题

如果 \(e\) 是特征向量，那么 \(-e\) 也是特征向量。

因此 PCA 只能确定轴线，不能自动确定轴的正方向。

实际使用时，需要根据外部条件统一方向，例如：

• 相机视点
• 重力方向
• 已知物体朝向
• 指定的参考方向

例如：

if (pcaAxes.col(0).dot(referenceDirection) < 0.0)
    pcaAxes.col(0) *= -1.0;

还需要保证右手坐标系：

if (pcaAxes.determinant() < 0.0)
    pcaAxes.col(2) *= -1.0;

目标坐标系也应满足右手系条件。

---

23. 结果验证

23.1 验证质心

转换后的点云质心应接近：

\[o \]

23.2 验证轴对齐

应满足：

\[Re_1\approx t_1 \]

\[Re_2\approx t_2 \]

\[Re_3\approx t_3 \]

23.3 验证旋转矩阵

应满足：

\[R^TR\approx I \]

以及：

\[\det(R)\approx1 \]

Eigen 检查：

const double orthogonalError =
    (R.transpose() * R
     - Eigen::Matrix3d::Identity()).norm();

const double determinant =
    R.determinant();

---

24. 核心结论

点在 PCA 坐标系中的局部坐标：

\[a=E^T(p-c) \]

原来的相对向量：

\[p-c=Ea \]

将 PCA 轴替换为目标轴，并保持坐标系数不变：

\[q'=Ta \]

移动到目标原点：

\[p'=Ta+o \]

代入后：

\[\boxed{ p'=TE^T(p-c)+o } \]

旋转矩阵：

\[\boxed{ R=TE^T } \]

平移向量：

\[\boxed{ t=o-Rc } \]

标准刚体变换：

\[\boxed{ p'=Rp+t } \]

最简理解：

PCA 首先求出点在物体自身基底下的坐标。对齐时保持这些坐标系数不变，将原 PCA 基底替换为目标基底，最后加上目标原点，得到移动后的世界坐标。