New 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"


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

//Global variables.
bool newPC = false;
sensor_msgs::PointCloud2 in_pc;
sensor_msgs::PointCloud2 out_pc;

ros::Publisher pub_pc; //point cloud
ros::Publisher pub_di; //depth image
ros::Publisher pub_sc; //spherical coordinates
tf::TransformListener *tfListener;

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.
    tfListener = 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);


    while(ros::ok())
    {
        ros::spinOnce();

        //If no new Point Cloud arrived, spinOnce again.
        if(newPC == false)
            continue;

        //Transform point cloud.
        transformPC();

        //Generate and publish depth image and spherical coordinates point cloud.
        generateDepthAndSpherical();

        newPC = false;
    }

    delete tfListener;

    return 0;
}

////
// This function reads point clouds from the '/camera/depth_registered/points'
// topic and saves them to the variable 'in_pc'.
void tfPointCloud(const sensor_msgs::PointCloud2ConstPtr& msg)
{
    in_pc = sensor_msgs::PointCloud2(*msg);
    newPC = true;
}

////
// Transforms the point cloud 'in_pc' from Kinect's referential to IMPEP's referential
// and publishes the new point clouds to '/camera/impep_depth_registered/points'.
void transformPC()
{
    //Transform point cloud.
    bool hasTF = false;
    while(!hasTF)
        hasTF = tfListener->waitForTransform("/camera_rgb_optical_frame", "/impep_frame", ros::Time::now(), ros::Duration(3.0));
    pcl_ros::transformPointCloud("/impep_frame", in_pc, out_pc, *tfListener);

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

////
// Using the transformed point cloud, generate a new depth image, published
// to '/camera/impep_depth_registered/image', and a point cloud using spherical
// coordinates instead of cartesian coordinates, published to
// '/camera/impep_depth_registered/points_spherical'.
void generateDepthAndSpherical()
{
    ////
    // Depth image initializations.
    sensor_msgs::Image out_di;
    pcl::PointCloud< pcl::PointXYZRGB > pc;

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

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

    ////
    // Point cloud with spherical coordinates initializations.
    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);

    ////
    // Create each pixel from the coordinates of each point in the cloud
    // and 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;

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

            //Spherical coordinates
            pc_sc.points[index].d = depth;
            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 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);

    //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);
}