Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

How to get rotation matrix or angles #7907

Closed
ori30ffs opened this issue Dec 3, 2020 · 15 comments
Closed

How to get rotation matrix or angles #7907

ori30ffs opened this issue Dec 3, 2020 · 15 comments

Comments

@ori30ffs
Copy link

ori30ffs commented Dec 3, 2020

Required Info
Camera Model D455
Operating System & Version Win 10
Platform PC
Language python

Issue Description

Hello,

I was trying to get rotation information from gyro stream. I need it for rotate my pointcloud.
I refer to this page rs-motion. But unfortunately I don't understand how gyro values are changed.
In my experiments I was rotate camera around one dimensions, but all 3 values was changed

Now I'm using static camera but I still need to find ita rotation.

Q1: What gyro stream values is? Is it some raw data?

Q2: How I can convert this values to angles or rotation matrix for static camera?

Q3: Does frames from camera and IMU synchronized?

Q4: Is it possible to read all frames from .bag (not realtime reading)

@MartyG-RealSense
Copy link
Collaborator

MartyG-RealSense commented Dec 3, 2020

Hi @ori30ffs

  1. The IMU's gyro provides data about the camera's rotational angle in three axes, X (Pitch), Y (Yaw) and Z (Roll). The IMU in the RealSense cameras does not have an internal calibrator, and so it provides raw angle data that can have motion correction applied to it by software to take account of the known inaccuracy factors in the IMU hardware component. For example, an IMU that has a y-accel value of 9.6 in its raw state before correction may become closer to the ideal target of 9.80 after correction.

  2. The link below provides Python scripting for getting the IMU values from the camera, which should relay values representing the camera in its current stationary position on its fixing point (eg a mount or a tripod).

#4391

  1. Intel's guide to getting IMU data from D435i or T265 states: "The D435i depth camera generates and transmits the gyro and accelerometer samples independently, as the inertial sensors exhibit different FPS rates (200/400Hz for gyro, 63/250Hz for accelerometer). Each IMU data packet is timestamped using the depth sensor hardware clock to allow temporal synchronization between gyro, accel and depth frames".

https://www.intelrealsense.com/how-to-getting-imu-data-from-d435i-and-t265/

  1. Yes, you can read the frames from a bag in non-realtime by making use of the instruction set_real_time(false). A couple of example references in Python for doing so are in the links below.

#3682 (comment)

https://stackoverflow.com/questions/62310081/is-there-a-simple-way-to-find-the-total-number-of-frames-recorded-in-a-bag-file

You could alternatively make use of the SDK's rs-convert tool in Windows to extract frames from a bag in a range of formats. If you are not able to compile its C++, the Windows version of the RealSense SDK provides a pre-built executable version of the tool called rs-convert.exe in the SDK Tools folder.

Further information about launching and controlling rs-convert can be found in the link below.

https://support.intelrealsense.com/hc/en-us/community/posts/360051132573/comments/360014080493

@ori30ffs
Copy link
Author

ori30ffs commented Dec 3, 2020

Hello @MartyG-RealSense
Thanks for your reply. I run #4391 script with my bag file from static camera, now I see this values:

Angle -  X: 1.03   Y: 150.84   Z: -18.07
Angle -  X: 0.99   Y: 150.84   Z: -18.07
Angle -  X: 0.99   Y: 150.84   Z: -15.74
Angle -  X: 0.99   Y: 150.87   Z: 97.96
Angle -  X: 0.99   Y: 150.87   Z: 72.9
Angle -  X: 1.03   Y: 150.84   Z: 116.03
Angle -  X: 0.99   Y: 150.87   Z: 72.0
Angle -  X: 0.99   Y: 150.83   Z: 73.8
Angle -  X: 0.99   Y: 150.83   Z: 158.32
Angle -  X: 0.99   Y: 150.87   Z: -16.2
Angle -  X: 0.29   Y: 150.87   Z: 71.1
Angle -  X: -33.92   Y: 150.87   Z: 28.19
Angle -  X: -16.17   Y: 150.87   Z: 88.26
Angle -  X: 0.92   Y: 150.87   Z: -16.2
Angle -  X: 0.21   Y: 150.87   Z: 72.9
Angle -  X: -32.83   Y: 150.87   Z: 111.52
Angle -  X: 35.55   Y: 150.83   Z: -148.58
Angle -  X: -0.77   Y: 150.87   Z: 70.2
Angle -  X: -88.97   Y: 150.87   Z: -18.0
Angle -  X: -88.48   Y: 150.87   Z: -14.4
Angle -  X: -61.14   Y: 150.83   Z: 161.02
Angle -  X: 1.03   Y: 150.88   Z: 117.83
Angle -  X: 1.03   Y: 150.88   Z: 160.13
Angle -  X: 0.99   Y: 150.88   Z: 67.39
Angle -  X: 0.99   Y: 150.84   Z: -153.07
Angle -  X: 0.99   Y: 150.88   Z: -150.37
Angle -  X: 0.99   Y: 150.88   Z: -19.87
Angle -  X: 1.03   Y: 150.84   Z: -104.84
Angle -  X: 0.99   Y: 150.88   Z: 52.13
Angle -  X: 1.56   Y: 150.84   Z: -14.47
Angle -  X: 28.86   Y: 150.84   Z: 158.33
Angle -  X: 89.23   Y: 150.84   Z: -14.47
Angle -  X: 0.99   Y: 150.84   Z: 160.13
Angle -  X: 1.52   Y: 150.88   Z: 71.93
Angle -  X: 27.39   Y: 150.88   Z: 71.93
Angle -  X: 20.36   Y: 150.88   Z: 73.73
Angle -  X: 63.16   Y: 150.87   Z: 160.49
Angle -  X: 1.03   Y: 150.87   Z: 88.19
Angle -  X: 1.03   Y: 150.87   Z: -15.74
Angle -  X: 0.99   Y: 150.83   Z: 96.23
Angle -  X: 0.99   Y: 150.87   Z: -15.74
Angle -  X: 1.03   Y: 150.83   Z: 97.96
Angle -  X: 1.03   Y: 150.83   Z: 69.22
Angle -  X: 0.99   Y: 150.83   Z: -62.1
Angle -  X: 1.03   Y: 150.83   Z: -16.28
Angle -  X: 2.26   Y: 150.83   Z: 70.12
Angle -  X: 62.03   Y: 150.83   Z: -16.28
Angle -  X: -54.16   Y: 150.87   Z: 72.82
Angle -  X: 1.03   Y: 150.87   Z: 114.3
Angle -  X: 1.03   Y: 150.87   Z: -16.28
Angle -  X: 1.03   Y: 150.87   Z: 71.92
Angle -  X: 0.99   Y: 150.87   Z: 70.12
Angle -  X: 0.99   Y: 150.83   Z: -18.0
Angle -  X: 1.03   Y: 150.87   Z: -18.08
Angle -  X: -0.77   Y: 150.87   Z: -18.08
Angle -  X: -87.21   Y: 150.87   Z: -18.08
Angle -  X: 0.99   Y: 150.83   Z: -16.28
Angle -  X: 0.99   Y: 150.87   Z: 73.69
Angle -  X: 0.99   Y: 150.87   Z: 160.62
Angle -  X: 0.99   Y: 150.83   Z: 96.23
Angle -  X: 0.09   Y: 150.87   Z: -14.48
Angle -  X: -43.11   Y: 150.72   Z: 158.29
Angle -  X: 0.09   Y: 150.72   Z: -16.31
Angle -  X: -43.07   Y: 150.87   Z: 71.92
Angle -  X: 0.99   Y: 150.83   Z: 71.02
Angle -  X: 0.99   Y: 150.87   Z: 29.09
Angle -  X: 2.79   Y: 150.87   Z: 132.29
Angle -  X: 89.23   Y: 150.87   Z: -18.08
Angle -  X: 1.03   Y: 150.87   Z: -18.08
Angle -  X: 0.99   Y: 150.87   Z: -18.0
Angle -  X: 0.99   Y: 150.87   Z: -16.2
Angle -  X: 0.64   Y: 150.83   Z: 72.29
Angle -  X: -16.56   Y: 150.87   Z: 90.36
Angle -  X: -16.21   Y: 150.87   Z: 91.79
Angle -  X: 0.99   Y: 150.87   Z: 156.52
Angle -  X: 1.03   Y: 150.87   Z: -103.13
Angle -  X: 0.99   Y: 150.87   Z: 134.09
Angle -  X: 0.99   Y: 150.83   Z: 70.13
Angle -  X: 0.99   Y: 150.83   Z: -18.08
Angle -  X: 1.03   Y: 150.83   Z: -14.47
Angle -  X: 0.99   Y: 150.87   Z: 158.33
Angle -  X: 1.03   Y: 150.87   Z: -16.27
Angle -  X: 1.03   Y: 150.87   Z: 70.12
Angle -  X: 1.03   Y: 150.83   Z: -18.08
Angle -  X: 0.99   Y: 150.83   Z: -16.81
Angle -  X: -0.81   Y: 150.83   Z: 44.05
Angle -  X: -87.21   Y: 150.83   Z: -18.11
Angle -  X: 0.13   Y: 150.87   Z: -14.48
Angle -  X: -43.11   Y: 150.83   Z: 160.2
Angle -  X: 1.47   Y: 150.87   Z: 72.46
Angle -  X: 24.6   Y: 150.87   Z: 97.43
Angle -  X: 0.99   Y: 150.87   Z: 45.89
Angle -  X: 0.09   Y: 150.87   Z: 71.92
Angle -  X: -43.07   Y: 150.87   Z: 70.12
Angle -  X: -0.77   Y: 150.87   Z: -18.0
Angle -  X: -87.17   Y: 150.83   Z: -18.0
Angle -  X: 0.99   Y: 150.87   Z: -18.07
Angle -  X: -0.77   Y: 150.83   Z: -18.11
Angle -  X: -89.01   Y: 150.87   Z: -18.0
Angle -  X: -87.21   Y: 150.83   Z: -15.15
Angle -  X: 0.99   Y: 150.83   Z: 127.11
Angle -  X: 1.56   Y: 150.87   Z: 72.0
Angle -  X: 27.96   Y: 150.83   Z: 71.92
Angle -  X: 45.99   Y: 150.87   Z: 70.12
Angle -  X: 44.56   Y: 150.87   Z: -14.48
Angle -  X: -25.0   Y: 150.87   Z: 161.92
Angle -  X: 1.03   Y: 150.87   Z: 161.39
Angle -  X: 0.99   Y: 150.87   Z: 134.09
Angle -  X: 0.99   Y: 150.87   Z: 70.12
Angle -  X: 0.99   Y: 150.87   Z: -15.67
Angle -  X: 0.99   Y: 150.87   Z: 98.56
Angle -  X: 0.99   Y: 150.83   Z: 97.95
Angle -  X: 0.99   Y: 150.83   Z: 72.0
Angle -  X: 1.03   Y: 150.87   Z: 72.82
Angle -  X: 1.03   Y: 150.87   Z: 116.38
Angle -  X: 0.5   Y: 150.87   Z: 85.68
Angle -  X: -25.57   Y: 150.87   Z: 72.0
Angle -  X: -25.04   Y: 150.87   Z: 72.29
Angle -  X: 0.99   Y: 150.87   Z: 90.36
Angle -  X: 1.03   Y: 150.83   Z: 89.7
Angle -  X: 0.09   Y: 150.87   Z: 50.26
Angle -  X: -43.07   Y: 150.83   Z: -104.47
Angle -  X: 1.03   Y: 150.83   Z: 70.12
Angle -  X: 0.99   Y: 150.87   Z: -16.2
Angle -  X: 2.83   Y: 150.87   Z: 70.13
Angle -  X: 89.19   Y: 150.87   Z: -16.28
Angle -  X: 0.99   Y: 150.87   Z: 70.13
Angle -  X: 1.03   Y: 150.83   Z: -19.88
Angle -  X: 0.99   Y: 150.87   Z: -104.4
Angle -  X: 1.03   Y: 150.83   Z: 73.72
Angle -  X: 0.99   Y: 150.83   Z: 160.12
Angle -  X: 0.99   Y: 150.87   Z: 70.12
Angle -  X: 0.99   Y: 150.87   Z: -18.08
Angle -  X: 0.32   Y: 150.87   Z: -15.38
Angle -  X: -34.24   Y: 150.87   Z: 116.92
Angle -  X: -34.05   Y: 150.83   Z: 116.56
Angle -  X: -22.62   Y: 150.87   Z: 97.06
Angle -  X: 0.13   Y: 150.87   Z: 27.82
Angle -  X: -43.97   Y: 150.87   Z: 71.92
Angle -  X: -43.11   Y: 150.83   Z: 70.12
Angle -  X: 0.99   Y: 150.87   Z: -18.0
Angle -  X: 0.99   Y: 150.83   Z: -16.28
Angle -  X: 0.99   Y: 150.84   Z: 71.93
Angle -  X: 1.03   Y: 150.87   Z: 71.93
Angle -  X: 0.99   Y: 150.88   Z: 72.0
Angle -  X: 1.03   Y: 150.88   Z: 73.73
Angle -  X: 1.03   Y: 150.88   Z: 161.93
Angle -  X: 0.99   Y: 150.87   Z: 159.23
Angle -  X: 1.03   Y: 150.88   Z: 24.23
Angle -  X: 1.27   Y: 150.88   Z: -104.51
Angle -  X: 15.65   Y: 150.88   Z: 71.89
Angle -  X: 45.09   Y: 150.88   Z: 70.2
Angle -  X: 1.03   Y: 150.88   Z: -18.07
Angle -  X: 1.03   Y: 150.84   Z: -14.47
Angle -  X: 1.03   Y: 150.84   Z: 158.86
Angle -  X: 1.03   Y: 150.88   Z: 9.76
Angle -  X: 1.03   Y: 150.88   Z: 72.0
Angle -  X: 0.55   Y: 150.87   Z: 72.46
Angle -  X: -23.07   Y: 150.87   Z: 98.49
Angle -  X: -22.58   Y: 150.87   Z: 96.16
Angle -  X: 1.03   Y: 150.87   Z: -18.0
Angle -  X: 1.03   Y: 150.87   Z: -18.08
Angle -  X: 1.03   Y: 150.87   Z: -18.08
Angle -  X: 1.93   Y: 150.87   Z: -16.2
Angle -  X: 46.4   Y: 150.83   Z: 70.2
Angle -  X: 63.19   Y: 150.87   Z: -16.28
Angle -  X: 1.03   Y: 150.87   Z: 73.72
Angle -  X: 1.03   Y: 150.83   Z: 160.73
Angle -  X: 1.4   Y: 150.98   Z: 97.92
Angle -  X: 19.96   Y: 150.87   Z: 73.72
Angle -  X: 44.61   Y: 150.87   Z: 160.66
Angle -  X: -22.58   Y: 150.83   Z: 96.12
Angle -  X: 0.99   Y: 150.83   Z: -18.08
Angle -  X: 0.32   Y: 150.98   Z: -16.2
Angle -  X: -32.02   Y: 150.98   Z: 72.0
Angle -  X: 0.99   Y: 150.87   Z: 70.12
Angle -  X: 1.03   Y: 150.87   Z: -14.48
Angle -  X: 1.89   Y: 150.83   Z: 156.52
Angle -  X: 45.09   Y: 150.83   Z: -104.48
Angle -  X: 1.03   Y: 150.87   Z: 71.92
Angle -  X: 1.03   Y: 150.83   Z: 71.92
Angle -  X: 1.7   Y: 150.87   Z: 72.79
Angle -  X: 36.26   Y: 150.87   Z: 116.89
Angle -  X: 35.55   Y: 150.87   Z: 117.82
Angle -  X: 0.99   Y: 150.83   Z: 162.0
Angle -  X: 1.03   Y: 150.87   Z: 158.4
Angle -  X: 1.03   Y: 150.87   Z: -14.4
Angle -  X: 0.99   Y: 150.83   Z: 160.2
Angle -  X: -0.77   Y: 150.83   Z: 70.12
Angle -  X: -87.17   Y: 150.83   Z: -14.4
Angle -  X: 1.03   Y: 150.83   Z: 162.0
Angle -  X: 1.03   Y: 150.87   Z: 158.32
Angle -  X: 1.03   Y: 150.83   Z: -18.0
Angle -  X: 1.03   Y: 150.83   Z: -18.11
Angle -  X: 1.03   Y: 150.83   Z: -18.11
Angle -  X: 0.99   Y: 150.87   Z: -16.56
Angle -  X: 0.46   Y: 150.87   Z: 58.24
Angle -  X: -26.84   Y: 150.87   Z: 70.12
Angle -  X: -86.9   Y: 150.87   Z: -16.95
Angle -  X: 16.18   Y: 150.83   Z: 38.91

