neka-nat/cupoch

icp TransformationEstimationPointToPlane yields different result compare to open3d

funnyvalentine2363 opened this issue · 0 comments

import cupoch as cph

target = cph.io.read_point_cloud("PointCloud_1.ply")
source = cph.io.read_point_cloud("PointCloud_2.ply")

import numpy as np
source.estimate_normals(cph.geometry.KDTreeSearchParamRadius(radius=1 * 2, max_nn=30))
target.estimate_normals(cph.geometry.KDTreeSearchParamRadius(radius=1 * 2, max_nn=30))
reg_p2p = cph.registration.registration_icp(
    source, target, 0.15,np.identity(4).astype(np.float32),
    estimation_method = cph.registration.TransformationEstimationPointToPlane())
print(reg_p2p.transformation,reg_p2p.inlier_rmse,reg_p2p.fitness)

result:

[warning] check_det failed, empty vector will be returned
[[ 9.9999994e-01 -2.3211574e-04 3.1052838e-04 -5.6119312e-02]
[ 2.3210575e-04 1.0000000e+00 4.1026891e-05 1.5950035e-01]
[-3.1053583e-04 -4.0970357e-05 9.9999994e-01 1.3352202e-01]
[ 0.0000000e+00 0.0000000e+00 0.0000000e+00 1.0000000e+00]] 0.11659056693315506 0.5279329419136047

def icp_p2plane(source,target,distance):
    source.estimate_normals(o3d.geometry.KDTreeSearchParamHybrid(radius=1*2,max_nn=30))
    target.estimate_normals(o3d.geometry.KDTreeSearchParamHybrid(radius=1*2,max_nn=30))
    reg_p2p = o3d.pipelines.registration.registration_icp(
        source, target, distance,init = np.identity(4).astype(np.float32),
        estimation_method = o3d.pipelines.registration.TransformationEstimationPointToPlane())
    return reg_p2p

target = o3d.io.read_point_cloud("PointCloud_1.ply")
source = o3d.io.read_point_cloud("PointCloud_2.ply")
reg_p2p_cpu = icp_p2plane(source,target,0.15)
print(reg_p2p_cpu.transformation,reg_p2p_cpu.inlier_rmse,reg_p2p_cpu.fitness)

result:
[[ 9.99999876e-01 -1.51703967e-04 4.75125722e-04 -3.06633247e-02]
[ 1.51902093e-04 9.99999902e-01 -4.16987856e-04 5.89422629e-01]
[-4.75062417e-04 4.17059977e-04 9.99999800e-01 2.32936885e-01]
[ 0.00000000e+00 0.00000000e+00 0.00000000e+00 1.00000000e+00]] 0.0851198294781214 0.8659489299658403

PointCloud_2.ply.zip
PointCloud_1.ply.zip

edit:
if i simply use source.estimate_normals(), the results are also different
it seems like during icp process, some condition were not considered and then the program breaks and throw halfway output. really appreciate your help.