IMU sensor data conversion to roll and pitch

asked 2019-11-24 16:42:47 -0500

rax gravatar image

updated 2019-11-24 16:59:25 -0500

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 flag offensive close merge delete