Solved: Using D435 sensor : Mapping coordinates from color to depth after aligning frames

MCC001 · ‎03-21-2019

Camera: D435

OS: Windows 10

language: C#

I had been successfully using a program with SR300 which I'm now migrating to support D435 sensor only with latest SDK. The older SDK had some methods to map coordinates from optical to depth which I cant find in the new SDK.

However with the limited C# samples available I can see there is a way of aligning color and depth frames. However the aligned frame seems to be a VideoFrame and not DepthFrame. With DepthFrame I could do use the CopyTo function to access depth values by iterating through the pixels.

I'm wondering how can I similarly access depth values from a the aligned Videoframe.

My progressing code is below - which works fine without aligning frames, but then I cant find a way to map a pixel from color to its depth equivalent. Also for some reason if I use the align filter, the frames just freeze ) guess one of the native calls throw an error )

Please note Q1 & Q2 in commented code below. Any help will be greatly appreciated. Thank you.

var token = tokenSource.Token;
        ushort[] depthdata = new ushort[640 * 480];
        var t = Task.Factory.StartNew(() =>
        {
            while (!token.IsCancellationRequested)
            {
                //// We use the frames that are captured from live camera as the input data for the SW device
                using (var frames = pipeline.WaitForFrames())
                {
                    var depthFrame = frames.DepthFrame.DisposeWith(frames);
                    var colorFrame = frames.ColorFrame.DisposeWith(frames);
                    sync.SubmitFrame(depthFrame);
                    sync.SubmitFrame(colorFrame);
                }
                // Dispaly the frames that come from the SW device after synchronization
                using (var new_frames = sync.WaitForFrames())
                {
                    if (new_frames.Count == 2)
                    {
                        new_frames.ApplyFilter(align); // Q1: This results in freeze after few seconds. 
                        var depthFrame = new_frames.DepthFrame.DisposeWith(new_frames);
                        var colorFrame = new_frames.ColorFrame.DisposeWith(new_frames);
                        // VideoFrame colorizedDepth = colorizer.Process(depthFrame).As<VideoFrame>().DisposeWith(new_frames);
                        depthFrame.CopyTo(depthdata);
                        //  var colorizedDepth = frames[Intel.RealSense.Stream.Depth, Format.Rgb8].As<VideoFrame>().DisposeWith(frames);
                        // colorizedDepth.CopyTo(depthdata);  // Q2: What should be size of depthdata array here ( Stride * Height ?) OR ( Width * height ) and how can i iterate through it to access z values ? 
                        WriteableBitmap colorizedDepthImage;
                        unsafe
                        {
                            //D435 - get access to data and then see the followin convertion to dataptr
                            _calibrationData.angleCameraTorsoRadians = 0.43;
                            fixed (ushort* dataptr = depthdata)
                            {
                                // MAIN OPERATION On DEPTH Data here ..// 
                                //measurer.Measure(dataptr, regions, _calibrationData);
                            }
                            fixed (ushort* dataptr = depthdata)
                            {
                                // Colorize Depth Image for Display ( this is for SR300 where I programmed the colors myself for depth ) 
                                colorizedDepthImage = _colorizer.GetColorizedBitmap(dataptr);
                                var depthStream = GetPngStream(colorizedDepthImage);
                            }
                            //For capturing Color stream as well 
                            var bytes = new byte[colorFrame.Stride * colorFrame.Height];
                            colorFrame.CopyTo(bytes);
                            // .... Mode code here // 
                            //Raise events to notify UI
                            RaiseFrameProcessedEvent(new ProcessedFrame(depthStream, regions, _calibrationData));
                            RaiseColorFrameAvailableEvent(new ColorFrame(colorStream));
                            depthStream.Dispose();
                        }
                    }
                }
            }
        }, token);

MartyG · ‎03-21-2019

An alternative to using Align in the RealSense SDK 2.0 is an instruction called rs2_project_color_pixel_to_depth_pixel

It maps a pixel in the color image to a pixel in the depth image. An example use of the instruction can be found in the link below, starting on line 116 of the script:

https://github.com/IntelRealSense/librealsense/blob/development/include/librealsense2/rsutil.h#L116

View solution in original post

MartyG · ‎03-21-2019

An alternative to using Align in the RealSense SDK 2.0 is an instruction called rs2_project_color_pixel_to_depth_pixel

It maps a pixel in the color image to a pixel in the depth image. An example use of the instruction can be found in the link below, starting on line 116 of the script:

https://github.com/IntelRealSense/librealsense/blob/development/include/librealsense2/rsutil.h#L116