写在前面
20250423 想给GT点云增加噪声并且随机旋转，然后用PCA方向进行提取三个坐标，最后旋转到一样的形状，有时成功有时失败，代码放在这里，mark一下：https://paste.org.cn/lYYK49ZwIn
最后用SVD分解解决了...
1. 点云PCA提取主方向
主要用于点云对齐，输入：点云；输出：三个正交方向
def get_principle_dirs(pts):
pts_pca = PCA(n_components=3)
pts_pca.fit(pts)
principle_dirs = pts_pca.components_
principle_dirs /= np.linalg.norm(principle_dirs, 2, axis=0)
return principle_dirs2. 点云对齐到标准坐标系
输入：点云
输出：转换后的点云以及逆旋转矩阵
def pca_alignment(pts, random_flag=False):
pca_dirs = get_principle_dirs(pts) # 获得主方向（特征向量）
if random_flag:
pca_dirs *= np.random.choice([-1, 1], 1)
rotate_1 = compute_roatation_matrix(pca_dirs[2], [0, 0, 1], pca_dirs[1])
pca_dirs = np.array(rotate_1 * pca_dirs.T).T
rotate_2 = compute_roatation_matrix(pca_dirs[1], [1, 0, 0], pca_dirs[2])
pts = np.array(rotate_2 * rotate_1 * np.matrix(pts.T)).T
inv_rotation = np.array(np.linalg.inv(rotate_2 * rotate_1)) # 逆旋转矩阵
return pts, inv_rotation3. 对齐旋转
这里还有两个需要用到的，主要是计算sour到dest的旋转矩阵，用于构造对齐变换以及绕轴theta弧度进行旋转
def compute_roatation_matrix(sour_vec, dest_vec, sour_vertical_vec=None):
# http://immersivemath.com/forum/question/rotation-matrix-from-one-vector-to-another/
if np.linalg.norm(np.cross(sour_vec, dest_vec), 2) 0 or np.abs(np.dot(sour_vec, dest_vec)) >= 1.0:
if np.dot(sour_vec, dest_vec) < 0:
return rotation_matrix(sour_vertical_vec, np.pi)
return np.identity(3)
alpha = np.arccos(np.dot(sour_vec, dest_vec))
a = np.cross(sour_vec, dest_vec) / np.linalg.norm(np.cross(sour_vec, dest_vec), 2)
c = np.cos(alpha)
s = np.sin(alpha)
R1 = [a[0] * a[0] * (1.0 - c) + c,
a[0] * a[1] * (1.0 - c) - s * a[2],
a[0] * a[2] * (1.0 - c) + s * a[1]]
R2 = [a[0] * a[1] * (1.0 - c) + s * a[2],
a[1] * a[1] * (1.0 - c) + c,
a[1] * a[2] * (1.0 - c) - s * a[0]]
R3 = [a[0] * a[2] * (1.0 - c) - s * a[1],
a[1] * a[2] * (1.0 - c) + s * a[0],
a[2] * a[2] * (1.0 - c) + c]
R = np.matrix([R1, R2, R3])
return R
def rotation_matrix(axis, theta):
# Return the rotation matrix associated with counterclockwise rotation about the given axis by theta radians.
axis = np.asarray(axis)
axis = axis / math.sqrt(np.dot(axis, axis))
a = math.cos(theta / 2.0)
b, c, d = -axis * math.sin(theta / 2.0)
aa, bb, cc, dd = a * a, b * b, c * c, d * d
bc, ad, ac, ab, bd, cd = b * c, a * d, a * c, a * b, b * d, c * d
return np.matrix(np.array([[aa + bb - cc - dd, 2 * (bc + ad), 2 * (bd - ac)],
[2 * (bc - ad), aa + cc - bb - dd, 2 * (cd + ab)],
[2 * (bd + ac), 2 * (cd - ab), aa + dd - bb - cc]]))4. 基于 SVD 的点云刚性对齐（Procrustes 分解）
在点云配准（registration）中，我们经常需要将“源”点云（source）刚性地对齐到“目标”点云（target），保证旋转和平移后两者重合。下面介绍一种经典且精确的 Procrustes 分解（或称 SVD 对齐）方法。
假设有两个点云：
- GT（ground-truth，目标点云）记作
\mathbf{X} = \{\,\mathbf{x}_i\in\mathbb{R}^3\}_{i=1}^N， - Noisy（源点云）记作
\mathbf{Y} = \{\,\mathbf{y}_i\in\mathbb{R}^3\}_{i=1}^N，
两者之间点的索引一一对应。
-
计算质心
\mathbf{\bar x} = \frac{1}{N}\sum_{i=1}^N \mathbf{x}_i,\quad \mathbf{\bar y} = \frac{1}{N}\sum_{i=1}^N \mathbf{y}_i. -
中心化
\mathbf{\tilde X} = [\,\mathbf{x}_1 - \mathbf{\bar x},\ \mathbf{x}_2 - \mathbf{\bar x},\ \dots\,],\quad \mathbf{\tilde Y} = [\,\mathbf{y}_1 - \mathbf{\bar y},\ \mathbf{y}_2 - \mathbf{\bar y},\ \dots\,]. -
构造协方差矩阵
\mathbf{H} = \mathbf{\tilde Y}^\top \,\mathbf{\tilde X}\quad \bigl(3\times 3\bigr). -
SVD 分解
\mathbf{H} = \mathbf{U}\,\boldsymbol\Sigma\,\mathbf{V}^\top. -
计算旋转矩阵
\mathbf{R} = \mathbf{V}\,\mathbf{U}^\top.若
\det(\mathbf{R})<0，说明出现了反射分量，需要把\mathbf{V}的最后一列取反后重算：\mathbf{V}[:,2]\;\leftarrow\;-\,\mathbf{V}[:,2],\quad \mathbf{R} \;=\; \mathbf{V}\,\mathbf{U}^\top. -
计算平移向量
\mathbf{t} = \mathbf{\bar x} - \mathbf{R}\,\mathbf{\bar y}. -
对齐变换
对源点云中的每个点\mathbf{y}_i施加刚性变换：\mathbf{y}_i' = \mathbf{R}\,\mathbf{y}_i + \mathbf{t}, \quad i = 1,\dots,N.
Python 代码示例
import numpy as np
def align_via_svd(src_pts, dst_pts):
# 1. 质心
c_src = src_pts.mean(axis=0)
c_dst = dst_pts.mean(axis=0)
# 2. 中心化
S = src_pts - c_src
D = dst_pts - c_dst
# 3. 协方差矩阵
H = S.T @ D
# 4. SVD
U, _, Vt = np.linalg.svd(H)
# 5. 旋转矩阵
R = Vt.T @ U.T
if np.linalg.det(R) < 0:
Vt[2, :] *= -1
R = Vt.T @ U.T
# 6. 平移向量
t = c_dst - R @ c_src
# 7. 应用变换
aligned = (R @ src_pts.T).T + t
return aligned, R, t