kinectRef2IMPEPRef.cc

//C++ related includes.
#include <cstdio>
#include <cmath>

//ROS related includes.
#include <ros/ros.h>
#include <sensor_msgs/PointCloud2.h>
#include <tf/transform_listener.h>

//PCL related includes.
#include <pcl/ros/conversions.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl_ros/transforms.h>

//OpenCV related includes.
#include <cv_bridge/cv_bridge.h>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/highgui/highgui.hpp>

//Self defined includes.
#include "IMPEP/PointDTPRGB.hh"

//Debug defines.
//#define DEBUG_CLOUD_TF

//Function declarations.
void tfPointCloud(const sensor_msgs::PointCloud2ConstPtr & );

//Global variable declarations.
ros::Publisher pub_pc; //point cloud
ros::Publisher pub_di; //depth image
ros::Publisher pub_sc; //spherical coordinates
tf::TransformListener *tf_list;

int main(int argc, char *argv[])
{
    //The node's name.
    std::string name = "kinectRef2IMPEPref";

    //Initialize ROS.
    ros::init(argc, argv, name);
    ros::NodeHandle node;

    //Initialize the Transform Listener.
    tf_list = new tf::TransformListener();

    //Subscribe to /camera/depth_registered/points (registered point cloud).
    ros::Subscriber sub = node.subscribe<sensor_msgs::PointCloud2>("/camera/depth_registered/points", 1000, tfPointCloud);

    //Publish to '/camera/impep_depth_registered/points' (point cloud registered
    //to IMPEP's referential).
    pub_pc = node.advertise<sensor_msgs::PointCloud2>("/camera/impep_depth_registered/points", 1000);

    //Publish to '/camera/impep_depth_registered/image' (depth image registered
    //to IMPEP's referential).
    pub_di = node.advertise<sensor_msgs::Image>("/camera/impep_depth_registered/image", 1000);

    //Publish to '/camera/impep_depth_registered/points_spherical' (point cloud
    //using spherical coordinates registered to IMPEP's referential).
    pub_sc = node.advertise<sensor_msgs::PointCloud2>("/camera/impep_depth_registered/points_spherical", 1000);

    ros::spin();

    delete tf_list;

    return 0;
}

////
// This function reads point clouds from the '/camera/depth_registered/points'
// topic, transforms them from Kinect's referential to IMPEP's referential
// and publishes the new point clouds to '/camera/impep_depth_registered/points'.
// Then, it uses this new point clouds to generate a new depth image and publish
// it to '/camera/impep_depth_registered/image'.
// It also uses the new point clouds to generate a point cloud using spherical
// coordinates instead of cartesian coordinates.
void tfPointCloud(const sensor_msgs::PointCloud2ConstPtr& msg)
{
    bool hasTF = false;

    ////
    // Transform point cloud.
    sensor_msgs::PointCloud2 out_pc = sensor_msgs::PointCloud2();

    while(!hasTF)
        hasTF = tf_list->waitForTransform("/camera_rgb_optical_frame", "/impep_frame", ros::Time::now(), ros::Duration(3.0));
    pcl_ros::transformPointCloud("/impep_frame", *msg, out_pc, *tf_list);

    //Publish point cloud to '/camera/impep_depth_registered/points'.
    pub_pc.publish(out_pc);


#ifdef DEBUG_CLOUD_TF
    //http://answers.ros.org/question/10947/reading-float-from-float32-data-from-kinect-pointcloud2/?answer=16165#post-id-16165

    pcl::PointCloud< pcl::PointXYZ > pc2;

    pcl::fromROSMsg(*msg, pc2);
    std::cout << "[BEFORE] (x,y,z) = " << pc2.points[0] << std::endl;

    pcl::fromROSMsg(out, pc2);
    std::cout << "[AFTER] (x,y,z) = " << pc2.points[0] << std::endl;
#endif

    ////
    // Use new point cloud to generate depth image.
    sensor_msgs::Image out_di;
    pcl::PointCloud< pcl::PointXYZRGB > pc;

    out_di = sensor_msgs::Image();
    pcl::fromROSMsg(*msg, pc);

    cv::Mat img = cv::Mat(480, 640, CV_32FC1);

    //Create each pixel from the coordinates of each point in the cloud.
    for(unsigned int i=0; i < pc.height; i++)
    {
        for(unsigned int j=0; j < pc.width; j++)
        {
            int index = i*pc.width + j;

            int x = pc.points[index].x;
            int y = pc.points[index].y;
            int z = pc.points[index].z;

            int depth = sqrt(x*x + y*y + z*z);
            img.at<float>(i, j) = depth;
        }
    }

    //Convert the 'cv::Mat' to 'sensor_msgs::Image'.
    cv_bridge::CvImage cv_di = cv_bridge::CvImage(out_di.header, "32FC1", img);
    cv_di.toImageMsg(out_di);

    //Publish new depth image.
    pub_di.publish(out_di);

    ////
    // Use new point cloud to generate point cloud with
    // spherical coordinates.
    sensor_msgs::PointCloud2 out_sc;
    pcl::PointCloud< PointDTPRGB > pc_sc;

    //Point cloud has same characteristics as the received one.
    pc_sc.header = pc.header;
    pc_sc.width = pc.width;
    pc_sc.height = pc.height;
    pc_sc.is_dense = pc.is_dense;
    pc_sc.sensor_origin_ = pc.sensor_origin_;
    pc_sc.sensor_orientation_ = pc.sensor_orientation_;

    pc_sc.points = std::vector< PointDTPRGB, Eigen::aligned_allocator< PointDTPRGB > >(pc.height * pc.width);

    //Convert each point from cartesian to spherical coordinates.
    for(unsigned int i=0; i < pc.height; i++)
    {
        for(unsigned int j=0; j < pc.width; j++)
        {
            int index = i*pc.width + j;

            int x = pc.points[index].x;
            int y = pc.points[index].y;
            int z = pc.points[index].z;

            pc_sc.points[index].d = sqrt(x*x + y*y + z*z);
            if(pc_sc.points[index].d == 0)
                pc_sc.points[index].t = 0;
            else
                pc_sc.points[index].t = acos(z / pc_sc.points[index].d);
            if(x == 0)
                pc_sc.points[index].p = 0;
            else
                pc_sc.points[index].p = atan(y / x);

            pc_sc.points[index].rgb = pc.points[index].rgb;
        }
    }

    //Convert from 'pcl::PointCloud' to 'sensor_msgs::PointCloud2'.
    pcl::toROSMsg(pc_sc, out_sc);

    //Publish new spherical coordinates point cloud.
    pub_sc.publish(out_sc);
}