ROS Answers: Open Source Q&A Forum - RSS feedhttps://answers.ros.org/questions/Open source question and answer forum written in Python and DjangoenROS Answers is licensed under Creative Commons Attribution 3.0Sun, 24 Nov 2019 16:42:47 -0600IMU sensor data conversion to roll and pitchhttps://answers.ros.org/question/338452/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.Sun, 24 Nov 2019 16:42:47 -0600https://answers.ros.org/question/338452/imu-sensor-data-conversion-to-roll-and-pitch/