123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108 |
"""演示3: 三维快速建模 — SfM/多视图三角化 + ICP 配准.
1. 左: 多视图三角化重建稀疏点云 (复用双视图 DLT 原理, 扩展到多视图)
2. 右: ICP 点云配准 — 两片点云对齐成完整模型
"""
import os
import numpy as np
import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import Axes3D
from sklearn.neighbors import NearestNeighbors
from figure_style import figures_dir, configure
configure()
OUT = figures_dir()
os.makedirs(OUT, exist_ok=True)
# ============ 左: 多视图三角化 ============
rng = np.random.default_rng(42)
true = rng.uniform(-1, 1, (30, 3)) # 真实 3D 点
K = np.array([[500, 0, 250], [0, 500, 250], [0, 0, 1]])
def cam(R, t):
return K @ np.hstack([R, t.reshape(-1, 1)])
views = []
for ang in np.linspace(0, np.pi, 4): # 4 个视角
R = np.array([[np.cos(ang), 0, np.sin(ang)],
[0, 1, 0],
[-np.sin(ang), 0, np.cos(ang)]])
views.append(cam(R, np.array([0.3*np.sin(ang), 0, 0])))
def project(P, pts):
ph = (P @ np.hstack([pts, np.ones((len(pts), 1))]).T).T
return ph[:, :2] / ph[:, 2:3]
# 多视图 DLT 三角化
uvs = [project(P, true) + rng.normal(0, 0.2, (30, 2)) for P in views]
rec = []
for k in range(len(true)):
A = []
for P, uv in zip(views, uvs):
u, v = uv[k]
A.append(u * P[2] - P[0]); A.append(v * P[2] - P[1])
A = np.array(A)
_, _, Vt = np.linalg.svd(A)
X = Vt[-1]; rec.append(X[:3] / X[3])
rec = np.array(rec)
err = np.linalg.norm(rec - true, axis=1).mean()
print(f"4视图三角化重建: 平均误差 {err:.4f}")
# ============ 右: ICP 配准 ============
def icp(src, tgt, iters=50):
"""经典 ICP: 最近邻对应 + 最小二乘刚体变换."""
nn = NearestNeighbors(n_neighbors=1).fit(tgt)
T = np.eye(4); src_cur = src.copy()
for _ in range(iters):
_, idx = nn.kneighbors(src_cur)
corr = tgt[idx[:, 0]]
# Kabsch
cs, ct = src_cur.mean(0), corr.mean(0)
H = (src_cur - cs).T @ (corr - ct)
U, S, Vt = np.linalg.svd(H)
R = Vt.T @ U.T
if np.linalg.det(R) < 0: R[:, -1] *= -1
t = ct - R @ cs
src_cur = (R @ src_cur.T).T + t
T[:3, :3] = R @ T[:3, :3]; T[:3, 3] = t + R @ T[:3, 3]
return src_cur, T
# 两片部分点云 (同一物体两个视角)
base = rng.uniform(-1, 1, (80, 3))
Rgt = np.array([[np.cos(0.5), -np.sin(0.5), 0], [np.sin(0.5), np.cos(0.5), 0], [0, 0, 1]])
tgt = (Rgt @ base.T).T + np.array([0.5, 0.2, 0])
src = base[:60] + rng.normal(0, 0.02, (60, 3)) # 重叠部分
aligned, _ = icp(src, tgt)
print(f"ICP 配准: 对齐前后到目标最近距离均值 "
f"{np.mean(np.min(np.linalg.norm(src[:, None]-tgt[None], 2, -1), 1)):.3f} → "
f"{np.mean(np.min(np.linalg.norm(aligned[:, None]-tgt[None], 2, -1), 1)):.3f}")
# ============ 可视化 ============
fig = plt.figure(figsize=(14, 5.5))
ax = fig.add_subplot(1, 3, 1, projection='3d')
ax.scatter(true[:, 0], true[:, 1], true[:, 2], c='tab:red', s=10, label='真实')
ax.scatter(rec[:, 0], rec[:, 1], rec[:, 2], c='tab:blue', s=8, alpha=0.7, label='重建')
ax.set_title(f'多视图三角化重建\n(4视图, 误差{err:.3f})'); ax.legend(fontsize=8)
ax = fig.add_subplot(1, 3, 2, projection='3d')
ax.scatter(src[:, 0], src[:, 1], src[:, 2], c='tab:green', s=6, label='源点云')
ax.scatter(tgt[:, 0], tgt[:, 1], tgt[:, 2], c='tab:gray', s=6, alpha=0.6, label='目标点云')
ax.set_title('ICP 前: 两片点云错位'); ax.legend(fontsize=8)
ax = fig.add_subplot(1, 3, 3, projection='3d')
ax.scatter(aligned[:, 0], aligned[:, 1], aligned[:, 2], c='tab:green', s=6, label='配准后源')
ax.scatter(tgt[:, 0], tgt[:, 1], tgt[:, 2], c='tab:gray', s=6, alpha=0.6, label='目标')
ax.set_title('ICP 后: 对齐'); ax.legend(fontsize=8)
for ax in fig.axes:
ax.set_xlabel('X'); ax.set_ylabel('Y'); ax.set_zlabel('Z')
ax.set_xticks([]); ax.set_yticks([]); ax.set_zticks([])
fig.suptitle('三维快速建模: 多视图重建 + ICP 配准 (经典几何方法)', fontsize=14)
fig.tight_layout(rect=(0, 0, 1, 0.92))
out = os.path.join(OUT, 'fig3d_reconstruct.png')
fig.savefig(out, dpi=150)
print(f"-> {out}")
|
