Introducing Kinect Fusion as a service

[PeterBowman]

OpenCV 4.0 (mid 2019) features an implementation of the Kinect Fusion algorithm (original paper (2011)). This is a rework of https://github.com/Nerei/kinfu_remake, which is itself a rework of PCL's KinFu. Following new OpenCV's guidelines, cv::kinfu::KinFu exploits OpenCL hardware acceleration instead of relying on CUDA alone, thus reaching wider graphics card audiences.

In OpenCV 4.2 (Ubuntu Focal), cv::dynafu::DynaFu is introduced to target the Dynamic Fusion enhancement (paper (2015), YouTube) for non-rigid scenes. Current master branch (4.5.0-dev) additionally features Kinect Fusion for large scale environments (cv::large_kinfu::LargeKinfu).

These implementations are bundled in the "rgbd" module of https://github.com/opencv/opencv_contrib. It is important to note the original algorithm is patented, therefore OpenCV must be compiled with the OPENCV_ENABLE_NONFREE option.

[PeterBowman]

KinFu accepts a decent set of configuration parameters encoded in the cv::kinfu::Params structure. OpenCV 4.2 introduces Params::truncateThreshold, while 4.4 adds an alternative configurable internal volume representation (hash-TSDF).

DynaFu parameters are mostly the same as those used by KinFu. Current master branch removes cv::dynafu::Params in favor of cv::kinfu::Params, which I reported at opencv/opencv_contrib#2619 (comment) since back-compat is currently broken.

[PeterBowman]

Work is almost done, I'm writing down extra info here for doc purposes. KinFu performs nicely, but DynaFu throws an assertion error on my machine: opencv/opencv_contrib#2528 (comment) (note OpenGL is a requirement for DynaFu, I had to apt install libgtkglext1-dev). Also, I'm not implementing Large Scale KinFu for now until the next OpenCV release is ready, mostly because of the WIP state the cv::dynafu::Params problem suggests (see previous comment).

[PeterBowman]

Since this is not documented anywhere, let me stress that OpenCV expects depth frames storing pixel floats that express distances in millimeters. On the other hand, YARP prefers meters.

[PeterBowman]

Note regarding cv::kinfu::KinFu::render:

vision/programs/sceneReconstruction/KinFu.cpp

Lines 139 to 145 in d9d0d07

	void render(yarp::sig::ImageOf<yarp::sig::PixelRgb> & image) const override
	{
	cv::UMat umat;
	handle->render(umat);
	cv::Mat mat = umat.getMat(cv::ACCESS_RW);
	image = yarp::cv::fromCvMat<yarp::sig::PixelRgb>(mat);
	}

The umat stores an 8UC4 image, that is, a 4-channel Phong-rendered representation of the reconstructed surface. Even though yarp::cv::fromCvMat assumes the input CV image is in the BGR color space (ref), cv::cvtColor knows how to cope with the mismatch between color identifiers (i.e. CV_BGR2RGB instead of CV_BGRA2RGB): implementation (mind that COLOR_RGB2BGR is equivalent to COLOR_BGR2RGB, ref).

Anyway, this Phong render is always grayscale. Therefore, I'd rather convert to yarp::sig::PixelMono and thus significantly reduce bandwidth (while also increasing performance): a1794ec.

vision/programs/sceneReconstruction/KinFu.cpp

Lines 139 to 146 in a1794ec

	void render(yarp::sig::ImageOf<yarp::sig::PixelMono> & image) const override
	{
	cv::UMat umat;
	handle->render(umat);
	cv::Mat mat = umat.getMat(cv::ACCESS_FAST); // no memcpy
	const auto & temp = yarp::cv::fromCvMat<yarp::sig::PixelBgra>(mat); // no conversion
	image.copy(temp); // bgra to grayscale (single step convert+assign)
	}

[PeterBowman]

[PeterBowman]

