修改下面的代码,要求实现如下效果:
1.将矩形和矩形中点改成长方体和三维点
2.将split_by_angle的角度判断改为在x,y平面的角度判定,z不限
import math
import numpy as np
import matplotlib.pyplot as plt
import matplotlib
matplotlib.rc("font", family='YouYuan')
EPS = 0.0001
MAX_ITER = 100
show_animation = False
# ===== ICP 相关函数 =====
def icp_matching(previous_points, current_points):
"""
Iterative Closest Point matching
"""
H = None
dError = np.inf
preError = np.inf
count = 0
while dError >= EPS:
count += 1
indexes, error = nearest_neighbor_association(previous_points, current_points)
Rt, Tt = svd_motion_estimation(previous_points[:, indexes], current_points)
# 更新当前点
current_points = (Rt @ current_points) + Tt[:, np.newaxis]
dError = preError - error
if dError < 0:
break
preError = error
H = update_homogeneous_matrix(H, Rt, Tt)
if dError <= EPS or MAX_ITER <= count:
break
R = np.array(H[0:-1, 0:-1])
T = np.array(H[0:-1, -1])
return R, T
def update_homogeneous_matrix(Hin, R, T):
r_size = R.shape[0]
H = np.zeros((r_size + 1, r_size + 1))
H[0:r_size, 0:r_size] = R
H[0:r_size, r_size] = T
H[r_size, r_size] = 1.0
if Hin is None:
return H
else:
return Hin @ H
def nearest_neighbor_association(previous_points, current_points):
"""
找到 current_points 中每个点在 previous_points 中的最近邻索引。
"""
n_prev = previous_points.shape[1]
n_curr = current_points.shape[1]
d = np.zeros((n_curr, n_prev))
for i in range(n_curr):
diff = previous_points - current_points[:, i:i+1]
d[i, :] = np.linalg.norm(diff, axis=0)
indexes = np.argmin(d, axis=1)
error = np.mean(np.min(d, axis=1))
return indexes, error
def svd_motion_estimation(previous_points, current_points):
pm = np.mean(previous_points, axis=1)
cm = np.mean(current_points, axis=1)
p_shift = previous_points - pm[:, np.newaxis]
c_shift = current_points - cm[:, np.newaxis]
W = c_shift @ p_shift.T
u, s, vh = np.linalg.svd(W)
R = (u @ vh).T
t = pm - (R @ cm)
return R, t
def loop_closure(R_list, T_list, R_total, T_total, a, k, threshold=0.1):
"""
回环估计:对于时刻 a 和时刻 a+k, 计算 ICP 变换,并更新回环变换。
"""
if a + k >= len(R_list): # 如果 a+k 时刻不存在,跳过
return R_total, T_total
A_prev = R_list[a]
A_curr = R_list[a + k]
# 使用 ICP 得到 A_a 和 A_(a+k) 之间的相对变换矩阵 R_a,a+k 和 T_a,a+k
R_a_k, T_a_k = icp_matching(A_prev, A_curr)
# 计算新的全局变换:R = R_a,a+k @ R_a 和 T = R_a,a+k @ T_a + T_a,a+k
R_new = R_a_k @ R_total[a]
T_new = R_a_k @ T_total[a] + T_a_k
# 比较新旧变换,计算误差
error = np.linalg.norm(R_new - R_total[a]) + np.linalg.norm(T_new - T_total[a])
if error < threshold:
# 如果误差足够小,执行调整,优化变换
R_total[a + k] = R_new
T_total[a + k] = T_new
print(f"回环检测成功,优化变换:R_new=\n{R_new}, T_new={T_new}")
else:
print(f"回环检测失败,误差过大:误差={error}")
return R_total, T_total
def interpolate(x_1, x_2, delta):
"""
插值函数:对两个坐标点进行线性插值
"""
return (1.0 - delta) * x_1 + delta * x_2
def generate_rectangle_points(length=20.0, width=10.0, x=0.0, y=0.0, theta=0,fen=0.005):
"""生成矩形边界点云并加噪声"""
k=np.arctan2(width,length)
t=np.sqrt((length/2)**2+(width/2)**2)
top = np.array([x + t*np.cos(theta+k), y + t*np.sin(theta + k)])
bottom = np.array([x + t*np.cos(theta-k), y +t*np.sin(theta -k)])
left = np.array([x + t*np.cos(theta+k-np.pi), y +t*np.sin(theta+k-np.pi)])
right = np.array([x + t*np.cos(theta+np.pi-k), y +t*np.sin(theta +np.pi-k)])
points = [top, bottom, left, right]
len_points = len(points)
DELTA_LIST = np.arange(0.0, 1.0, fen)
for i in range(len_points - 1):
for delta in DELTA_LIST:
points.append(interpolate(points[i], points[i+1], delta))
for delta in DELTA_LIST:
points.append(interpolate(points[len_points-1], points[0], delta))
x_coords = [point[0] for point in points]
y_coords = [point[1] for point in points]
points= [x_coords, y_coords]
print(points)
return np.array(points)
def split_by_angle(points, center=np.array([0, 0]), start=-30, step=8):
"""按角度扇区划分点云"""
sectors = []
angles = np.degrees(np.arctan2(points[1, :] - center[1],
points[0, :] - center[0]))
angles = (angles + 360) % 360
t=int(360/step)
for k in range(t):
theta1 = (start + step*np.random.uniform(0,2)) % 360
theta2 = (theta1 + 60) % 360 # 每个扇区宽度为60°
start=theta1
if theta1 < theta2:
mask = (angles >= theta1) & (angles <= theta2)
else: # 角度跨越360°边界
mask = (angles >= theta1) | (angles <= theta2)
Ak = points[:, mask]
sectors.append(Ak)
return sectors
def merge_point_clouds(A_list, R_list, T_list):
"""将所有点云A_k变换到A1坐标系并融合"""
merged = [A_list[0]]
R_total = [np.eye(2)]
T_total = [np.zeros(2)]
# 累积每个点云的全局变换
for k in range(1, len(A_list)):
Rk, Tk = R_list[k - 1], T_list[k - 1]
R_acc = R_total[-1] @ Rk
T_acc = R_total[-1] @ T_list[k - 1] + T_total[-1]
R_total.append(R_acc)
T_total.append(T_acc)
Ak_global = R_acc @ A_list[k] + T_acc[:, None]
merged.append(Ak_global)
merged_points = np.hstack(merged)
merged_points = np.unique(np.round(merged_points, 2), axis=1)
return merged_points, R_total, T_total
# ===== 主逻辑 =====
def main():
print("开始运行改造后的 ICP 例子...")
# Step 1: 生成矩形点云
rect_points = generate_rectangle_points()
rect_points2 = generate_rectangle_points(length=5, width=4,)
rect_points3 = generate_rectangle_points(length=4,width=3,x=7,y=3)
rect_points= np.hstack((rect_points,rect_points2,rect_points3))
print(rect_points)
steps=0.2
# Step 2: 按角度扇区切分
sectors = split_by_angle(rect_points,center=np.array([0, 0]), step=steps)
mk=int(360/steps)
t=720
R_list, T_list = [], []
used_sectors = []
# Step 3: 对相邻扇区进行 ICP
merged = [sectors[0]]
for k in range(1, len(sectors)):
A_prev = sectors[k ]
A_curr = sectors[k-1]
if A_prev.shape[1] < 5 or A_curr.shape[1] < 5:
continue
R, T = icp_matching(A_prev, A_curr)
R_list.append(R)
T_list.append(T)
used_sectors.append(A_curr)
print(f"ICP {k}: R=\n{R}\nT={T}")
# Step 4: 回环估计和全局优化
R_total = [np.eye(2)]
T_total = [np.zeros(2)]
# 累积每个点云的全局变换
for k in range(1, len(R_list)):
Rk, Tk = R_list[k - 1], T_list[k - 1]
R_acc = R_total[-1] @ Rk
T_acc = R_total[-1] @ T_list[k - 1] + T_total[-1]
R_total.append(R_acc)
T_total.append(T_acc)
for a in range(mk): # 假设有 10 个时刻 # 比如从 a+1 到 a+2
if a + t < len(R_list):
R_total, T_total = loop_closure(R_list, T_list, R_total, T_total, a, t)
for k in range(1, len(R_list)):
Ak_global = R_total[k] @ sectors[k] + T_total[k][:, None]
merged.append(Ak_global)
# Step 5: 融合所有点云到A1坐标系
merged_points = np.hstack(merged)
merged_points = np.unique(np.round(merged_points, 2), axis=1)
# Step 6: 可视化结果
plt.figure()
plt.scatter(rect_points[0, :], rect_points[1, :], s=3, label="原始矩形点云")
plt.scatter(merged_points[0, :], merged_points[1, :], s=3, label="ICP融合点云")
plt.legend()
plt.axis("equal")
plt.title("ICP融合结果对比")
plt.show()
if __name__ == "__main__":
main()
最新发布