Orthogonal projection of laserscanner data

Question

I recently discovered this ROS-package: http://wiki.ros.org/laser_ortho_projector . Which is basically exactly what I need. However I am not using ROS, so I need to do what is been done in this package myself.

Basically the information I have is the range measurement r and the angle theta for every measurement point of a 360 degree laserscan + I have the orientation in roll, pitch, yaw angles of the laserscanner. However yaw is not important for me and could be ignored.

I really can't get my head around how to project those points to the ground plane. I mean it is easy for the measurement point which align with the roll and pitch axes, but I don't know what to do with the points in between.

One solution I thought of is this:

1. Convert the measurement point (r, theta) in cartesian coordinates (x,y,z) - vector
2. Use rotations matrices: create rotation matrix for rotation around roll axis with roll angle, and adequately for the pitch axis. Multiplay bot matrices and then multiply it with (x,y,z) - vector.
3. Now the orthogonal projection of the of the measurement would be the (x,y,z) - vector with z=0.
4. Convert (x,y) - vector back to polar coordinates (r, theta).

However, especially step 2 is very complicated, because the rotation matrices change according to the sign of the roll and pitch angles, right?

I would like to note that the absolute value of role and pitch angles will always be < 90°, so there should not be an unambiguity with rotations..

Is there an easier (or maybe more elegant) way to solve my problem?

My guess is that this problem must have been solved basically for every robot application which uses a 2D-laserscanner that is not fixed to one axis. But I can not find the solution anywhere.

So I would be very glad if anyone of you could point me in the right direction.

Answer 1

Use the location and attitude measurements of your craft to develop a 4x4 transform matrix, then take the inverse of that matrix to put the points you measure back in terms of world coordinates. Then, if you want a projection just set the respective measurement to zero.

If you want a projection on the XY plane, set all Z coordinates to zero, set X to zero for a projection on the YZ plane, etc.

There are lots of ways to build the rotation matrix required for the transform matrix. You need to consult your IMU documentation to see how it is interpreting/outputting angle information to build the rotation matrix correctly.

As a final note, even if you don't need yaw information don't discard it. Depending on the IMU configuration yaw may not mean what you think it means and thus could have an impact on your transform matrix.