point_cloud_to_camera.cpp

Go to the documentation of this file.

//#define USE_MT_NODE_HANDLE
#include <boost/bind.hpp>
#include <boost/format.hpp>
#include <cv_bridge/cv_bridge.h>
#include <image_transport/image_transport.h>
#include <limits>
#include <nodelet/nodelet.h>
#include <opencv2/opencv.hpp>
#include <pcl/io/io.h>
#include <pluginlib/class_list_macros.h>
#include <ros/ros.h>
#include <sensor_msgs/CameraInfo.h>
#include <sensor_msgs/image_encodings.h>
#include <sensor_msgs/PointCloud2.h>
#include <tf/transform_datatypes.h>
#include <tf/transform_listener.h>

#include <deque>          // std::deque
#include <list>           // std::list

#define NO_EXTTYPE
#include <stdio.h>
#include "dynamic/num_array.h"

int getFieldIndex(const sensor_msgs::PointCloud2 &cloud, const std::string fieldName) {
    for (size_t i = 0; i < cloud.fields.size(); i++) {
        if (cloud.fields[i].name == fieldName) {
            return i;
        }
    }
    return -1;
}

cv::Mat rotationFromTransform(const tf::Transform &t) {
    return (cv::Mat_<double>(3, 3) << t.getBasis()[0][0], t.getBasis()[0][1], t.getBasis()[0][2],
            t.getBasis()[1][0], t.getBasis()[1][1], t.getBasis()[1][2],
            t.getBasis()[2][0], t.getBasis()[2][1], t.getBasis()[2][2]);
}

cv::Mat translationFromTransform(const tf::Transform &t) {
    return (cv::Mat_<double>(3, 1) << t.getOrigin()[0], t.getOrigin()[1], t.getOrigin()[2]);
}

cv::Mat matFromCloud(sensor_msgs::PointCloud2ConstPtr cloud, const std::string fieldName, const int numElements) {
    const int numPoints = cloud->width * cloud->height;
    int fieldIndex = getFieldIndex(*cloud, fieldName);
    void *data = const_cast<void *>(static_cast<const void *>(&cloud->data[cloud->fields[fieldIndex].offset]));
    int matType;
    switch (cloud->fields[fieldIndex].datatype) {
    case sensor_msgs::PointField::FLOAT32:
        matType = CV_32FC1;
        break;
    case sensor_msgs::PointField::FLOAT64:
        matType = CV_64FC1;
        break;
    case sensor_msgs::PointField::UINT8:
        matType = CV_8UC1;
        break;
    default:
        assert (0);
        break;
    }
    return cv::Mat(numPoints, numElements, matType, data, cloud->point_step);
}
/*
sensor_msgs::PointCloud2Ptr createCloudWithColorFrom(const sensor_msgs::PointCloud2ConstPtr inCloud) {
  sensor_msgs::PointField rgbField;
//  rgbField.name = "argb";
//  rgbField.datatype = sensor_msgs::PointField::UINT32;
  rgbField.name = "rgb";
  rgbField.datatype = sensor_msgs::PointField::FLOAT32;
  rgbField.count = 1;
  rgbField.offset = 0;
  sensor_msgs::PointCloud2 rgbCloud;
  rgbCloud.header = inCloud->header;
  rgbCloud.width = inCloud->width;
  rgbCloud.height = inCloud->height;
  rgbCloud.fields.push_back(rgbField);
  rgbCloud.is_bigendian = inCloud->is_bigendian;
  rgbCloud.is_dense = inCloud->is_dense;
  rgbCloud.point_step = 4;
  rgbCloud.row_step = rgbCloud.width * rgbCloud.point_step;
  rgbCloud.data.resize(4 * rgbCloud.width * rgbCloud.height, 0);
  sensor_msgs::PointCloud2Ptr outCloud(new sensor_msgs::PointCloud2);
  pcl::concatenateFields(*inCloud, rgbCloud, *outCloud);
  return outCloud;
}*/

class PointCloudImage : public nodelet::Nodelet {
public:
    PointCloudImage();
    virtual ~PointCloudImage();
    void onInit();
private:
    tf::TransformListener transformListener;
    ros::Subscriber cameraSubscriber;
    image_transport::Publisher imagePublisher;
    ros::Subscriber pointCloudSub;
    dynamic::num_array<float,2> laser_image;
    std::string fixedFrame;
    std::string camera_topic;
    //std::queue<sensor_msgs::PointCloud2> clouds;
    typedef sensor_msgs::PointCloud2ConstPtr tcloudptr;
    std::deque<tcloudptr> clouds;
    typedef std::deque<tcloudptr>::iterator clouds_it;
    double maxCloudAge;
    int pointCloudQueueSize;
    double waitForTransform;

