Range image generation for live kinect data in PCL (point cloud library)

Question

This is a PCL (Point cloud library) specific question. I am trying to display a range image for a live data stream from Kinect. I have been able to read the point cloud from Kinect and display the point cloud. Generating the range image however has been very painful. I have read the examples in PCL documentation and have used the same code as there. The range image being generated is very small and all NANs. showRangeImage function seems to hang (I have hence commented this call out). The 3D Viewer code from the PCL example gives an error in the vtkOutputWindow.

I am not sure what the sensorPose input should be for the createFromPointCloud function. I am currently using Eigen::Affine3f sensorPose = (Eigen::Affine3f)Eigen::Translation3f(0.0f, 0.0f, 0.0f); I dont know if that is causing the error.

I also tried to read from a pcd file on disk (the relevant code is in an else block in the program below). I have tried to use the same code and in the example in PCL documentation. Reading a pcd file from disk does not seem to work either.

Any suggestions will be greatly appreciated.

#include<pcl/point_cloud.h>
#include<pcl/point_types.h>
#include<pcl/io/openni_grabber.h>
#include<pcl/common/time.h>
#include<pcl/visualization/pcl_visualizer.h>    
#include<pcl/point_types.h>
#include<pcl/visualization/cloud_viewer.h>
#include<pcl/range_image/range_image.h>
#include<pcl/visualization/range_image_visualizer.h>
#include<pcl/io/pcd_io.h>

typedef pcl::PointXYZRGBA PointT;
typedef pcl::PointXYZ PointType;

pcl::visualization::CloudViewer viewer("PCL Viewer");
pcl::visualization::RangeImageVisualizer range_image_widget("Range image");
pcl::visualization::PCLVisualizer viewer3D ("3D Viewer");

//From PCL documentation example code
void 
setViewerPose (pcl::visualization::PCLVisualizer& viewer, const Eigen::Affine3f&         viewer_pose)
{
Eigen::Vector3f pos_vector = viewer_pose * Eigen::Vector3f(0, 0, 0);
Eigen::Vector3f look_at_vector = viewer_pose.rotation () * Eigen::Vector3f(0, 0, 1) + pos_vector;
Eigen::Vector3f up_vector = viewer_pose.rotation () * Eigen::Vector3f(0, -1, 0);
viewer.setCameraPose (pos_vector[0], pos_vector[1], pos_vector[2],
    look_at_vector[0], look_at_vector[1], look_at_vector[2],
    up_vector[0], up_vector[1], up_vector[2]);
}

//Call back method
void cloud_cb_ (const pcl::PointCloud<pcl::PointXYZRGBA>::ConstPtr &cloud)

