Full 3D Pose (Scale, Rotation and Translation) Estimation using Gyro and Acceleromter sensors fusion

Question

I am implementing a 3D pose estimation algoriothm on mobile device (Android) which has Gyro, Accelerometer and Magnetometer sensors. I have already develeoped a Visual SLAM algoirthm to estimate full 3D camera pose. I want to estimate same pose just by using these sensors.

I have seen the code for EKF based sensor fusion techniques, Attitude estiamtor, etc. But none of these give full 3D Pose. Insted these give only orientation (and not scale and translation)

Could any one suggest an open source C++ implementation (Not using ROS) for the problem?

Few links which I have already found:

https://github.com/simondlevy/TinyEKF

https://github.com/AIS-Bonn/attitude_estimator

Answer 1

The packages you've found don't estimate scale or 3d pose because that's not really feasible using just an imu. The only way to get 3d pose from an imu is to integrate acceleration (adjusting for attitude) but all real sensors have sensor bias so the error in the integration will grow unbounded.

To account for that, another sensor needs to be used to provide correction and bias estimation. In your case, the 3d pose estimation from the visual slam could perform this role. Other options include lidar based slam or gps.

Answer 2

I think you're looking at the problem the wrong way. Once you have an orientation, that gives you the transform between local (phone) and world coordinates. Then all you need to do is to transform your accelerometer data into the world frame and proceed with numeric integration.