The following example app queries the point cloud (also with normals) previously generated by this service via manually issued RPC commands ([rsm] + [paus]), then saves them to file in PCD format (PCL's data storage):

cmake_minimum_required(VERSION 3.12 FATAL_ERROR)
project(scene-pc-client LANGUAGES CXX)
find_package(YARP 3.4 REQUIRED os pcl sig)
add_executable(scene-pc-client main.cpp)
target_link_libraries(scene-pc-client YARP::YARP_os YARP::YARP_init YARP::YARP_sig YARP::YARP_pcl ${PCL_LIBRARIES})

#include <yarp/os/LogStream.h>
#include <yarp/os/Network.h>
#include <yarp/os/RpcClient.h>
#include <yarp/os/Vocab.h>
#include <yarp/sig/PointCloud.h>
#include <yarp/pcl/Pcl.h>

constexpr auto VOCAB_GET_POINTS = yarp::os::createVocab('g','p','c');
constexpr auto VOCAB_GET_POINTS_AND_NORMALS = yarp::os::createVocab('g','p','c','n');

int main()
{
    yarp::os::Network yarp;
    yarp::os::RpcClient rpc;

    if (!rpc.open("/sceneClient/rpc:c") || !yarp::os::Network::connect(rpc.getName(), "/sceneReconstruction/rpc:s"))
    {
        yError() << "unable to establish communication via" << rpc.getName();
        return 1;
    }

    yarp::os::Bottle cmd1 {yarp::os::Value(VOCAB_GET_POINTS, true)};
    yarp::sig::PointCloudXYZ cloud;

    if (!rpc.write(cmd1, cloud))
    {
        yError() << "unable to send first command";
        return 1;
    }

    yInfo() << "got cloud of" << cloud.size() << "points";
    yarp::pcl::savePCD<yarp::sig::DataXYZ, pcl::PointXYZ>("cloud.pcd", cloud);

    yarp::os::Bottle cmd2 {yarp::os::Value(VOCAB_GET_POINTS_AND_NORMALS, true)};
    yarp::sig::PointCloudXYZNormal cloudWithNormals;

    if (!rpc.write(cmd2, cloudWithNormals))
    {
        yError() << "unable to send second command";
        return 1;
    }

    yInfo() << "got cloud of" << cloudWithNormals.size() << "points with normals";
    yarp::pcl::savePCD<yarp::sig::DataXYZNormal, pcl::PointNormal>("cloudWithNormals.pcd", cloudWithNormals);

    return 0;
}

Note PCD files can be inspected with pcl_viewer and converted to PLY (supported by Meshlab) via pcl_pcd2ply. These utilities can be obtained via apt install pcl-tools.

[PeterBowman]

Done at 44c8df7. The list of RPC commands might be enhanced with a save_to_file command (either PLY or PCD format), which will be addressed in another issue if deemed convenient.

[PeterBowman]

Found by @jgvictores:

• https://rosindustrial.org/news/2019/8/7/yak-3d-reconstruction-in-ros2
• https://github.com/ros-industrial/yak

Probably quite equivalent to OpenCV's KinFu (and released two years earlier), also uses TSDF volumes.

[PeterBowman]

For future reference, OpenCV's rgbd module provides a set of utilities that could render (pun not intended) useful in other applications: docs. Notes:

• cv::rgbd::depthTo3d: same as yarp::sig::utils::depthToPC
• cv::rgbd::registerDepth: same as yarp::sig::utils::depthRgbToPC

[PeterBowman]

Large-scale KinFu implemented at 01598b6. Note OpenCV 4.5.1 is required.

[PeterBowman]

OpenCV 4.5.3 features colored KinFu: cv::colored_kinfu::ColoredKinFu.

[PeterBowman]

OpenCV 4.5.3 features colored KinFu: cv::colored_kinfu::ColoredKinFu.

Implemented at our end: 26a3a68. However, the upstream algorithm is not finished yet as color+depth frame integration is still a WIP and any attempt to run this algorithm will result in a "Not implemented" error (ref). At least we are prepared for future OpenCV releases with a working implementation. Renders are now forwarded via YARP ports as RGB instead of PixelMono (8-bit), and color frames along with intrinsics and frame sizes are conveniently queried if requested.

Edit: my bad, forgot to set --KINECT_FUSION::volumeType coloredtsdf, which does have a working implementation.

[PeterBowman]

OpenCV 4.5.5 (released a few days ago) also features colored point clouds, in addition to colored renders. I have implemented this on our side at 4cd9ed7 (the example has also learned to export colored clouds). It kinda works, but OpenCV does not support GPU colored TSDFs at the time of writing. In result, severely degraded performance can be observed and the algorithm resets on every few frames. Remember to use --KINECT_FUSION::algorithm colored_kinfu --KINECT_FUSION::volumeType coloredtsdf --KINECT_FUSION::volumeDims "(32 32 32)" (default size was 512, but it's unbearable). In addition, --SETTINGS::alignmentFrame RGB might be necessary/beneficial in the sensor config.