hight-level-robotics-congnitive-agent/librealsense/wrappers/opencv/kinfu

rs-kinfu Sample

Overview

KinectFusion is a real-time 3D reconstruction algorithm, which uses depth stream to estimate the camera motion. The current sensor pose is simultaneously obtained by tracking the live depth frame relative to the global model using a coarse-to-fine iterative closest point (ICP) algorithm, which uses all of the observed depth data available.

A detailed description of the algorithm can be found in the full paper.

This example utilizes the KinectFusion implementation available in RGBD module of opencv_contrib. For more details see the official documentation.

Installation

Install OpenCV and opencv_contrib as described in the official guide. Note that version 4 of OpenCV is required.

In step 5, along with configuring OPENCV_EXTRA_MODULES_PATH, check the OPENCV_ENABLE_NONFREE option.

For additional guidelines on building librealsene with OpenCV please follow OpenCV Samples for Intel RealSense cameras.

When configuring CMake for librealsense, in addition to checking BUILD_CV_EXAMPLES, check the BUILD_CV_KINFU_EXAMPLE flag. Also, ensure that BUILD_GRAPHICAL_EXAMPLES is checked.

Expected Output

The application should open a window and display a pointcloud which grows according to camera movement, fusing new frames into the existing scene.

The pointcloud is responsive to mouse movements and this way view from different perspectives is possible.

The following animation shows output retrieved using a D415 camera:

Code Overview

First we declare pointers to the objects of KinFu and Params. We initialize Params with the default values, later on we'll change the relevant parameters manually.

Ptr<KinFu> kf;
Ptr<Params> params = Params::defaultParams();

With D415, we use the preset RS2_RS400_VISUAL_PRESET_HIGH_DENSITY for optimization. Alternatively, RS2_RS400_VISUAL_PRESET_HIGH_DENSITY can be used in D435. Also, we define depth_scale which we'll use as a parameter for KinFu.

for (rs2::sensor& sensor : dev.query_sensors())
{
    if (rs2::depth_sensor dpt = sensor.as<rs2::depth_sensor>())
    {
        // Set some presets for better results
        dpt.set_option(RS2_OPTION_VISUAL_PRESET, RS2_RS400_VISUAL_PRESET_HIGH_DENSITY);
        // Depth scale is needed for the kinfu set-up
        depth_scale = dpt.get_depth_scale();
        break;
    }
}

Initialize post-processing filters (with default configuration), we'll use them later to improve depth quality before passing it to KinFu as input.

// Declare post-processing filters for better results
auto decimation = rs2::decimation_filter();
auto spatial = rs2::spatial_filter();
auto temporal = rs2::temporal_filter();

To avoid excessive noise from far away objects (that are not part of the scene we are interested in), we declare clipping_dist which sets the maximal depth to consider. Adjust max_dist according to your needs.

auto clipping_dist = max_dist;

Acquire frame size and camera intrinsics which will be used as parameters for KinFu.

auto intrin = stream_depth.as<rs2::video_stream_profile>().get_intrinsics();
// Use decimation once to get the final size of the frame
d = decimation.process(d);
auto w = d.get_width();
auto h = d.get_height();
Size size = Size(w, h);
auto intrin = stream_depth.as<rs2::video_stream_profile>().get_intrinsics();

Set frame size, intrinsics and depth factor using the values we previously found. Now params is ready to initialize a new KinFu object.

// Configure kinfu's parameters
params->frameSize = size;
params->intr = Matx33f(intrin.fx, 0, intrin.ppx,
                       0, intrin.fy, intrin.ppy,
                       0, 0, 1);
params->depthFactor = 1 / depth_scale;

// Initialize KinFu object
kf = KinFu::create(params);

Set a boolean to indicate whether it is safe to acquire the current pointcloud.

bool after_reset = false;

calc_cloud_thread handles the post processing of the depth frames, as well as running KinFu and obtaining the pointcloud. First, post-processing filters are applied.

// Use post processing to improve results
d = decimation.process(d);
d = spatial.process(d);
d = temporal.process(d);

As mentioned earlier, we discard pixels with depth higher than clipping_dist. This might help the algorithm focus on objects within the relevant range.

// Set depth values higher than clipping_dist to 0, to avoid unnecessary noise in the pointcloud
#pragma omp parallel for schedule(dynamic) //Using OpenMP to try to parallelise the loop
uint16_t* p_depth_frame = reinterpret_cast<uint16_t*>(const_cast<void*>(d.get_data()));
for (int y = 0; y < h; y++)
{
    auto depth_pixel_index = y * w;
    for (int x = 0; x < w; x++, ++depth_pixel_index)
    {
        // Get the depth value of the current pixel
        auto pixels_distance = depth_scale * p_depth_frame[depth_pixel_index];

        // Check if the depth value is invalid (<=0) or greater than the threshold
        if (pixels_distance <= 0.f || pixels_distance > clipping_dist)
        {
            p_depth_frame[depth_pixel_index] = 0;
        }
    }
}

KinFu is able to run on the GPU to improve performance. Since we perform depth processing and rendering on the CPU, copying frames from GPU to CPU and vice versa is required. Running on CPU alone using OpenCV's Mat instead of UMat is also possible.

// Define matrices on the GPU for KinFu's use
UMat points;
UMat normals;
// Copy frame from CPU to GPU
Mat f(h, w, CV_16UC1, (void*)d.get_data());
UMat frame(h, w, CV_16UC1);
f.copyTo(frame);
f.release();

Here we actually run KinFu. The algorithm attemtps to fuse the current depth frame with the existing scene. If it fails, the model obtained so far is reset and reconstruction of a new scene begins.

// Run KinFu on the new frame(on GPU)
if (!kf->update(frame))
{
    kf->reset(); // If the algorithm failed, reset current state

If after_reset is true, there is no pointcloud available (the scene was reset in the current iteration). Therefore, we can safely try to acquire the new pointcloud only when after_reset is false. getCloud stores the pointcloud's points and normals in OpenCV's UMat objects, and we'll use them for rendering.

if (!after_reset)
{
    kf->getCloud(points, normals);
    after_reset = false;
}

The main thread handles the rendering of the pointcloud, using the functions colorize_pointcloud, which assigns an RGB value for each pixel based on it's depth, and draw_kinfu_pointcloud, which renders the pointcloud using OpenGL.

The function export_to_ply is also available and creates a PLY file containing the pointcloud, using colorized points and normals.