    void pointCloudCallback(const sensor_msgs::PointCloud2ConstPtr &cloudMsg);
    void cameraInfoCallback(const sensor_msgs::CameraInfoConstPtr &cameraInfoMsg);
};

PointCloudImage::PointCloudImage() :
    transformListener(ros::Duration(10.0)),
    fixedFrame("/odom"),
    maxCloudAge(10.0),
    pointCloudQueueSize(200),
    waitForTransform(1.0) {
}

PointCloudImage::~PointCloudImage() {
}

void PointCloudImage::onInit() {
    NODELET_INFO("PointCloudImage::onInit: Initializing...");

#ifdef USE_MT_NODE_HANDLE
    ros::NodeHandle &nh = getMTNodeHandle();
    ros::NodeHandle &pnh = getMTPrivateNodeHandle();
#else
    ros::NodeHandle &nh = getNodeHandle();
    ros::NodeHandle &pnh = getPrivateNodeHandle();
#endif

    // Get and process parameters.
    pnh.param("fixed_frame", fixedFrame, fixedFrame);
    NODELET_INFO("PointCloudImage::onInit: Fixed frame: %s.", fixedFrame.c_str());
    pnh.param("point_cloud_queue_size", pointCloudQueueSize, pointCloudQueueSize);
    pointCloudQueueSize = pointCloudQueueSize >= 1 ? pointCloudQueueSize : 1;
    NODELET_INFO("PointCloudImage::onInit: Point cloud queue size: %i.", pointCloudQueueSize);
    pnh.param("wait_for_transform", waitForTransform, waitForTransform);
    waitForTransform = waitForTransform >= 0.0 ? waitForTransform : 0.0;
    NODELET_INFO("PointCloudImage::onInit: Wait for transform timeout: %.2f s.", waitForTransform);
    pnh.param("camera_topic", camera_topic, std::string("openni_camera"));
    NODELET_INFO("PointCloudImage::onInit: camera_topic: %s.", camera_topic.c_str());

    // Subscribe list of camera topics.
    image_transport::ImageTransport it(nh);
    //std::string cameraTopic = nh.resolveName((boost::format("camera_%i") % iCam).str(), true);
    //NODELET_INFO("PointCloudImage::onInit: Subscribing camera %s.", cameraTopic.c_str());
    //cameraSubscribers[iCam] = it.subscribeCamera(cameraTopic, imageQueueSize, (boost::bind(&PointCloudImage::cameraCallback, this, _1, _2, iCam)));
    cameraSubscriber = nh.subscribe(camera_topic+"/camera_info", 1, &PointCloudImage::cameraInfoCallback,this);
    imagePublisher = it.advertise(camera_topic+"/laser_image", 5);

    // Subscribe point cloud topic.
    std::string pclInTopic = nh.resolveName("pcl_in", true);
    NODELET_INFO("PointCloudImage::onInit: Subscribing point cloud %s.", pclInTopic.c_str());
    pointCloudSub = nh.subscribe<sensor_msgs::PointCloud2>(pclInTopic, pointCloudQueueSize+10, &PointCloudImage::pointCloudCallback, this);

}