I expect near values in each frame, but X and Z angles has strong hesitation. Can you help me to understant the physics of it?

@MartyG-RealSense
Copy link
Collaborator

I looked carefully at your problem with X and Z and the script that you used. Could you try using a lower value for alpha in this line please:

alpha = 0.98

A high alpha weights the calculations towards the gyro but can cause drift. Reducing the alpha weights the calculations towards the accelerometer but makes it more sensitive. As the camera is static though, sensitivity may not matter so much in this case.

@ori30ffs
Copy link
Author

ori30ffs commented Dec 3, 2020

I tried it. This has no effect.
I used 0.98, 0.5, 0.02 alphas
Also, I looked at accel data, It almost every time has [0,0,0] value (sometime <0.01)
Maybe it's a problem with IMU? Is there any way to check and confirm that IMU is working properly?

@MartyG-RealSense
Copy link
Collaborator

MartyG-RealSense commented Dec 3, 2020

I recall a case about the RealSense T265 Tracking Camera (which uses the same IMU component) where its values were drifting at start-up even when stationary, and it was advised to give the IMU enough initial motion to help it to understand its local position. That may not be an option that you can easily test in your case though if the D435i is rigidly mounted instead of being on a tripod.

It may be worth calibrating your IMU with the Python-based IMU calibration tool, which Dorodnic the RealSense SDK Manager has confirmed is compatible with D455.

