Sample IMU + PCL project for D435i

[GruffyPuffy]

---

Required Info
Camera Model	D435i
Firmware Version	05.10.13.00
Operating System & Version	Linux (Ubuntu 18)
Kernel Version (Linux Only)	4.15.0-43-generic
Platform	PC
SDK Version	2.17.0 + PCL
Language	C++
Segment	others

Issue Description

Hi there! I just wanted to show a small sample I made for the D435i. I found no other sample where the camera was integrated with PCL (to do point cloud maths) as well as using the IMU to rotate the cloud based on camera rotation. I have done a very simple filter for ACCEL + GYRO using the ACCEL to drift-compensate the GYRO. Note that I do not care about YAW in this sample (as the ACCEL cannot drift compensate that and I did not need it for my project).

The code uses the PCL visualizer to view the cloud as well as I added a second coordinate system for the camera rotation in order to visualize when you turn/rotate the camera. The idea with the sample is to rotate the cloud based on camera rotation in order to keep the "floor" (if indoor) aligned with the point cloud coordinate system...but see it as a test of the IMU-data.

All can be found here: https://github.com/GruffyPuffy/imutest

A small issue I have had. I tried to use the "callback" method suggested here to use the higher framerates of the GYRO and ACCEL (compared to DEPTH frames). It seems like if I turn on ONLY GYRO then I get it at good rate, but when I turn on something with a slower rate, the GYRO will not be stable in full speed. Same for ACCEL. I also tested this with "wait_for_frames" as well as "poll_for_frames", but I did not see any difference. In my sample I have therefore used the "poll" method and tried to use the timestaps when I integrate (i.e. sum up) the GYRO. Seems like it is working pretty Ok now. Feel free to suggest better filtering (Kalman?) and/or other suggestions...the source is there for you to improve! :)

Regards
Stefan

[dorodnic]

This is awesome @GruffyPuffy !

Thank you for sharing. We are also working on something similar to add to RealSense samples. If you are interested in contributing or listing your code under community projects, you are welcomed to open a pull-request.

The Madgwick filter (suggested before) implementation is GPL, making it difficult to integrate in a permissive open-source. Are you getting satisfactory results with your method?

We are also working on a white-paper / tutorial on IMU calibration, this will most likely improve the results.

[GruffyPuffy]

@dorodnic
I will see when/if I get some time to do a pull-request...I would be happy to contribute.
About results. I get decent results, I think it is very important to get good timestamps and/or good rate accuracy for us to do this good/better. See this as a first implementation where I at least figured out all angles and offsets... :)

Update: Btw...I did implement a gyro bias compensation. It is turned off by default as that would force the user to hold the camera steady while calculating the bias.

[GruffyPuffy]

Small update for the interested reader.

If you want more background material on how I calculate the accelerometer angles please see this text:
https://www.instructables.com/id/Accelerometer-Gyro-Tutorial/

I am using a "complementary filter" instead of (for example) Kalman in this example. Try googling this and you will get lots of comparisons/explanations on this. One sample link to read could be this: http://www.pieter-jan.com/node/11

Hopefully this will clear things up on what I tried to do. Please let me know if I made some mistakes or if you have suggestions on other methods.

[dorodnic]

Hi @GruffyPuffy
Anna created dedicated SDK example based on your work - you are welcomed to review and comment here.
Attribution to your work is included in the readme. Thanks :)