Question about getting global coordinates of lidar point cloud from relative in Webots

Question

I need to do custom mapping of surroundings with lidar using mobile robot in Webots. What I use for that:

• GPS for getting robot position.
• Compass for getting direction robot.
• Lidar for getting info about surroundings.

I did translation and rotation of relative points from lidar, which worked well when robot is on flat surface.

But no matter how much I tried I can't figure out how to get accurate global coordinates from point cloud relative points, when robot is even a bit tilted.

My guess is that it suppose to use spatial transformation matrices, but I not sure how to use Webots Compass values in rotation matrix.

Maybe someone can show code example or explain the math behind it or there is a method that I missed in Webots?

Answer 1

Basic Example of solution on Python:

from scipy.spatial.transform import Rotation as Rotation

RobotPoint = gps.getValues()
STR = Rotation.from_quat(InertialUnit.getQuaternion())
for RelativeCloudPoint in lidar.getPointCloud():
  Point2 = STR.apply(RelativeCloudPoint)
  GlobalCloudPoint = RelativeCloudPoint + RobotPoint

Using InternalUnit to get Quaternion for spartial rotation matrix. Then apply it to relative coordinates. After that add to it real robot coordinates from GPS. In the end you will get global coordinates of points you need.

Answer 2

I guess you’re using GPS to get the initial transformation, matching lidar points with ICP and re-calculating the pose update.

• P’1 = Pose_gps * P1
• Match P2 with P’1 using ICP
• Pose = P2 * pinv(P1)

I would use a Kalman filter with the pose in state vector and use the motion model in the state machine to constraint the motion.

Don’t forget to use quaternions and avoid gimble lock ;)