#7571

@ori30ffs
Copy link
Author

ori30ffs commented Dec 3, 2020

Okay, I'll calibrate it. Please note, we have this problem with two cameras.
I thought d455 has calibration by default, is it right?

@MartyG-RealSense
Copy link
Collaborator

MartyG-RealSense commented Dec 3, 2020

The RGB and left and right sensors are calibrated in the factory. Originally, the 400 Series IMU was not calibrated in the factory and had to be user-calibrated after purchase with the Python tool. I have heard differing information about whether the IMU is now factory-calibrated too or not. Nevertheless, doing your own IMU calibration is a good course of action if you are not satisfied with the values being provided by it.

@MartyG-RealSense
Copy link
Collaborator

Hi @ori30ffs Do you require further assistance with this case, please? Thanks!

@MartyG-RealSense
Copy link
Collaborator

Case closed due to no further comments received.

@PierreLouisT
Copy link

Hello everybody. I want to change the referential. From camera referential to a fixe referential as a room for example.
With the accelerometer I can have ax, ay and az (acceleration on x,y,z). Depending on the orientation of the camera, gravity can be on X,Y or Z axis based on the camera referential. Using Rotation Matrix, how to project the correct axis ?
For example, if I put down the camera, Gravity will be on Z axis. How to make the good transformation to have the pointcloud oriented on the good axis, here Y
I hope you will understand my problem, I can add information if needed,
Thanks in advance,

