Some questions for the euler angles integration

[sysuzyc]

Hi, @TixiaoShan , thanks for your excellent work !
In imageProjection.cpp/imuDeskewInfo() function, the euler integration is written as:

            // get angular velocity
            double angular_x, angular_y, angular_z;
            imuAngular2rosAngular(&thisImuMsg, &angular_x, &angular_y, &angular_z);

            // integrate rotation
            double timeDiff = currentImuTime - imuTime[imuPointerCur-1];
            imuRotX[imuPointerCur] = imuRotX[imuPointerCur-1] + angular_x * timeDiff;
            imuRotY[imuPointerCur] = imuRotY[imuPointerCur-1] + angular_y * timeDiff;
            imuRotZ[imuPointerCur] = imuRotZ[imuPointerCur-1] + angular_z * timeDiff;
            imuTime[imuPointerCur] = currentImuTime;

The angular_x, angular_y, angular_z is the angular velocity in IMU message.
We can get the angle integration by last angles and current angular velocity and time.
But the euler angle can't be added directly.
Is it better to write using quaternion?
The quaternion for two rotation could be multiply directly, such as:

            // get angular velocity
            double angular_x, angular_y, angular_z;
            imuAngular2rosAngular(&thisImuMsg, &angular_x, &angular_y, &angular_z);

            // integrate rotation
            double timeDiff = currentImuTime - imuTime[imuPointerCur-1];
            
            // using quaternion
            tf::Quaternion last_orientation;
            last_orientation = tf::createQuaternionFromRPY(imuRotX[imuPointerCur-1], imuRotY[imuPointerCur-1], imuRotZ[imuPointerCur-1]);
            tf::Quaternion delta_orientation;
            delta_orientation = tf::createQuaternionFromRPY(angular_x * timeDiff, angular_y * timeDiff, angular_z * timeDiff);
            tf::Quaternion curr_orientation = last_orientation * delta_orientation;
            double imuRoll, imuPitch, imuYaw;
            tf::Matrix3x3(curr_orientation).getRPY(imuRoll, imuPitch, imuYaw);
            imuRotX[imuPointerCur] = imuRoll;
            imuRotY[imuPointerCur] = imuPitch;
            imuRotZ[imuPointerCur] = imuYaw;
            imuTime[imuPointerCur] = currentImuTime;

[TixiaoShan]

Have you tested this new approach? How is the performance?
@sysuzyc

[stale]

This issue has been automatically marked as stale because it has not had recent activity. It will be closed if no further activity occurs. Thank you for your contributions.