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 :D

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.

Kind regards

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.