PL

@MartyG-RealSense
Copy link
Collaborator

MartyG-RealSense commented Jun 22, 2022

Hi @PierreLouisT Please do provide further information about what you are trying to achieve.

The IMU and its acceleration and gravity values have no effect on how a depth pointcloud behaves.

If the camera is rotated then whatever direction the front of the camera is facing in will be the Z axis that represents depth / distance.

Are you hoping to have another axis such as Y represent distance (the direction that the camera is facing) instead?

Thanks very much for your patience!

@PierreLouisT
Copy link

Hi Marty ! Absolutely, that is what I need to do. I tried to apply Rotation to project the point cloud to another referential without success. The main goal is to reconstruct 3D image from multiples cameras in a location differently oriented from others. From multiples cameras I wish to get one point cloud in one referential

@MartyG-RealSense
Copy link
Collaborator

MartyG-RealSense commented Jun 24, 2022

@PierreLouisT If you are able to use ROS then Intel have a guide for stitching together pointclouds from multiple RealSense cameras facing in different directions, both with cameras attached to a single computer or two computers.

https://www.intelrealsense.com/how-to-multiple-camera-setup-with-ros/

image

image

Alternatively, a RealSense ROS user at IntelRealSense/realsense-ros#1176 (comment) also suggested an edit to the RealSense ROS wrapper's code to change how the IMU axes are represented. As mentioned above though, the IMU would not have an influence on point clouds.


If your project requires the use of Python to align multiple clouds then the RealSense SDK's rs2_transform_point_to_point instruction is worth investigating. More information about this can be found at #7853 (comment)

@FeiSong123
Copy link

Hi ! Is there any way for l515 to know the roll angle, yaw angle and pitch angle of the camera relative to the initial position using C++ ?

@MartyG-RealSense
Copy link
Collaborator

Hi @FeiSong123 The RealSense SDK's C++ IMU example program rs-motion, which works with L515, may be a useful reference for you.

https://github.com/IntelRealSense/librealsense/tree/master/examples/motion

Lines of the rs-motion.cpp code for retrieving roll, pitch and yaw values are highlighted here:

https://github.com/IntelRealSense/librealsense/blob/master/examples/motion/rs-motion.cpp#L138-L140

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Projects
None yet
Development

No branches or pull requests

4 participants