Skip to content

icp TransformationEstimationPointToPlane yields different result compare to open3d #107

@funnyvalentine2363

Description

@funnyvalentine2363
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.

Metadata

Metadata

Assignees

No one assigned

    Labels

    No labels
    No labels

    Projects

    No projects

    Milestone

    No milestone

    Relationships

    None yet

    Development

    No branches or pull requests

    Issue actions