Texturing the Intel Real Sense point cloud in Labview with the color camera

Intel has supplied a great LabView wrapper for the RealSense SDK 2.0 (LibRealSense DLL).
LabView Wrapper     (https://github.com/IntelRealSense/librealsense/tree/development/wrappers/labview)


However, as of 20190708  [July 8 2019] the wrapper uses SDK 2.11.0, which does not support the T265 tracking camera.  Also, the color camera of the D435 doesn't use the wrapped SDK.

This post will outline the process of using the D435 color camera to texture the point cloud.

Point cloud displayed with color texture

- To get images from the color camera in labview, you need to use National Instruments Vision Acquisition software.  In labview this is the NI-IMAQdx pallete.
- To manipulate the images you need the NI Vision Development Module.  It contains most of the sub VI's I use to turn the color image into a point cloud texture.

- Open the color camera and configure a Grab where you are initializing the Intel RealSense camera.
- Create an image type to be used later by the Grab vi.  I do this in a Functional Global Variable.

The Functional Global Variable holds all the images, and uses the serial number of the camera to create image names.

I use a sequence to acquire the image and depth data.  First I get the data from the RealSense SDK, then I get the color image from the NI-IMAQdx Grab vi.  Note that the Grab VI takes the image in memory, it doesn't wait for the next image.

All the wrapped DLL VIs that get depth and left grayscale imge data.

The ni-imaqdx grab is in the lower left.  It just needs to camera session from the initialization, and an image type [pointer].

The depth information is created based on the left grayscale camera.

! ! ! The Color camera and the Left grayscale camera do NOT have the same Field Of View (FOV).
!  !  !  ! And they are not the same size.
!   !   !   !   ! And the color camera, even though it has a higher resolution, has a smaller FOV

- 4 points are needed to define a Tetragon.  The points need to be where the greyscale image's corners would be in the color image,  You can calculate that from the scale difference betweeen the images, but I do it by calculating a homography matrix between cooresponding points in the two images, then multiplying the grayscale corner points by the homography matrix.

- Place 4 small object in the corners of the color image.
- Use an ROI tool to identify the corresponding points in the color and the left grayscale image.

- Solve the Homography matrix from the corresponding points

- Translate the grayscale corner points to the color image (even though we know they are outside of the color image.)

- Using the NI Tetragon Extract VI, pass in the translated grayscale corners and the dimensions of the grayscale mage, as well as the color image and a destination image.

- The output of the extract will be the color image scaled and shifted to line up with the grayscale and the depth image.
!! NOTE: The tetragon extract does not perform perspective correction.  The 4 points need to be on the same plane.  Please leave a comment if you have a better solution for labview.

- The .XYZ point cloud needs an RGB value from each pixel in the depth image.  In this step of the sequence the XYZ, RGB, and Normals are calculated.  More detail about the XYZ and RGB arrays will follow.

- The XYZ array converts each depth pixel to it's X,Y,Z location with Z being a measure of distance from the camera.  The X, and Y are scaled based on the Z  distance and the camera focal length.  The Intel realSense camera provides the intrinsic camera model where focal length is in pixels.

[Edit 20210220 - 

Zworld = the value of the depth pixel in millimeters 

Xworld =  ( (xdepth - ppx) * Zworld) / fx

Yworld = ( ( ydepth - ppy) * Zworld) / fy

where 

xdepth is the x value of the pixel in the depth image where top left = 0 and bottom right = image width -1

ydepth is the y value of the pixel in the depth image where top left = 0 and bottom right = image height -1

ppx is the principal point in x.  this is often the center of the image.

ppy is the principal point in y, again often the center y of the image.

fx is the focal length of the lens but its units are in pixels, not in millimeters.  

fy is the focal length of the lens in pixels, and is pretty much the same as fx.

Note, that the value of the pixel in the depth image  is the distance in millimeters that object is from the camera.  

To find the camera intrinsic parameters run the 

rs-sensor-control.exe

that will is part of the RealSense SDK

C:\Program Files (x86)\Intel RealSense SDK 2.0\tools

For a 1280x720 depth image from the D435 you can these values (which are close, but wont match your camera perfectly)

fx 645.912

fy 645.912

ppx 640

ppy 360

For a 640 x 360 depth image you can use these intrinsics (again, not exact)

fx 322.956

fy 322.956

ppx 320

ppy 180

Notice that the scale of the 640x320 is 1/2 of the 1280 x720. 

  end Edit ]

- The texture image must be converted to a 1D array of RGB values to match the XYZ array.

- Although outside of the scope of this post, this is how I calculate the normals per point:

The 2D depth array is used in this loop.

- The 1D arrays are placed into a point cloud functional global variable.

- The Point Cloud FGV has a write .XYZ text file function.  Once written the text file can be read by other programs, such as CloudCompare (https://www.danielgm.net/cc/)  and MeshLab.

-----
 Here is a sample point cloud:
Miscellaneous stuff

https://drive.google.com/file/d/1BqyCqsdOnIXcrCtNC4F2uBuuFD8Wcr-S/view?usp=sharing


If you open it in cloud compare, make sure to set the column to X, Y, Z, Normal X, Normal Y, Normal Z, Color.R(0-1), Color G(0-1), Color B(0-1), Ignore the last column.

Example viewed in cloud compare: