cv.ppf_match_3d_PPF3DDetector.trainModel() fails with valid float32 input in Python bindings

[YukiSato-cmd]

I'm encountering an issue with the cv.ppf_match_3d_PPF3DDetector.trainModel() function when using the Python API in OpenCV 4.8.0.76. The method raises an assertion error even though the input NumPy array has the correct shape and data type.

    detector.trainModel(model_pc)
cv2.error: OpenCV(4.12.0) /io/opencv_contrib/modules/surface_matching/src/ppf_match_3d.cpp:205: error: (-215:Assertion failed) PC.type() == CV_32F || PC.type() == CV_32FC1 in function 'trainModel'

opencv-contrib-python     4.12.0.88
opencv-python             4.12.0.88
opencv-python-headless    4.12.0.88

import numpy as np
import open3d as o3d
import cv2 as cv
import os

def load_pointcloud_as_numpy(path, voxel_size=0.005):
    if path.endswith(".ply"):
        pcd = o3d.io.read_point_cloud(path)
        if not pcd.has_points():
            raise RuntimeError("PLY file contains no points.")
        pcd.estimate_normals()
        pcd = pcd.voxel_down_sample(voxel_size)
        pts = np.asarray(pcd.points, dtype=np.float32)
        normals = np.asarray(pcd.normals, dtype=np.float32)
        return np.hstack((pts, normals))
    else:
        mesh = o3d.io.read_triangle_mesh(path)
        if not mesh.has_triangles():
            raise RuntimeError("Input mesh has no triangles.")
        mesh.compute_vertex_normals()
        pcd = mesh.sample_points_poisson_disk(5000)
        pcd = pcd.voxel_down_sample(voxel_size)
        pts = np.asarray(pcd.points, dtype=np.float32)
        normals = np.asarray(pcd.normals, dtype=np.float32)
        return np.hstack((pts, normals))

def convert_to_ppf_format(pcd_np):
    pcd_np = np.asarray(pcd_np, dtype=np.float32)
    if pcd_np.ndim != 2 or pcd_np.shape[1] != 6:
        raise ValueError(f"Input must be of shape (N, 6), but got {pcd_np.shape}")
    pcd_cv = pcd_np.reshape((-1, 1, 6)).astype(np.float32)

    print(f"[DEBUG] PPF formatted input shape: {pcd_cv.shape}")
    print(f"[DEBUG] PPF formatted dtype: {pcd_cv.dtype}")
    return pcd_cv

def run_ppf_icp(source_np, target_np):
    model_pc = convert_to_ppf_format(source_np)
    scene_pc = convert_to_ppf_format(target_np)
    
    detector = cv.ppf_match_3d_PPF3DDetector(0.025, 0.05)
    print("Training PPF detector...")
    detector.trainModel(model_pc)

    print("Matching...")
    results = detector.match(scene_pc, 0.05, 0.05)
    print(f"Found {len(results)} pose(s)")

    if len(results) == 0:
        raise RuntimeError("No poses found.")
    pose = results[0].pose
    print("Initial pose from PPF:\n", pose)

    icp = cv.ppf_match_3d_ICP(100, 0.005, 2.5, 5)
    refined_pose = icp.registerModelToScene(model_pc, scene_pc, pose)
    print("Refined pose (ICP):\n", refined_pose.pose)
    return refined_pose.pose

def apply_transformation_and_save(source_np, target_np, transformation):
    trans = np.asarray(transformation, dtype=np.float64)

    source_pts = source_np[:, :3]
    target_pts = target_np[:, :3]

    ones = np.ones((source_pts.shape[0], 1))
    homog = np.hstack((source_pts, ones))
    transformed = (trans @ homog.T).T[:, :3]

    source_pcd = o3d.geometry.PointCloud()
    source_pcd.points = o3d.utility.Vector3dVector(transformed)
    source_pcd.paint_uniform_color([1, 0, 0])

    target_pcd = o3d.geometry.PointCloud()
    target_pcd.points = o3d.utility.Vector3dVector(target_pts)
    target_pcd.paint_uniform_color([0, 1, 0])

    o3d.io.write_point_cloud("ppf_aligned_source.ply", source_pcd)
    o3d.io.write_point_cloud("ppf_target.ply", target_pcd)
    print("Saved aligned results as PLY.")

if __name__ == "__main__":
    source_path = "source.stl"
    target_path = "scene.ply"

    print("Loading point clouds...")
    source_np = load_pointcloud_as_numpy(source_path)
    target_np = load_pointcloud_as_numpy(target_path)

    print("Running PPF + ICP...")
    final_pose = run_ppf_icp(source_np, target_np)

    print("Applying transformation and saving result...")
    apply_transformation_and_save(source_np, target_np, final_pose)