# IMU sensor data conversion to roll and pitch

I am using a geometry_msgs/Vector3 linear_acceleration from sensor_msgs/Imu. I need to get the the roll and pitch from this message. Below is the formula, I am using:

 float roll  = 180 * atan2(accelY, sqrt(accelX * accelX + accelZ * accelZ)) / pi();
float pitch = 180 * atan2(accelX, sqrt(accelY * accelY + accelZ * accelZ)) / pi();


where accelX, accelY and accelZ are linear acceleration parameters.

After getting the the roll and pitch, I am using the forming the rotation matrices for roll and pitch:

     // for pitch
float theta1 = degreesToRadians(pitch);
rotationMatrix1(0,0) = cos(theta1);
rotationMatrix1(0,1) = 0;
rotationMatrix1(0,2) = -sin(theta1);
rotationMatrix1(1,0) = 0;
rotationMatrix1(1,1) = 1;
rotationMatrix1(1,2) = 0;
rotationMatrix1(2,0) = sin(theta1);
rotationMatrix1(2,1) = 0;
rotationMatrix1(2,2) = cos(theta1);

// for roll
float theta2 = degreesToRadians(roll);
rotationMatrix2(0,0) = 1;
rotationMatrix2(0,1) = 0;
rotationMatrix2(0,2) = 0;
rotationMatrix2(1,0) = 0;
rotationMatrix2(1,1) = cos(theta2);
rotationMatrix2(1,2) = sin(theta2);
rotationMatrix2(2,0) = 0;
rotationMatrix2(2,1) = -sin(theta2);
rotationMatrix2(2,2) = cos(theta2);

// getting the roll and pitch in one matrix
rotationMatrix = rotationMatrix1 * rotationMatrix2;


Finally applying transformation to point cloud using pcl::transformPointCloud:

pcl::transformPointCloud(*input_cloud,*output_cloud,rotationMatrix);


But, I am not getting the desired result - where my point cloud should stay steady. I need to figure out where is it going wrong.

Any comments would be appreciated regarding this issue. Thanks.

edit retag close merge delete