I am currently trying to use single-wired scanning sonar to obtain the Cloud map of underwater environment, by which I'd like to navigate the ROV in a pre-mapped area(As shown in attached image, green is the updated cloud map and blue one is the pre-obtained map of the area, I want to determine the displacement by using the regional cloud map and the fixed map).

I tried to use the package of Open3d, but the effect is horrible. Meanwhile, as I'm able to determine the yaw angle by using IMU, I want to only obtain the translation. Is there any open-source package to achieve that?