I tried to use open3d==0.13.0 to match target orientation box rx=45,ry=45,zr=45 in degree with source box with transformation_init = np.asarray([[1,0,0,0], [0,1,0,0], [0,0,1,0], [0,0,0,1]]), but the result still does not match with the source box.
import numpy as np import open3d as o3d from scipy.spatial.transform import Rotation as R import copy points = [] for dim in range(3): for val in [0, 2]: face_points_dim = np.full((20, 20), val) face_points_other1, face_points_other2 = np.meshgrid(np.linspace(0, 2, 20), np.linspace(0, 2, 20)) face_points = np.empty((20, 20, 3)) face_points[:, :, dim] = face_points_dim face_points[:, :, (dim + 1) % 3] = face_points_other1 face_points[:, :, (dim + 2) % 3] = face_points_other2 points.append(face_points.reshape(-1, 3)) xyz = np.concatenate(points, axis=0) pcd = o3d.geometry.PointCloud() pcd.points = o3d.utility.Vector3dVector(xyz) pcd.estimate_normals() rotation = R.from_euler('xyz', [45, 45, 45], degrees=True) rotated_xyz = rotation.apply(xyz) target = o3d.geometry.PointCloud() target.points = o3d.utility.Vector3dVector(rotated_xyz) target.estimate_normals() transformation_init = np.asarray([[1,0,0,0], [0,1,0,0], [0,0,1,0], [0,0,0,1]]) convergence_criteria = o3d.pipelines.registration.ICPConvergenceCriteria(max_iteration=2000, relative_fitness=1e-5, relative_rmse=1e-5) reg_p2p = o3d.pipelines.registration.registration_icp( pcd, target, 0.02, transformation_init, o3d.pipelines.registration.TransformationEstimationPointToPlane(), convergence_criteria ) print(reg_p2p) print("Transformation is:") print(reg_p2p.transformation) final_rotation = reg_p2p.transformation[:3, :3] final_translation = reg_p2p.transformation[:3, 3] print("Final rotation is:") print(final_rotation) print("Final translation is:") print(final_translation) source_temp = copy.deepcopy(pcd) source_temp.transform(transformation_init) print("\nShowing initial matching") o3d.visualization.draw_geometries([source_temp, target], zoom=0.5, front=[0, 0, -1], lookat=[2, 2, 2], up=[0, 1, 0]) source_temp = copy.deepcopy(pcd) source_temp.transform(reg_p2p.transformation) print("\nShowing final matching") o3d.visualization.draw_geometries([source_temp, target], zoom=0.5, front=[0, 0, -1], lookat=[2, 2, 2], up=[0, 1, 0]) I also tried modified code where max_iteration, relative_fitness, and relative_rmse are set as required in ICPConvergenceCriteria
2 Answers
This is a code update. If I use an threshold of 50 it works as expected and added Gaussian noise to the target.
import copy import numpy as np import open3d as o3d from scipy.spatial.transform import Rotation as R def draw_registration_result(source, target, transformation, zoom=1.5): source_temp = copy.deepcopy(source) target_temp = copy.deepcopy(target) source_temp.paint_uniform_color([0, 0, 1]) target_temp.paint_uniform_color([0, 1, 0]) print("\nShowing initial matching") o3d.visualization.draw_geometries([source_temp, target_temp], zoom=zoom, front=[0.9288, -0.2951, -0.2242], lookat=[0, 1, 1], up=[0, 0, 1]) print("\nShowing final matching") source_temp.transform(transformation) coord_frame = o3d.geometry.TriangleMesh.create_coordinate_frame() o3d.visualization.draw_geometries([source_temp, target_temp, coord_frame], zoom=zoom, front=[0.9288, -0.2951, -0.2242], lookat=[0, 1, 1], up=[0, 0, 1]) def create_box(noise_std_dev=0.0): points = [] for dim in range(3): for val in [0, 2]: face_points_dim = np.full((20, 20), val) face_points_other1, face_points_other2 = np.meshgrid(np.linspace(0, 2, 20), np.linspace(0, 2, 20)) face_points = np.empty((20, 20, 3)) face_points[:, :, dim] = face_points_dim face_points[:, :, (dim + 1) % 3] = face_points_other1 face_points[:, :, (dim + 2) % 3] = face_points_other2 points.append(face_points.reshape(-1, 3)) xyz = np.concatenate(points, axis=0) pcd = o3d.geometry.PointCloud() pcd.points = o3d.utility.Vector3dVector(xyz) pcd.estimate_normals() rotation = R.from_euler('xyz', [45, 0, 0], degrees=True) rotated_xyz = rotation.apply(xyz) # Add Gaussian noise to the target # Standard deviation of the noise. You can adjust this value. noise = np.random.normal(0, noise_std_dev, rotated_xyz.shape) noisy_rotated_xyz = rotated_xyz + noise target = o3d.geometry.PointCloud() target.points = o3d.utility.Vector3dVector(noisy_rotated_xyz) target.estimate_normals() transformation_init = np.asarray([[1,0,0,0], [0,1,0,0], [0,0,1,0], [0,0,0,1]]) return pcd, target, transformation_init def extract_euler_from_matrix(transformation_matrix): x, y, z = transformation_matrix[:3, 3] rotation_matrix = transformation_matrix[:3, :3] rx = np.degrees(np.arctan2(rotation_matrix[2, 1], rotation_matrix[2, 2])) ry = np.degrees(-np.arctan2(rotation_matrix[2, 0], np.sqrt(rotation_matrix[2, 1]**2 + rotation_matrix[2, 2]**2))) rz = np.degrees(np.arctan2(rotation_matrix[1, 0], rotation_matrix[0, 0])) return x, y, z, rx, ry, rz def registration_icp(pcd, target, transformation_init): convergence_criteria = o3d.pipelines.registration.ICPConvergenceCriteria(max_iteration=2000, relative_fitness=1e-5, relative_rmse=1e-5) thr = 50.0 reg_p2p = o3d.pipelines.registration.registration_icp( pcd, target, thr, transformation_init, o3d.pipelines.registration.TransformationEstimationPointToPlane(), convergence_criteria ) ''' Transformation matrix. ''' print("Transformation is:") print(reg_p2p.transformation) final_rotation = reg_p2p.transformation[:3, :3] final_translation = reg_p2p.transformation[:3, 3] print(final_rotation) print("Final translation is:") print(final_translation) x, y, z, rx, ry, rz = extract_euler_from_matrix(transformation_matrix=reg_p2p.transformation) print(f"x:{x}, y:{y}, z:{z}, rx:{rx}, ry:{ry}, rz:{rz}") ''' Transformation matrix. ''' source_temp = copy.deepcopy(pcd) source_temp.transform(transformation_init) draw_registration_result(source=source_temp,target=target,transformation=reg_p2p.transformation) if __name__ == "__main__": pcd, target, transformation_init = create_box(noise_std_dev=0.07) registration_icp(pcd=pcd, target=target, transformation_init=transformation_init) Transformation is: [[ 9.99987173e-01 1.57532059e-03 -4.81376216e-03 4.57822184e-03] [-4.50522269e-03 7.10944598e-01 -7.03233590e-01 2.93253628e-04] [ 2.31449985e-03 7.03246256e-01 7.10942576e-01 1.86811886e-03] [ 0.00000000e+00 0.00000000e+00 0.00000000e+00 1.00000000e+00]] [[ 0.99998717 0.00157532 -0.00481376] [-0.00450522 0.7109446 -0.70323359] [ 0.0023145 0.70324626 0.71094258]] Final translation is: [0.00457822 0.00029325 0.00186812] Extract euler: x:0.00457822183653805, y:0.0002932536277546905, z:0.0018681188593605135, rx:44.68818712412947, ry:-0.13261119149981235, rz:-0.25813181050792067 Showing initial matching Showing final matching
ICP is suitable when a good initial approximation for the transformation is known and is good because the data may be incomplete or noisy and the relationships between points are unknown. In our case, the order of the points is preserved under the transformation we want to find. Thus, we can immediately use the method of determining the transformation between points (open3d.pipelines.registration.TransformationEstimationPointToPoint). This method is called the Kabsch algorithm and is implemented in the Eigen library (umeyama.h). As an alternative, you can use the method from the following repository github/MIT-SPARK/TEASER-plusplus.
Here is the code using the correct method
import numpy as np import open3d as o3d from scipy.spatial.transform import Rotation as R import copy points = [] for dim in range(3): for val in [0, 2]: face_points_dim = np.full((20, 20), val) face_points_other1, face_points_other2 = np.meshgrid(np.linspace(0, 2, 20), np.linspace(0, 2, 20)) face_points = np.empty((20, 20, 3)) face_points[:, :, dim] = face_points_dim face_points[:, :, (dim + 1) % 3] = face_points_other1 face_points[:, :, (dim + 2) % 3] = face_points_other2 points.append(face_points.reshape(-1, 3)) xyz = np.concatenate(points, axis=0) pcd = o3d.geometry.PointCloud() pcd.points = o3d.utility.Vector3dVector(xyz) # pcd.estimate_normals() # we don't need normals rotation = R.from_euler('xyz', [45, 45, 45], degrees=True) rotated_xyz = rotation.apply(xyz) target = o3d.geometry.PointCloud() target.points = o3d.utility.Vector3dVector(rotated_xyz) # target.estimate_normals() estimation_method = open3d.pipelines.registration.TransformationEstimationPointToPoint() correspondences = open3d.utility.Vector2iVector(np.tile(np.arange(len(target.points)), reps=(2, 1)).T) transformation = estimation_method.compute_transformation(source=pcd, target=target, corres=correspondences) source_in_target = copy.deepcopy(pcd).transform(transformation) source_in_target.paint_uniform_color([1., 0., 0.]) target.paint_uniform_color([0., 1., 0.]) error = np.linalg.norm(np.asarray(source_in_target.points) - np.asarray(target.points), axis=-1).max() print(f"Max geometric error between points from source and target {error:.2f}") # we translate source by small amount to show that is different from target o3d.visualization.draw_geometries([source_in_target.translate([0.01, 0., 0.]), target]) 
