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?

  • $\begingroup$ You can also use a InertialUnit node and retrieve the ground truth quaternion for the orientation of the robot. Then, you may want to compare the results you get with the InertialUnit to the one you get with the Compass. This sample robot controller may be helpful to understand how to compare them. $\endgroup$ Commented Nov 30, 2022 at 8:47
  • 1
    $\begingroup$ @OlivierMichel, didn't know such device existed before. So in the end InertialUnit.getQuaternion() is what I was looking for. $\endgroup$
    – RavenCloud
    Commented Dec 1, 2022 at 0:56

2 Answers 2


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.


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 ;)


Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.

Not the answer you're looking for? Browse other questions tagged or ask your own question.