TSDF integrate with icp transform matrix issue

[dinesh19102001]

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.