open3d line two points

最新推荐文章于 2024-07-19 00:00:00 发布

zmx2008313

最新推荐文章于 2024-07-19 00:00:00 发布

阅读量300

点赞数 3

文章标签： python

本文链接：https://blog.csdn.net/zmx2008313/article/details/138318758

版权

本文介绍了如何使用Open3D库中的ICP（IterativeClosestPoint）算法对两个点云进行配准，通过提供示例代码展示了从初始位姿开始，计算并应用变换矩阵，最终可视化配准结果的过程。

摘要由CSDN通过智能技术生成

import random

import open3d as o3d
import numpy as np

# 获取示例数据
source_cloud = o3d.io.read_point_cloud("output_point_cloud_with_color1.ply")
target_cloud = o3d.io.read_point_cloud("output_point_cloud_with_color2.ply")
source_cloud.paint_uniform_color([1, 0.706, 0])
target_cloud.paint_uniform_color([0, 0.651, 0.929])
threshold = 0.02  # RMSE残差阈值，小于该残差阈值，迭代终止

# 初始位姿
trans_init = np.asarray([[0.862, 0.011, -0.507, 0.5],
                         [-0.139, 0.967, -0.215, 0.7],
                         [0.487, 0.255, 0.835, -1.4], [0.0, 0.0, 0.0, 1.0]])

# 显示未配准点云
o3d.visualization.draw_geometries([source_cloud, target_cloud],
                                  zoom=0.4459,
                                  front=[0.9288, -0.2951, -0.2242],
                                  lookat=[1.6784, 2.0612, 1.4451],
                                  up=[-0.3402, -0.9189, -0.1996])

# 点到点的ICP
result = o3d.pipelines.registration.registration_icp(
    source_cloud, target_cloud, threshold, trans_init,
    o3d.pipelines.registration.TransformationEstimationPointToPoint())
print(result)
print("Transformation is:")
print(result.transformation)

# 显示点到点的配准结果
source_cloud.transform(result.transformation)

print("0:",np.asarray(source_cloud.points))
print("1:",np.asarray(target_cloud.points))
points_0=np.asarray(source_cloud.points)
points_1=np.asarray(target_cloud.points)
print("shape:",points_0.shape)
print("shape1:",points_1.shape)


line_set = o3d.geometry.LineSet()
start = points_0
normal_scale=0.005
end = points_1#points_0 + (np.asarray(points_0) * normal_scale)
points = np.concatenate((start, end))
line_set.points = o3d.utility.Vector3dVector(points)
size = len(start)

color=[0, 1, 0]
line_set.lines = o3d.utility.Vector2iVector(np.asarray([[i, i + size] for i in range(0, size)]))
color=[random.uniform(0,1),random.uniform(0,1),random.uniform(0,1)]
line_set.paint_uniform_color(color)


#line_set.colors = o3d.utility.Vector3dVector(colors)
o3d.visualization.draw_geometries([source_cloud, target_cloud,line_set])

o3d.visualization.draw_geometries([source_cloud, target_cloud],
                                  zoom=0.4459,
                                  front=[0.9288, -0.2951, -0.2242],
                                  lookat=[1.6784, 2.0612, 1.4451],
                                  up=[-0.3402, -0.9189, -0.1996])