I modified the code for add_points in the loop and implemented point cloud processing, but there was no effect. How should I continue to make changes? Or is there something I haven't noticed?
for index, loop in enumerate(detected_loops):
assert loop.query_submap_id == self.current_working_submap.get_id()
loop_index = self.current_working_submap.get_last_non_loop_frame_index() + index + 1
if self.use_sim3:
pose_world_detected = self.map.get_submap(loop.detected_submap_id).get_pose_subframe(loop.detected_submap_frame)
pose_world_query = self.current_working_submap.get_pose_subframe(loop_index)
pose_world_detected = gtsam.Pose3(pose_world_detected)
pose_world_query = gtsam.Pose3(pose_world_query)
H_relative_lc = pose_world_detected.between(pose_world_query).matrix()
else:
# points_world_detected = self.map.get_submap(loop.detected_submap_id).get_frame_pointcloud(loop.detected_submap_frame).reshape(-1, 3)
# points_world_query = self.current_working_submap.get_frame_pointcloud(loop_index).reshape(-1, 3)
# H_relative_lc = ransac_projective(points_world_query, points_world_detected)
points_world_detected = self.map.get_submap(loop.detected_submap_id).get_frame_pointcloud(loop.detected_submap_frame).reshape(-1, 3)
points_world_query = self.current_working_submap.get_frame_pointcloud(loop_index).reshape(-1, 3)
colors_detected = self.map.get_submap(loop.detected_submap_id).get_frame_color(loop.detected_submap_frame).reshape(-1, 3).astype(np.float32) / 255.0
colors_query = self.current_working_submap.get_frame_color(loop_index).reshape(-1, 3).astype(np.float32) / 255.0
H_relative_lc = ransac_projective(points_world_query, points_world_detected)
points_query_in_detected = apply_homography(H_relative_lc, points_world_query)
pcd_det = o3d.geometry.PointCloud()
pcd_det.points = o3d.utility.Vector3dVector(points_world_detected)
pcd_qry = o3d.geometry.PointCloud()
pcd_qry.points = o3d.utility.Vector3dVector(points_query_in_detected)
pcd_det.colors = o3d.utility.Vector3dVector(colors_detected)
pcd_qry.colors = o3d.utility.Vector3dVector(colors_query)
moved_lc, trans_lc = move(down=-0.05, up=0.05, base_pcd=pcd_det, move_pcd=pcd_qry, is_begin=False)
moved_lc = moved_lc.transform(np.linalg.inv(H_relative_lc))
moved_points = np.asarray(moved_lc.points)
moved_colors = np.asarray(moved_lc.colors)
moved_colors = (moved_colors * 255).astype(np.uint8)
self.map.get_submap(loop.detected_submap_id).set_frame_pointcloud(loop.detected_submap_frame, moved_points.reshape(self.map.get_submap(loop.detected_submap_id).get_frame_pointcloud(loop.detected_submap_frame).shape))
self.map.get_submap(loop.detected_submap_id).set_frame_color(loop.detected_submap_frame, moved_colors.reshape(self.map.get_submap(loop.detected_submap_id).get_frame_color(loop.detected_submap_frame).shape))
# H_relative_lc = trans_lc @ H_relative_lc
# o3d.io.write_point_cloud("detected.pcd", pcd_det)
# o3d.io.write_point_cloud("query.pcd", pcd_qry)
# o3d.io.write_point_cloud(f"./test_pcd/{num}.pcd", moved_lc)
# o3d.visualization.draw_geometries([moved_lc])
I modified the code for add_points in the loop and implemented point cloud processing, but there was no effect. How should I continue to make changes? Or is there something I haven't noticed?