I'm trying a MVP sample that aligns a 2D Mask that is generating by converting it to point cloud using the depth map generated by ZED Stereo camera.

I'm trying to align a 3D Object file to that point cloud using C# and Unity but for now I'm testing it using python. The 3D Model and the PCD file is here They are size of 2MB~ I want to align this

with

This is the result, and here is my attempt

The mask is generated from that scene

Here is my attempt:

import open3d as o3d
import numpy as np

def load_point_clouds(obj_file, pcd_file, num_points=10000):
    source = o3d.io.read_triangle_mesh(obj_file)
    source_pcd = source.sample_points_uniformly(number_of_points=num_points)
    target = o3d.io.read_point_cloud(pcd_file)
    return source_pcd, target

def filter_points_above_level(pcd, z_level):
    points = np.asarray(pcd.points)
    filtered_points = points[points[:, 1] <= z_level]  # Filter based on z-axis
    filtered_pcd = o3d.geometry.PointCloud()
    filtered_pcd.points = o3d.utility.Vector3dVector(filtered_points)
    return filtered_pcd

def find_dense_region(pcd, voxel_size=0.05):
    # Get points array
    points = np.asarray(pcd.points)
    
    # Find bounding box
    min_point = np.min(points, axis=0)
    max_point = np.max(points, axis=0)
    
    # Create voxel grid dimensions
    dims = ((max_point - min_point) / voxel_size).astype(int) + 1
    voxel_grid = np.zeros(dims)
    
    # Assign points to voxels
    indices = ((points - min_point) / voxel_size).astype(int)
    for idx in indices:
        voxel_grid[tuple(idx)] += 1
        
    # Find densest region
    dense_mask = voxel_grid > np.mean(voxel_grid[voxel_grid > 0])
    if not np.any(dense_mask):
        return None, None
        
    dense_indices = np.argwhere(dense_mask)
    dense_min = dense_indices.min(axis=0) * voxel_size + min_point
    dense_max = dense_indices.max(axis=0) * voxel_size + min_point
    
    return dense_min, dense_max

def extract_and_align(source, target, min_bound, max_bound):
    # Extract region
    target_points = np.asarray(target.points)
    mask = np.all((target_points >= min_bound) & (target_points <= max_bound), axis=1)
    roi = o3d.geometry.PointCloud()
    roi.points = o3d.utility.Vector3dVector(target_points[mask])
    
    # Scale source
    source_scale = np.linalg.norm(np.asarray(source.points), axis=1).mean()
    target_scale = np.linalg.norm(np.asarray(roi.points), axis=1).mean()
    scale = target_scale / source_scale * 0.5  # Reduced scale factor
    
    source_points = np.asarray(source.points) * scale
    source.points = o3d.utility.Vector3dVector(source_points)
    
    # Align centers
    source.translate(roi.get_center() - source.get_center())
    
    # ICP
    reg = o3d.pipelines.registration.registration_icp(
        source, roi, 0.05, np.eye(4),
        o3d.pipelines.registration.TransformationEstimationPointToPoint(),
        o3d.pipelines.registration.ICPConvergenceCriteria(max_iteration=200)
    )
    source.transform(reg.transformation)
    return source

def main():
    source, target = load_point_clouds("untitled.obj", "ahmed.pcd")
    source.paint_uniform_color([1, 0, 0])
    target.paint_uniform_color([0, 1, 0])
    target = filter_points_above_level(target, 10)
    o3d.visualization.draw_geometries([target])
    min_bound, max_bound = find_dense_region(target)
    if min_bound is not None:
        aligned_source = extract_and_align(source, target, min_bound, max_bound)
        o3d.visualization.draw_geometries([aligned_source, target])
    else:
        print("Could not find suitable region")

if __name__ == "__main__":
    main()