D455 motion frame is slow in C#

[choe-hyun-ho]

---

Required Info
Camera Model	D455
Firmware Version	05.13.00.50
Operating System & Version	Windows 11
Kernel Version (Linux Only)
Platform	PC
SDK Version	2.51.4348
Language	C#/Unity
Segment

Hi Teams,

I am trying to use D455 with C#/Unity.
After reviewing several examples including rs-motion(C++) and rs-capture(C#), I managed to build simple C# application using both cameras & IMU simultaneously.

If I enable motion stream only, it works fine, and enoughly fast.
But if I enable motion stream and RGB/depth camera stream simultaneously, motion frame rate becomes much slow (about 5~6 frame per second), even if I don't add any video frame handling codes.

Please see the below C# code.
This is a very simple .NET console application which is reading several streams from D455, and measure/print frame rate every second.
When I enabled only accelerometer and gyroscope, it can read hundreds of frames per each second, but with Color/Depth camera stream altogether, only 5~6 frames received for IMU.
I also checked USB connection status via RS viewer. It says connected to USB 3.2.

In C example codes, and In RealSense Viewer, this problem doesn't happen.

using System;
using System.Collections.Generic;
using System.Runtime.InteropServices;
using System.Linq;
using System.Text;
using System.Timers;
using Intel.RealSense;

namespace imu
{
    public struct Vector3
    {
        public float x, y, z;
    }
    class Program
    {
        static void Main(string[] args)
        {
            Pipeline pipeline;
            Timer timer;
            try
            {
                // Create and config the pipeline to strem color and depth frames.
                pipeline = new Pipeline();
                int count = 0;

                using (var ctx = new Context())
                {
                    var devices = ctx.QueryDevices();
                    var dev = devices[0];

                    Console.WriteLine("\nUsing device 0, an {0}", dev.Info[CameraInfo.Name]);
                    Console.WriteLine("    Serial number: {0}", dev.Info[CameraInfo.SerialNumber]);
                    Console.WriteLine("    Firmware version: {0}", dev.Info[CameraInfo.FirmwareVersion]);

                    var sensors = dev.QuerySensors();
                    var depthSensor = sensors[0];
                    var colorSensor = sensors[1];
                    var motionSensor = sensors[2];

                    var depthProfile = depthSensor.StreamProfiles
                                        .Where(p => p.Stream == Stream.Depth)
                                        .OrderBy(p => p.Framerate)
                                        .Select(p => p.As<VideoStreamProfile>()).First();

                    var colorProfile = colorSensor.StreamProfiles
                                        .Where(p => p.Stream == Stream.Color)
                                        .OrderBy(p => p.Framerate)
                                        .Select(p => p.As<VideoStreamProfile>()).First();

                    var accelProfile = motionSensor.StreamProfiles
                                        .Where(p => p.Stream == Stream.Accel)
                                        .OrderBy(p => p.Framerate)
                                        .Select(p => p.As<MotionStreamProfile>()).First();

                    var gyroProfile = motionSensor.StreamProfiles
                                        .Where(p => p.Stream == Stream.Gyro)
                                        .OrderBy(p => p.Framerate)
                                        .Select(p => p.As<MotionStreamProfile>()).First();

                    var cfg = new Config();
                    cfg.EnableStream(Stream.Depth, depthProfile.Width, depthProfile.Height, depthProfile.Format, depthProfile.Framerate);
                    cfg.EnableStream(Stream.Color, colorProfile.Width, colorProfile.Height, colorProfile.Format, colorProfile.Framerate);
                    cfg.EnableStream(Stream.Accel, accelProfile.Format, accelProfile.Framerate);
                    cfg.EnableStream(Stream.Gyro, gyroProfile.Format, gyroProfile.Framerate);

                    var pp = pipeline.Start(cfg);
                }
                timer = new Timer(1000);
                timer.Enabled = true;
                timer.Elapsed += delegate (object sender, ElapsedEventArgs e)
                {
                    Console.WriteLine("IMU {0} FPS", count);
                    count = 0;
                };
                while (true)
                {
                    // We wait for the next available FrameSet and using it as a releaser object that would track
                    // all newly allocated .NET frames, and ensure deterministic finalization
                    // at the end of scope. 
                    using (var frames = pipeline.WaitForFrames())
                    {
//                        var colorFrame = frames.ColorFrame.DisposeWith(frames);
//                        var depthFrame = frames.DepthFrame.DisposeWith(frames);
                        var gyroFrame = frames.FirstOrDefault<MotionFrame>(Stream.Gyro, Format.MotionXyz32f).DisposeWith(frames);
//                        Console.WriteLine("TM={0} GX={1} GY={2} GZ={3}", gyroFrame.Timestamp, gyroFrame.MotionData.x, gyroFrame.MotionData.y, gyroFrame.MotionData.z);
                        count++;
                        var accelFrame = frames.FirstOrDefault<MotionFrame>(Stream.Accel, Format.MotionXyz32f).DisposeWith(frames);
//                        Console.WriteLine("TM={0} AX={1} AY={2} AZ={3}", accelFrame.MotionData.x, accelFrame.MotionData.y, accelFrame.MotionData.z);

                        // TODO: Render the frames.
                    }
                }
            }
            catch (Exception ex)
            {
                Console.WriteLine(ex.Message);
            }
        }
    }
}

[MartyG-RealSense]

Hi @choe-hyun-ho It is a known issue that if depth, RGB and IMU are enabled simultaneously in the RealSense Viewer or a self-created script then problems can occur for some RealSense users (not all), most commonly a 'No Frames Received' error on one of the streams.

It does not matter which order the three streams are enabled in, as it is the enabling of the IMU (Motion Module) that is the trigger for the issue. I note though that you are not experiencing problems with three streams enabled in the Viewer. The C++ rs-capture and rs-multicam programs are not affected by the issue and can stream depth, RGB and IMU simultaneously.

A workaround for RealSense users affected by the depth-RGB-IMU issue in C++ is to use callbacks. Information about accessing the IMU with callbacks in C# can be found at #8746

In Python the most successful workaround is to create two separate pipelines - IMU alone on one pipeline and depth / RGB on the other pipeline - as described by the Python script at #5628 (comment)

#10858 discusses accessing the IMU with C# in Unity.

---

It may be worth exploring creating multiple pipelines in the RealSense Unity wrapper and putting the IMU on a separate one by modifying a technique at the link below for defining multiple instances of RsDevice for multiple cameras. Instead though, use a single camera serial number with profiles for depth + RGB on one RsDevice instance and an IMU profile on the other RsDevice.

https://support.intelrealsense.com/hc/en-us/community/posts/360047049173-Community-contribution-for-using-multiple-cameras-in-RealSense-Unity-wrapper

---

If you are not already using the RealSense Unity wrapper for your Unity project, details are here:

https://github.com/IntelRealSense/librealsense/tree/master/wrappers/unity

[choe-hyun-ho]

Thanks, MartyG.

With your advice, I managed to get successful result in my test C# code.
It can get 400 samples/s for accelerometer, and 200 samples/s for gyroscope, and quite reasonable result.
I used callback instead of WaitForFrame().
The below is my final code.

It works fine, but is seems to be bug in typecasting Frame to MotionFrame.
When I use PtrToStructure, the code works fine.
But when I use the following code instead to get MotionFrame object, I can't get frame at all.

var f = frame.As<MotionFrame>();

My code is wrong, or bug in typecaster?

using System;
using System.Collections.Generic;
using System.Runtime.InteropServices;
using System.Linq;
using System.Text;
using System.Timers;
using Intel.RealSense;

namespace imu
{
    public struct Vector3
    {
        public float x, y, z;
    }
    class Program
    {
        static void Main(string[] args)
        {
            Pipeline pipeline;
            Timer timer;
            try
            {
                // Create and config the pipeline to strem color and depth frames.
                pipeline = new Pipeline();
                int accel_count = 0, gyro_count = 0;

                using (var ctx = new Context())
                {
                    var devices = ctx.QueryDevices();
                    var dev = devices[0];

                    Console.WriteLine("\nUsing device 0, an {0}", dev.Info[CameraInfo.Name]);
                    Console.WriteLine("    Serial number: {0}", dev.Info[CameraInfo.SerialNumber]);
                    Console.WriteLine("    Firmware version: {0}", dev.Info[CameraInfo.FirmwareVersion]);

                    var sensors = dev.QuerySensors();
                    var depthSensor = sensors[0];
                    var colorSensor = sensors[1];
                    var motionSensor = sensors[2];

                    var depthProfile = depthSensor.StreamProfiles
                                        .Where(p => p.Stream == Stream.Depth)
                                        .OrderBy(p => p.Framerate)
                                        .Select(p => p.As<VideoStreamProfile>()).First();

                    var colorProfile = colorSensor.StreamProfiles
                                        .Where(p => p.Stream == Stream.Color)
                                        .OrderBy(p => p.Framerate)
                                        .Select(p => p.As<VideoStreamProfile>()).First();

                    var accelProfile = motionSensor.StreamProfiles
                                        .Where(p => p.Stream == Stream.Accel)
                                        .OrderBy(p => p.Framerate)
                                        .Select(p => p.As<MotionStreamProfile>()).First();

                    var gyroProfile = motionSensor.StreamProfiles
                                        .Where(p => p.Stream == Stream.Gyro)
                                        .OrderBy(p => p.Framerate)
                                        .Select(p => p.As<MotionStreamProfile>()).First();

                    var cfg = new Config();
                    cfg.EnableStream(Stream.Depth, depthProfile.Width, depthProfile.Height, depthProfile.Format, depthProfile.Framerate);
                    cfg.EnableStream(Stream.Color, colorProfile.Width, colorProfile.Height, colorProfile.Format, colorProfile.Framerate);
                    cfg.EnableStream(Stream.Accel, accelProfile.Format, accelProfile.Framerate);
                    cfg.EnableStream(Stream.Gyro, gyroProfile.Format, gyroProfile.Framerate);

                    var pp = pipeline.Start(cfg, frame => {
                        if (frame.IsComposite)
                        {
                            using (var frames = frame.As<FrameSet>())
                            {
                                var colorFrame = frames.ColorFrame.DisposeWith(frames);
                                var depthFrame = frames.DepthFrame.DisposeWith(frames);
                                // TODO: Render the frames.
                                var gyroFrame = frames.FirstOrDefault<Frame>(Stream.Gyro, Format.MotionXyz32f).DisposeWith(frames);
                                if (gyroFrame != null)
                                {
                                    var a = Marshal.PtrToStructure<Vector3>(gyroFrame.Data);
                                    Console.WriteLine("GX={0} GY={1} GZ={2}", a.x, a.y, a.z);
                                    gyro_count++;
                                }
                                var accelFrame = frames.FirstOrDefault<Frame>(Stream.Accel, Format.MotionXyz32f).DisposeWith(frames);
                                if (accelFrame != null)
                                {
                                    var b = Marshal.PtrToStructure<Vector3>(accelFrame.Data);
                                    accel_count++;
                                    Console.WriteLine("AX={0} AY={1} AZ={2}", b.x, b.y, b.z);
                                }
                                return;
                            }
                        }
                        using (var p = frame.Profile)
                        {
                            if (p.Stream == Stream.Color || p.Stream == Stream.Depth)
                            {
                                // TODO: Render the frames.
                            }
                            else if (p.Stream == Stream.Gyro && p.Format == Format.MotionXyz32f)
                            {
                                //var f = frame.As<MotionFrame>();
                                //Console.WriteLine("GX={0} GY={1} GZ={2}", f.MotionData.x, f.MotionData.y, f.MotionData.z);
                                var a = Marshal.PtrToStructure<Vector3>(frame.Data);
                                //Console.WriteLine("GX={0} GY={1} GZ={2}", a.x, a.y, a.z);
                                gyro_count++;
                            }
                            else if (p.Stream == Stream.Accel && p.Format == Format.MotionXyz32f)
                            {
                                //var f = frame.As<MotionFrame>();
                                //Console.WriteLine("AX={0} AY={1} AZ={2}", f.MotionData.x, f.MotionData.y, f.MotionData.z);
                                var b = Marshal.PtrToStructure<Vector3>(frame.Data);
                                //Console.WriteLine("AX={0} AY={1} AZ={2}", b.x, b.y, b.z);
                                accel_count++;
                            }
                        }
                    });
                }
                timer = new Timer(1000);
                timer.Enabled = true;
                timer.Elapsed += delegate (object sender, ElapsedEventArgs e)
                {
                    Console.WriteLine("ACC: {0} FPS, GYR: {1} FPS", accel_count, gyro_count);
                    accel_count = gyro_count = 0;
                };
                Console.ReadLine();
            }
            catch (Exception ex)
            {
                Console.WriteLine(ex.Message);
            }
        }
    }
}

[MartyG-RealSense]

If a script uses callbacks then it should include the word callback in the brackets of the pipe start instruction so that the SDK knows that it is a callback script. Does performance improve if you write the pipe start line like this:

var pp = pipeline.Start(cfg,callback,frame => {

[choe-hyun-ho]

Hmmm...
MartyG, it is incorrect.
My code is the correct syntax to defining callback function using lambda notation.

Anyway, my last question is why the following statement is not woring properly.
Can you check this?

var f = frame.As<MotionFrame>();

[MartyG-RealSense]

Looking at the overall code, it may be better to simplify the Else instructions from two down to one by having only Format.MotionXyz32f as the Else condition and the Accel and Gyro checks nested within it as If conditions. So the program will not check that the stream is Accel or Gyro unless the stream format is MotionXyz32f

using (var p = frame.Profile)
{

if (p.Stream == Stream.Color || p.Stream == Stream.Depth)
{
// TODO: Render the frames.
}

else if (p.Format == Format.MotionXyz32f)
{

if (p.Stream == Stream.Gyro)
{
//var f = frame.As<MotionFrame>();
//Console.WriteLine("GX={0} GY={1} GZ={2}", f.MotionData.x, f.MotionData.y, f.MotionData.z);
var a = Marshal.PtrToStructure<Vector3>(frame.Data);
//Console.WriteLine("GX={0} GY={1} GZ={2}", a.x, a.y, a.z);
gyro_count++;
}
                           
if (p.Stream == Stream.Accel)
{
//var f = frame.As<MotionFrame>();
//Console.WriteLine("AX={0} AY={1} AZ={2}", f.MotionData.x, f.MotionData.y, f.MotionData.z);
var b = Marshal.PtrToStructure<Vector3>(frame.Data);
//Console.WriteLine("AX={0} AY={1} AZ={2}", b.x, b.y, b.z);
accel_count++;
 }

// Close the Else statement
}
// Close the Using statement
);

[choe-hyun-ho]

OK, MartyG.
Thanks for your advice.

Can you run my code yourself?
When the following code is used,

var a = Marshal.PtrToStructure<Vector3>(frame.Data);

The result is as follows:

ACC: 415 FPS, GYR: 188 FPS
ACC: 411 FPS, GYR: 201 FPS
ACC: 412 FPS, GYR: 203 FPS
ACC: 411 FPS, GYR: 201 FPS
...

But the following code is used:

var f = frame.As<MotionFrame>();

The result is as follows:

ACC: 16 FPS, GYR: 16 FPS
ACC: 0 FPS, GYR: 0 FPS
ACC: 0 FPS, GYR: 0 FPS
ACC: 0 FPS, GYR: 0 FPS
ACC: 0 FPS, GYR: 0 FPS
ACC: 0 FPS, GYR: 0 FPS
ACC: 0 FPS, GYR: 0 FPS
ACC: 0 FPS, GYR: 0 FPS
ACC: 0 FPS, GYR: 0 FPS
...

Why this drastic difference is made?
I want to check why the second one is not correctly working.

Thanks.

[MartyG-RealSense]

My computer is not equipped with a development environment to compile scripts, unfortunately.

Does it work if you use var f = frame.MotionFrame

[choe-hyun-ho]

There is no property like MotionFrame in Frame class.
Even in Frameset, MotionFrame property is not existing, even in the latest SDK.

I want to check the above symptom is only happening in my environment, or generic.
In fact, marshalling and casting pointer to structure is not a native approach for C#, and I feel Frame.As or Frameset.MotionFrame approach is more natural for me.
But, it is not working as expected, but I also noticed sometimes this typecasting is working in other cases.

I am curious that this code causing problem only for me, or treated as bug.

[MartyG-RealSense]

The MotionFrame error may have occurred because you changed from using (var frames = pipeline.WaitForFrames()) to callback and var f = frame.MotionFrame relies on the 'frames' definition in the WaitForFrames() line to work.

using (var frames = pipeline.WaitForFrames())
{

// Later in the script

var f = frames.MotionFrame

However, another RealSense user reported in #4418 (comment) that MotionFrame was not supported in C#, so using the WaitForFrames approach may not have resolved the problem anyway.

It was also confirmed at #2996 (comment) that MotionFrame was not supported in C#, and it was suggested that the IMU be accessed via PtrToStructure<Vector>. You have already succeeded with PtrToStructure<Vector> however and are seeking an alternative approach.

[MartyG-RealSense]

Hi @choe-hyun-ho Do you require further assistance with this case, please? Thanks!

[choe-hyun-ho]

The issue I reported is that the following code is not working as expected.

var f = frame.As<MotionFrame>();

And after all the following discussion, I think C# wrapper lacks of some features/functions for IMU like this.
'''
var f = frames.MotionFrame
'''

I hope this fixed in the next release of SDK,

And there is no more assistance needed about this issue. Thx.

[MartyG-RealSense]

I followed up about the <motionframe> class and found that although #2996 (comment) in January 2019 stated that the C# wrapper did not support it, the motionframe class was actually added to the wrapper the following month in February 2019.

https://github.com/IntelRealSense/librealsense/blob/master/wrappers/csharp/Intel.RealSense/Frames/MotionFrame.cs

[MartyG-RealSense]

Case closed due to no further assistance required.