def getPointCloud(pred):
center = np.array(pred.center)
# print("original centers", center)
mu=0
sigma=0.1
pointcloud=[]
for id, c in enumerate(center):
c = np.array(c)
if c[0] > 0 and pred.confidence[id] > 0:
# for ele in pred.boxes[id]:
# ele[2]=0
# pointcloud.append(ele)
num_points = int(pred.confidence[id]*100)
centers = np.repeat(c.reshape(1,3),num_points,axis=0)
centers += np.random.normal(mu, sigma, size=(num_points,3))
for ele in centers:
ele[2]=0
pointcloud.append(ele)
pointcloud=np.array(pointcloud)
print("new pointcloud", pointcloud)
return pointcloud
if __name__ == '__main__':
pcd_veh = getPointCloud(pred_veh)
pcd_inf = getPointCloud(pred_inf)
source = o3d.geometry.PointCloud()
source.points = o3d.utility.Vector3dVector(np.asarray(pcd_inf))
target = o3d.geometry.PointCloud()
target.points = o3d.utility.Vector3dVector(np.asarray(pcd_veh))
o3d.visualization.draw_geometries([source,target],
zoom=0.455,
front=[-0.4999, -0.1659, -0.8499],
lookat=[2.1813, 2.0619, 2.0999],
up=[0.1204, -0.9852, 0.1215])
trans_init=np.eye(4)
threshold = 100.0
reg_p2p = o3d.pipelines.registration.registration_icp(
source, target, threshold, trans_init,
o3d.pipelines.registration.TransformationEstimationPointToPoint())
print(reg_p2p)
print("Transformation is:")
print(reg_p2p.transformation)
source.transform(reg_p2p.transformation)
o3d.visualization.draw_geometries([source,target],
zoom=0.455,
front=[-0.4999, -0.1659, -0.8499],
lookat=[2.1813, 2.0619, 2.0999],
up=[0.1204, -0.9852, 0.1215])随机噪声 icp匹配