TSDF integrate with icp transform matrix issue

Hi Techies,

Recently, I had an opportunity to work with Open3D for 3D reconstruction.

My use case is as follows:

1. captured data of an object placed on a rotating base plate (saving 30 depth frames per second, took 12 seconds for full rotation).
2. segmented the object only, removing the base plate and surrounding noise.
3. applied ICP to get the transformation matrix between the first and second frames.
4. used the integrate function with the ICP transformation.
5. repeat setp 2,3,4 until last frame and extract the mesh from the volume.

`
CX = 326.265
CY = 249.746
FX = 318.598
FY = 320.999

# Initialize TSDF volume for 3D reconstruction
voxel_size = 0.003 # 5mm voxel resolution
tsdf_volume = otd.pipelines.integration.ScalableTSDFVolume(
voxel_length=voxel_size,
sdf_trunc=voxel_size * 5,
color_type=otd.pipelines.integration.TSDFVolumeColorType.NoColor
)

for i in range(0, 360, 5):

plyFile = foPath + str(i) + exte

print("ProcessFile name : ", plyFile)

pcd = otd.io.read_point_cloud(str(plyFile))

# otd.visualization.draw_geometries([pcd], window_name="org")

filteredPCD = passFil(pcd)

roiGroundPCD = GetROIObjCloud(roiMinBound, roiMaxBound, filteredPCD)

roiGroundPCD.transform(extrinsic_matrix)

centerX, centerY, depthVal = GetCenterPoints(roiGroundPCD, 640, 480, FX, FY, CX, CY)

movePCDToCenter(roiGroundPCD, centerX, centerY, depthVal)

# otd.visualization.draw_geometries([roiGroundPCD], window_name="centroided")

roiGroundPCD.estimate_normals(otd.geometry.KDTreeSearchParamHybrid(radius=30.0, max_nn=30))

noiseRemovedPcd = None

if(i != 0):

# Apply ICP for tracking (pose estimation)
reg_p2p = otd.pipelines.registration.registration_icp(
roiGroundPCD, first_pcd, 15.00, np.eye(4),
otd.pipelines.registration.TransformationEstimationPointToPlane()
)

pose = reg_p2p.transformation

roiGroundPCD_CPY = copy.deepcopy(roiGroundPCD)

roiGroundPCD_CPY.transform(extrinsic_matrix_inv)

noiseRemovedPcd = NoiseRemovalObj(roiGroundPCD_CPY)

noiseRemovedPcd.transform(extrinsic_matrix)

if previousTransform is not None:
transform = previousTransform @ pose

else:
transform = pose

depthimg = pointcloud_to_depth(noiseRemovedPcd, 640, 480, FX, FY, CX, CY)

depth_image = otd.geometry.Image(depthimg) # Convert meters to mm

color_image = otd.geometry.Image(np.ones((depthimg.shape[0], depthimg.shape[1], 3), dtype=np.uint8))

# Create the RGBD Image
rgbd = otd.geometry.RGBDImage.create_from_color_and_depth(
color_image,
depth_image,
depth_scale=1000.0, # Convert mm to meters
depth_trunc=3.0, # Max depth 3m
convert_rgb_to_intensity=False
)

transform_tsdf = transform.copy()

transform_tsdf[:3, 3] /= 1000.0

intrinsic = otd.camera.PinholeCameraIntrinsic(640, 480, FX, FY, CX, CY)

# Integrate into TSDF volume
tsdf_volume.integrate(rgbd, intrinsic, transform_tsdf)#transform

first_pcd = copy.deepcopy(roiGroundPCD)

noiseRemovedPcd.transform(transform)

previousTransform = transform

else:

roiGroundPCD_CPY = copy.deepcopy(roiGroundPCD)

roiGroundPCD_CPY.transform(extrinsic_matrix_inv)

noiseRemovedPcd = NoiseRemovalObj(roiGroundPCD_CPY)

noiseRemovedPcd.transform(extrinsic_matrix)

depthimg = pointcloud_to_depth(roiGroundPCD, 640, 480, FX, FY, CX, CY)

depth_image = otd.geometry.Image(depthimg) # Convert meters to mm

color_image = otd.geometry.Image(np.zeros((depthimg.shape[0], depthimg.shape[1], 3), dtype=np.uint8))

# Create the RGBD Image
rgbd = otd.geometry.RGBDImage.create_from_color_and_depth(
color_image,
depth_image,
depth_scale=1000.0, # Convert mm to meters
depth_trunc=3.0, # Max depth 3m
convert_rgb_to_intensity=False
)

intrinsic = otd.camera.PinholeCameraIntrinsic(640, 480, FX, FY, CX, CY)

# Integrate into TSDF volume
tsdf_volume.integrate(rgbd, intrinsic, np.eye(4))

first_pcd = copy.deepcopy(roiGroundPCD)


mergPCd = mergPCd + noiseRemovedPcd

mesh = tsdf_volume.extract_triangle_mesh()
mesh.compute_vertex_normals()
otd.visualization.draw_geometries([mesh], window_name="combined")


mesh = tsdf_volume.extract_triangle_mesh()
mesh.compute_vertex_normals()
otd.visualization.draw_geometries([mesh])
`

The issue is:
expected result is overlapped mesh, but i get the mesh merged each other side by side like below image.


could explain what is issue ? and How to resolve it.