void PointCloudImage::cameraInfoCallback(const sensor_msgs::CameraInfoConstPtr &cameraInfoMsg) {
    NODELET_DEBUG("PointCloudImage::cameraCallback: camera info frame %s.", cameraInfoMsg->header.frame_id.c_str());

    laser_image.resize(exttype::mint2(cameraInfoMsg->width,cameraInfoMsg->height));


    for(clouds_it pcloud=clouds.begin(); pcloud!=clouds.end(); ++pcloud) {
        const sensor_msgs::PointCloud2ConstPtr cloudMsg = *pcloud;

        const std::string cameraFrame = cameraInfoMsg->header.frame_id;
        const std::string cloudFrame = cloudMsg->header.frame_id;

        if (!transformListener.waitForTransform(cameraFrame, cameraInfoMsg->header.stamp, // target frame and time
                                                cloudFrame, cloudMsg->header.stamp, // source frame and time
                                                fixedFrame, ros::Duration(waitForTransform))) {
            NODELET_WARN("PointCloudImage::pointCloudCallback: Could not transform point cloud from frame %s to camera frame %s. Skipping the image...",
                         cloudFrame.c_str(), cameraFrame.c_str());
            continue;
        }
        tf::StampedTransform cloudToCamStamped;
        transformListener.lookupTransform(cameraFrame, cameraInfoMsg->header.stamp, // target frame and time
                                          cloudFrame, cloudMsg->header.stamp, // source frame and time
                                          fixedFrame, cloudToCamStamped);
        cv::Mat camRotation = rotationFromTransform(cloudToCamStamped);
        cv::Mat camTranslation = translationFromTransform(cloudToCamStamped);
        cv::Mat objectPoints;
        matFromCloud(cloudMsg, "x", 3).convertTo(objectPoints, CV_64FC1);
        // Rigid transform with points in rows: X' = X * R + t.
        objectPoints = objectPoints * camRotation.t() + cv::repeat(camTranslation.t(), objectPoints.rows, 1);
        cv::Mat cameraMatrix(3, 3, CV_64FC1, const_cast<void *>(reinterpret_cast<const void *>(&cameraInfoMsg->K[0])));
        cv::Mat distCoeffs(1, 5, CV_64FC1, const_cast<void *>(reinterpret_cast<const void *>(&cameraInfoMsg->D[0])));

        cv::Mat imagePoints;
        cv::projectPoints(objectPoints.reshape(3), cv::Mat::zeros(3, 1, CV_64FC1), cv::Mat::zeros(3, 1, CV_64FC1), cameraMatrix, distCoeffs, imagePoints);

        int numPoints = objectPoints.rows*objectPoints.cols;
        for (int iPoint = 0; iPoint < numPoints; iPoint++) {
            const double z = objectPoints.at<double>(iPoint, 2);
            // Skip points behind the camera.
            if (z <= 0.0) {
                continue;
            }
            const cv::Vec2d pt = imagePoints.at<cv::Vec2d>(iPoint, 0);
            const double x = round(pt.val[0]);
            const double y = round(pt.val[1]);
            // Skip points outside the image.
            if (x < 0.0 || y < 0.0 || x >= static_cast<double>(cameraInfoMsg->width) || y >= static_cast<double>(cameraInfoMsg->height)) {
                continue;
            }
            // Apply static mask with image ROI to be used for coloring.
            const int yi = static_cast<int>(y);
            const int xi = static_cast<int>(x);
            laser_image(xi,yi) = z;
        }
    }
    // publish image here
    {
        sensor_msgs::ImagePtr msg(new sensor_msgs::Image());
        msg->header = cameraInfoMsg->header;
        int num_bytes = laser_image.byte_size();
        msg->data.resize(num_bytes);
        memcpy(&msg->data[0],laser_image.begin(),num_bytes);
        msg->encoding = sensor_msgs::image_encodings::TYPE_32FC1;
        msg->step = laser_image.byte_stride(0,1);
        msg->height = laser_image.size()[1];
        msg->width = laser_image.size()[0];
        //msg->header.seq = count;
        imagePublisher.publish(msg);
    }
}

void PointCloudImage::pointCloudCallback(const sensor_msgs::PointCloud2ConstPtr &cloudMsg) {
    const int numPoints = cloudMsg->width * cloudMsg->height;
    const std::string cloudFrame = cloudMsg->header.frame_id;
    if (numPoints == 0 || cloudMsg->data.size() == 0) {
        NODELET_WARN("PointCloudImage::pointCloudCallback: Skipping empty point cloud in frame %s.", cloudFrame.c_str());
        return;
    }
    if(clouds.size() >=  pointCloudQueueSize) {
        clouds.pop_back();
    };
    //sensor_msgs::PointCloud2 cloud;
    clouds.push_front(cloudMsg);
    //sensor_msgs::PointCloud2Ptr outCloud = createCloudWithColorFrom(cloudMsg);
    //cv::Mat rgbMat = matFromCloud(outCloud, "rgb", 1);
    //rgbMat.setTo(defaultColor);
    // Initialize vector with projection distances from image center, used as a quality measure.
    //std::vector<float> dist(numPoints, std::numeric_limits<float>::infinity());
    //pointCloudPub.publish(outCloud);
}

/* namespace point_cloud_color */

//PLUGINLIB_DECLARE_CLASS(point_cloud_color, point_cloud_color, point_cloud_color::PointCloudImage, nodelet::Nodelet)


int main(int argc, char **argv) {
    ros::init(argc, argv, "bla");
    PointCloudImage node;
    ros::spin();
}