{
static unsigned count = 0;
static double last = pcl::getTime ();
if (++count == 30)
{
    double now = pcl::getTime ();
    std::cout << "distance of center pixel :" << cloud->points [(cloud->width >> 1) * (cloud->height + 1)].z << " mm. Average framerate: " << double(count)/double(now - last) << " Hz" <<  std::endl;
    count = 0;
    last = now;
}
// PCL viewer //
// Display pointcloud:
viewer.showCloud (cloud);

//DO USEFUL OPERATIONS HERE
//Create a range image and display it

// We now want to create a range image from the above point cloud, with a 1deg angular resolution
float angularResolution = (float) (  1.0f * (M_PI/180.0f));  //   1.0 degree in radians
float maxAngleWidth     = (float) (360.0f * (M_PI/180.0f));  // 360.0 degree in radians
float maxAngleHeight    = (float) (180.0f * (M_PI/180.0f));  // 180.0 degree in radians
Eigen::Affine3f sensorPose = (Eigen::Affine3f)Eigen::Translation3f(0.0f, 0.0f, 0.0f);
pcl::RangeImage::CoordinateFrame coordinate_frame = pcl::RangeImage::CAMERA_FRAME;
float noiseLevel=0.00;
float minRange = 0.0f;
int borderSize = 1;

boost::shared_ptr<pcl::RangeImage> range_image_ptr(new pcl::RangeImage);
pcl::RangeImage& rangeImage = *range_image_ptr; 

//Range image for live stream from Kinect
if(1){
rangeImage.createFromPointCloud(*cloud, angularResolution, maxAngleWidth, maxAngleHeight,
    sensorPose, coordinate_frame, noiseLevel, minRange, borderSize);
}

//Alternative test - from PCD file on disk
else{
    pcl::PointCloud<PointType>::Ptr point_cloud_ptr (new pcl::PointCloud<PointType>);
    pcl::PointCloud<PointType>& point_cloud = *point_cloud_ptr;

    //pcd file from http://download.ros.org/data/pcl/
    if (pcl::io::loadPCDFile ("src\\office_scene.pcd", point_cloud) == -1)
        std::cout<<"Cannot load scene file\n";
    Eigen::Affine3f scene_sensor_pose (Eigen::Affine3f::Identity ());
    scene_sensor_pose = Eigen::Affine3f (Eigen::Translation3f (point_cloud.sensor_origin_[0],
        point_cloud.sensor_origin_[1],
        point_cloud.sensor_origin_[2])) *
        Eigen::Affine3f (point_cloud.sensor_orientation_);

    rangeImage.createFromPointCloud(point_cloud, angularResolution, maxAngleWidth, maxAngleHeight,
        scene_sensor_pose, coordinate_frame, noiseLevel, minRange, borderSize);
}

std::cout << rangeImage << "\n";

//showRangeImage seems to take a very long time (infinite loop?). Hence commented out
//range_image_widget.showRangeImage(rangeImage);

//viewer3D gives error
viewer3D.setBackgroundColor (1, 1, 1);
pcl::visualization::PointCloudColorHandlerCustom<pcl::PointWithRange> range_image_color_handler (range_image_ptr, 0, 0, 0);
viewer3D.addPointCloud (range_image_ptr, range_image_color_handler, "range image");
viewer3D.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "range image");
viewer3D.initCameraParameters ();
setViewerPose(viewer3D, rangeImage.getTransformationToWorldSystem ());


}

int main ()
{
// create a new grabber for OpenNI devices
pcl::Grabber* interface = new pcl::OpenNIGrabber();

// make callback function from member function
boost::function<void (const pcl::PointCloud<pcl::PointXYZRGBA>::ConstPtr&)> f =
    boost::bind (&cloud_cb_, _1);

// connect callback function for desired signal. In this case its a point cloud with color values
boost::signals2::connection c = interface->registerCallback (f);

// start receiving point clouds
interface->start ();

// wait until user quits program with Ctrl-C, but no busy-waiting -> sleep (1);
while (true)
    boost::this_thread::sleep (boost::posix_time::seconds (1));

// stop the grabber
interface->stop ();

    return (0);
}`

I think you should use RangeImagePlanar instead of RangeImage. — Simson

Vitaliy Prooks Vitaliy Prooks · Accepted Answer · 2015-02-16T16:01:53

mnut, I found a way to solve your problem. So the range image you get is too small because you set angular parameters wrong. The correct way is to use specifications of Kinect sensor Specs for two lines below (refer to https://msdn.microsoft.com/en-us/library/jj131033.aspx):

float maxAngleWidth = (float) (57.0f * (M_PI / 180.0f));
float maxAngleHeight = (float) (43.0f * (M_PI / 180.0f));
float angularResolution = (float)(57.0f / 640.0f * (M_PI/180.0f));

The numbers 57 and 43 are viewing angles of the field of view, and thus angular resolution is amount of pixels per one degree, so you should divide 57 by 640 or 43 by 480.

For sensorPose I used the following value (which is default):

Eigen::Affine3f sensorPose = Eigen::Affine3f::Identity();

Finally, in order to display the resulting range image, use following:

range_image_widget.showRangeImage(rangeImage);
range_image_widget.spinOnce();  // This line was missing

But the result you will get is far from being satisfying, because frame rate will be quite small. See the following video for demonstration: http://youtu.be/iao3BeI4LqM

And viewer3D is not suited to display rangeImages.

Range image generation for live kinect data in PCL (point cloud library)

1 Answers