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

#include <boost/shared_ptr.hpp>

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

#include <QtGui/QApplication>
#include <QtGui/QLabel>
#include <QtCore/QBasicTimer>
#include <QtGui/QPainter>
#include <QtGui/QDialog>
#include <QtGui/QMainWindow>
//#include <QtGui/QMessageBox>
#include <QtGui/QKeyEvent>
#include <QtGui/QMouseEvent>
//
//#include <GL/glut.h>

//#include "GL/gl.h"
#include "GL/glew.h"
#include "GL/freeglut.h"
#include <GL/glut.h>

#include "geom/nmmatrix.h"

using namespace exttype;

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

void glcheck() {
    GLenum errCode;
    const GLubyte *errString;
    if ((errCode = glGetError()) != GL_NO_ERROR) {
        errString = gluErrorString(errCode);
        fprintf (stderr, "OpenGL Error: %s, code %x\n", errString,errCode);
        fflush(stdout);
        exit(1);
    };
};

//_______________________________________________________________________________________

class PointCloudImage : public nodelet::Nodelet {
public:
    PointCloudImage();
    virtual ~PointCloudImage();
    void clear();
    void onInit();
public:
    QLabel * label;
    QImage img;
public:
    int Width;
    int Height;
    int count;
    ros::NodeHandle nh;
    ros::NodeHandle pnh;
    image_transport::ImageTransport * it;//(nh);
private:
    GLuint fbo, d_render_buf;
    float farVal;
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::deque<cv::Mat> w_clouds; //poit clouds mapped into fixed_frame
    typedef std::deque<cv::Mat>::iterator w_clouds_it;
    //
    //double maxCloudAge;
    int pointCloudQueueSize;
    double waitForTransform;
    void init_image(int width, int height);
    void pointCloudCallback(const sensor_msgs::PointCloud2ConstPtr &cloudMsg);
    void cameraInfoCallback(const sensor_msgs::CameraInfoConstPtr &cameraInfoMsg);
};

PointCloudImage::PointCloudImage() :
    nh(),
    pnh("~"),
    //transformListener(nh,ros::Duration(100.0),true),
    transformListener(ros::Duration(200.0),true),
    fixedFrame("/map"),
    //maxCloudAge(10.0),
    pointCloudQueueSize(500),
    waitForTransform(2),
    label(0),
    Width(0),Height(0),count(0),it(0) {
        farVal = 100;
}

PointCloudImage::~PointCloudImage() {
    clear();
};

void PointCloudImage::clear() {
    if(label) {
        delete label;
    };
    glDeleteFramebuffers(1,&fbo);
    glDeleteRenderbuffers(1,&d_render_buf);
};

void PointCloudImage::onInit() {
    NODELET_INFO("PointCloudImage::onInit: Initializing...");
    // 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"));
//    pnh.param("camera_frame", camera_frame, camera_topic);
//    pnh.param("laser_frame", laser_frame, std::string("/laser"));
    //NODELET_INFO("PointCloudImage::onInit: camera_topic: %s.", camera_topic.c_str());
    std::string pclInTopic;
    pnh.param("scan_point_cloud", pclInTopic, std::string("scan_point_cloud"));
    //
    cameraSubscriber = nh.subscribe(camera_topic+"/camera_info", 1, &PointCloudImage::cameraInfoCallback,this);
    std::cout<<"Subscribed "<<camera_topic+"/camera_info"<<"\n"; fflush(stdout);
    pointCloudSub = nh.subscribe<sensor_msgs::PointCloud2>(pclInTopic, 10, &PointCloudImage::pointCloudCallback, this);
    std::cout<<"Subscribed "<<pclInTopic<<"\n"; fflush(stdout);
    it = new image_transport::ImageTransport(nh);
    imagePublisher = it->advertise(camera_topic+"/laser_image", 5);
    std::cout<<"Advertised "<<camera_topic+"/laser_image"<<"\n"; fflush(stdout);
    label = new QLabel();
#ifndef DISPLAY
    label->setVisible(false);
#else
    label->show();
#endif
    label->setScaledContents(true);
}

void PointCloudImage::init_image(int width, int height) {
    Width = width;
    Height = height;
    img = QImage(Width, Height, QImage::Format_RGB32);
    label->resize(Width*2,Height*2);
    //
    glutInitWindowSize(Width,Height);
    int argc1=0;
    char *argv1[0];
    glutInit(&argc1, argv1);
    glutCreateWindow("GLEW window of the RIGHT size");
    glewExperimental=GL_TRUE;
    GLenum err = glewInit();
    if (GLEW_OK != err) {
        /* Problem: glewInit failed, something is seriously wrong. */
        fprintf(stderr, "Error: %s\n", glewGetErrorString(err));
        exit(1);
    };
    printf("Vendor: %s\n", glGetString (GL_VENDOR));
    printf("Renderer: %s\n", glGetString (GL_RENDERER));
    printf("Version: %s\n", glGetString (GL_VERSION));
    fflush(stdout);
    //
    glGenFramebuffers(1,&fbo);
    glGenRenderbuffers(1,&d_render_buf);
    glBindFramebuffer(GL_FRAMEBUFFER,fbo);
    //
    glBindRenderbuffer(GL_RENDERBUFFER,d_render_buf);
    glRenderbufferStorage(GL_RENDERBUFFER, GL_DEPTH_COMPONENT, Width, Height);
    glcheck();
    glFramebufferRenderbuffer(GL_FRAMEBUFFER, GL_DEPTH_ATTACHMENT, GL_RENDERBUFFER, d_render_buf);
    //
    GLenum e = glCheckFramebufferStatus(GL_FRAMEBUFFER);
    if(e!=GL_FRAMEBUFFER_COMPLETE) {
        //perror("Error: GL FrameBuffer, ");
        GLenum errCode;
        const GLubyte *errString;

        if ((errCode = glGetError()) != GL_NO_ERROR) {
            errString = gluErrorString(errCode);
            fprintf (stderr, "OpenGL Error: %s, %x, %x\n", errString,e,errCode);

            fflush(stdout);
        }
//       std::cout<<gluErrorString(e)<<"\n";
        exit(1);
    };
//
    glEnable(GL_DEPTH_TEST);
    glcheck();
    glClear(GL_DEPTH_BUFFER_BIT);
    glcheck();
    glDisable(GL_STENCIL_TEST);
};

void PointCloudImage::cameraInfoCallback(const sensor_msgs::CameraInfoConstPtr &cameraInfoMsg) {
    //
    bool laser_empty = false;
    ++count;
    if(Width!=cameraInfoMsg->width) {
        laser_image.resize(exttype::mint2(cameraInfoMsg->width,cameraInfoMsg->height));
        init_image(cameraInfoMsg->width,cameraInfoMsg->height);
    };


    std::string err;
    transformListener.waitForTransform(cameraInfoMsg->header.frame_id, cameraInfoMsg->header.stamp, // target frame and time
                                       fixedFrame, cameraInfoMsg->header.stamp, // source frame and time
                                       fixedFrame, ros::Duration(0.01)); //    this is time critical, dont wait

    tf::StampedTransform tf1;
    try {
        transformListener.lookupTransform(cameraInfoMsg->header.frame_id, cameraInfoMsg->header.stamp, // target frame and time
                                          fixedFrame, cameraInfoMsg->header.stamp, // source frame and time
                                          fixedFrame, tf1);
    } catch (tf::TransformException ex) {
        std::cout<<"skipping image frame -- no tf "<<fixedFrame<<"->"<<cameraInfoMsg->header.frame_id<<"\n";
        laser_image << farVal;
        return;
    };

    glBindFramebuffer(GL_FRAMEBUFFER,fbo);
    glClear(GL_DEPTH_BUFFER_BIT);
    {// set projection matrix
    geom::matrixn<4> m;
        m << 0;
        m(0,0) = cameraInfoMsg->K[0]/Width*2;
        m(0,1) = cameraInfoMsg->K[1]/Width*2;
        m(1,0) = cameraInfoMsg->K[3]/Height*2;
        m(1,1) = cameraInfoMsg->K[4]/Height*2;
        m(3,3) = 1;
        m(2,2) = 1;
        glMatrixMode(GL_PROJECTION);
        glLoadMatrixd(&m(0,0));
        float v = 0.05; // cut at 5 cm
        glFrustum(-v,v,-v,v,v,farVal);
    }
    { //set transformation matrix to current camera
        double m[16];
        tf1.getOpenGLMatrix(m);
        glMatrixMode(GL_MODELVIEW);
        for(int j=0; j<4; ++j)m[2+j*4] = -m[2+j*4]; // invert z axis
        glLoadMatrixd(m);
    }

         glPointSize(5.0);
    for(w_clouds_it pcloud=w_clouds.begin(); pcloud!=w_clouds.end(); ++pcloud) {
         glBegin(GL_POINTS);
         cv::Mat & X = *pcloud;
         for(int i=0;i<X.rows;++i){
             glVertex3fv(&X.at<float>(i,0));
         };
         glEnd();
         glcheck();
    };

//
//
//
//
//    bool laser_empty = true;
//    //laser_image << 0;
//    //Before drawing
//    glcheck();
//    glBindFramebuffer(GL_FRAMEBUFFER,fbo);
//    glcheck();
//    //glBindRenderbuffer(GL_RENDERBUFFER,render_buf);
//    //glcheck();
//    //glFramebufferRenderbuffer(GL_FRAMEBUFFER, GL_DEPTH_ATTACHMENT, GL_RENDERBUFFER, render_buf);
//    //glcheck();
//    //glViewport(0, 0, Width, Height);
//    //glcheck();
//    glClear(GL_DEPTH_BUFFER_BIT);
//    glcheck();
//
//    //after drawing
//    //std::vector<std::uint8_t> data(width*height*4);
//    //glReadBuffer(GL_COLOR_ATTACHMENT0);
//    //glReadPixels(0,0,width,height,GL_BGRA,GL_UNSIGNED_BYTE,&data[0]);
//
//
//    // Return to onscreen rendering:
//    //glBindFramebuffer(GL_DRAW_FRAMEBUFFER,0);
//    //glcheck();
//
//    //
//    {
//        geom::matrixn<4> m;
//        m << 0;
//        int l = 0;
//        std::cout<<"\n";
//        m(0,0) = cameraInfoMsg->K[0]/Width*2;
//        m(0,1) = cameraInfoMsg->K[1]/Width*2;
//        m(0,3) = 0;//cameraInfoMsg->K[2];
//        m(1,0) = cameraInfoMsg->K[3]/Height*2;
//        m(1,1) = cameraInfoMsg->K[4]/Height*2;
//        m(1,3) = 0;//cameraInfoMsg->K[5];
//        m(3,0) = 0;//cameraInfoMsg->K[6]/Height;
//        m(3,1) = 0;//cameraInfoMsg->K[7]/Height;
//        m(3,3) = 1;//cameraInfoMsg->K[8]/Height;
//        m(2,2) = 1;//1.0/Height*2;
//
//        //m(4,4) = 1;
//
//        geom::vect4 a = m*geom::vect4(0,0,100,1);
//        a = a/=a[2];
//        glMatrixMode(GL_PROJECTION);
//        //glLoadIdentity();
//        //gluPerspective(45.0f,1, 0.1f, 100.0f);
//        glLoadMatrixd(&m(0,0));
//        //glFrustum(0,Width,0,Height,1,10000);
//        //glFrustum(-1,1,-1,1,0.01,100);
//        //GLfloat aspect = (GLfloat)Width / (GLfloat)Height;
//        //glMatrixMode(GL_PROJECTION);
//        //glLoadIdentity(); // Reset
//        //gluPerspective(60.0f, aspect, 0.1f, 100.0f);
//        //glFrustum(-1.0f,1.0f,-1.0f,1.0f,1.0f,1000.0f);
//        //glFrustum(-1.1f,1.1f,-1.1f,1.1f,1.0f,1000.0f);
//        //glLoadIdentity();
//        //glFrustum(-1.0f,1.0f,-1.0f,1.0f,1.0f,farVal);
//        //glFrustum(-0.1f,0.1f,-0.1f,0.1f,0.1f,farVal);
//        float v = 0.05; // cut at 5 cm
//        glFrustum(-v,v,-v,v,v,farVal);
//    };
//    //
//
//    double m[16];
//    //
//
//    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;
//
//
//        //! wait only for the initial tf, otherwise dont' wait
//        transformListener.waitForTransform(cameraFrame, ros::Time(0), // target frame and time
//                                           cloudFrame, ros::Time(0), // source frame and time
//                                           fixedFrame, ros::Duration(waitForTransform));
//
//        /*
//                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;
//
//        try {
//            transformListener.lookupTransform(cameraFrame, cameraInfoMsg->header.stamp, // target frame and time
//                                              //cloudFrame, cloudMsg->header.stamp-ros::Duration(0.5), // source frame and time
//                                              cloudFrame, cloudMsg->header.stamp, // source frame and time
//                                              fixedFrame, cloudToCamStamped);
//        } catch (tf::TransformException ex) {
//            ROS_ERROR("%s",ex.what());
//            break;//continue;
//        };
//
//        //
//
//        cloudToCamStamped.getOpenGLMatrix(m);
//        glMatrixMode(GL_MODELVIEW);
//        for(int j=0; j<4; ++j)m[2+j*4] = -m[2+j*4]; // invert z axis
//        glLoadMatrixd(m);
//        //glLoadIdentity();
//        //glTranslatef(0.0f, 0.0f, -10.0f);
//        glcheck();
//        glPointSize(3.0);
//        //glBegin(GL_QUADS);
//        glBegin(GL_POINTS);
//        //
//
//        cv::Mat camRotation = rotationFromTransform(cloudToCamStamped);
//        cv::Mat camTranslation = translationFromTransform(cloudToCamStamped);
//        cv::Mat objectPoints;
//        cv::Mat objectPoints1;
//        matFromCloud(cloudMsg, "x", 3).convertTo(objectPoints1, CV_32FC1);
//        matFromCloud(cloudMsg, "x", 3).convertTo(objectPoints, CV_64FC1);
//
//        objectPoints1 = objectPoints1.reshape(3);
//
//
//
//        // 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, (void*)&cameraInfoMsg->K[0]);
//        cv::Mat distCoeffs(1, 5, CV_64FC1, (void*)&cameraInfoMsg->D[0]);
//
//        cv::Mat imagePoints;
//        objectPoints = objectPoints.reshape(3);
//        int numPoints = objectPoints.rows;//*objectPoints.cols;
//        cv::projectPoints(objectPoints, cv::Mat::zeros(3, 1, CV_64FC1), cv::Mat::zeros(3, 1, CV_64FC1), cameraMatrix, distCoeffs, imagePoints);
//
//        for (int iPoint = 0; iPoint < numPoints; iPoint++) {
//            //std::cout<<iPoint<<"\n"; fflush(stdout);
//            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);
//            int xi = int(pt.val[0]);
//            int yi = int(pt.val[1]);
//            if (xi < 0 || yi < 0 || xi >= Width || yi >= Height) {
//                continue;
//            }
//            //laser_image(xi,yi) = z;
//            //
//            //const cv::Vec3d p = objectPoints1.at<cv::Vec3d>(iPoint, 0);
//            //CvPoint3D32f p = objectPoints1.at<CvPoint3D32f>(iPoint, 0);
//            //double * p = &objectPoints1.at<double>(iPoint, 0);
//            float * p = &objectPoints1.at<float>(iPoint, 0);
//            float P[3];
//            P[0] = p[0];
//            P[1] = p[1];
//            P[2] = p[2];
//            glVertex3fv(P);
//            /*
//            p.x+=0.1;
//            glVertex3fv(p);
//            p.y+=0.1;
//            glVertex3fv(&p.x);
//            p.x-=0.1;
//            glVertex3fv(&p.x);
//            */
//            //
//            laser_empty = false;
//        };
//
//        //glVertex3f(0.0f,1.0f,-1.0f);
//        //glVertex3f(0.0f,10.0f,-1.0f);
//        //glVertex3f(0.0f,0.0f,-10.0f);
//        //glVertex3f(0.0f,0.0f,-0.2f);
//        //glVertex2f(0,0);
//        //glVertex2f(0.5,0.5);
//        //glVertex2f(1,1);
//        //glVertex2f(1.2,1);
//        //glVertex2f(-1.2,-1);
//
//        glEnd();
//
//        //glVertex2f(0,-1);
//        //glVertex2f(0,1);
//        //glVertex2f(10,10);
//        //glVertex2f(100,100);
//
//        //break;
//    };
//    //glFinish();
    //
    //glcheck();
    //glReadBuffer(GL_COLOR_ATTACHMENT0);//GL_DEPTH_ATTACHMENT);
    //glcheck();
    glReadPixels(0,0,Width,Height,GL_DEPTH_COMPONENT,GL_FLOAT,&laser_image[0]);
    glcheck();
    //laser_image(Width-1,Height-1) = 0.01;
    //laser_image(Width-3,Height-3) = 0.01;
    laser_image*=farVal;
    float min_depth = std::max(float(0.01),laser_image.min().first);
    float max_depth = min_depth;
    for(int i = 0; i < (Width) *(Height); ++i) {
        if (laser_image[i] <farVal && max_depth < laser_image[i]) {
            max_depth = laser_image[i];
        };
    };
    max_depth = max_depth+0.1;
    //float max_depth = laser_image.max().first;
    std::cout<<"["<<min_depth<<","<<max_depth<<"]\n";
    // publish image here
    {
        sensor_msgs::ImagePtr msg(new sensor_msgs::Image());
        msg->header = cameraInfoMsg->header;
        msg->encoding = sensor_msgs::image_encodings::TYPE_32FC1;
        if(laser_empty) {
            msg->data.resize(0);
            msg->step = 0;
            msg->height = 0;
            msg->width = 0;
        } else {
            int num_bytes = laser_image.byte_size();
            msg->data.resize(num_bytes);
            memcpy(&msg->data[0],laser_image.begin(),num_bytes);
            msg->step = laser_image.byte_stride(0,1);
            msg->height = laser_image.size()[1];
            msg->width = laser_image.size()[0];
            //
            {
                fflush(stdout);
                QRgb * pimg = (QRgb*)img.bits();
                for(int i = 0; i < (Width) *(Height); ++i) {
                    //float * pz = &laser_image[i];
                    //double V = 1.0;
                    //pimg[i] = qRgb(pixel[0],pixel[1],pixel[2]);
                    pimg[i] = 0;
                    if(laser_image[i]<farVal) {
                        double v = 1/(double(laser_image[i]));
                        double V = (v-1/max_depth)/(1/min_depth-1/max_depth);
                        pimg[i] = qRgb(floor((V)*255),floor((V)*255),0);
                    };
                };
                img.setPixel(Width-2,Height-2,qRgb(255,0,255));
                img.setPixel(10,10,qRgb(255,0,255));
                label->setPixmap(QPixmap::fromImage(img));
            };
        };
        //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) {
        return;
    };

    transformListener.waitForTransform(fixedFrame, cloudMsg->header.stamp, // target frame and time
                                       cloudMsg->header.frame_id, cloudMsg->header.stamp, // source frame and time
                                       fixedFrame, ros::Duration(waitForTransform));

    tf::StampedTransform tf1;
    try {
        transformListener.lookupTransform(fixedFrame, cloudMsg->header.stamp, // target frame and time
                                          cloudMsg->header.frame_id, cloudMsg->header.stamp, // source frame and time
                                          fixedFrame, tf1);
    } catch (tf::TransformException ex) {
        std::cout<<"skipping scan -- no tf "<<cloudMsg->header.frame_id<<"->"<<fixedFrame<<"\n";
        return;
    };

    if(w_clouds.size() >=  pointCloudQueueSize) {
        w_clouds.pop_back();
    };

    cv::Mat camRotation;
    rotationFromTransform(tf1).convertTo(camRotation,CV_32FC1);
    cv::Mat camTranslation;
    translationFromTransform(tf1).convertTo(camTranslation,CV_32FC1);
    cv::Mat objectPoints;
    matFromCloud(cloudMsg, "x", 3).convertTo(objectPoints, CV_32FC1);
    objectPoints = objectPoints * camRotation.t() + cv::repeat(camTranslation.t(), objectPoints.rows, 1);
    objectPoints = objectPoints.reshape(3);
    w_clouds.push_front(objectPoints);
}

//___________________________________________________________

class myApp : public QApplication {
public:
    QLabel label;
    PointCloudImage node;
    int timer_count;
public:
    myApp(int & argc, char ** argv):QApplication(argc,argv) {
        timer_count = 0;
        node.onInit();
        startTimer(1000/20);
    };
    //
    ~myApp() {
    };

    void timerEvent(QTimerEvent * event) {
        ros::spinOnce();
        ++timer_count;
        if(node.count<1) {
            if(timer_count % 100 ==0) {
                std::cout<<"waiting for inputs\n";
                std::cout.flush();
            };
        };
        //
        if(!ros::ok()) {
            //ros::shutdown();
            printf("quitting");
            fflush(stdout);
            exit(0);
        };
    };
    bool notify(QObject * receiver, QEvent * ev) {
        if(ev->type() == QEvent::MouseButtonPress) {
            QMouseEvent * e = static_cast<QMouseEvent *>(ev);
            if(e->button()==Qt::LeftButton) {
                return true;
            };
        };
        if(ev->type() == QEvent::KeyPress) {
            QKeyEvent * e = static_cast<QKeyEvent *>(ev);
            if(e->key() == Qt::Key_Backspace) {
                return true;
            } else if(e->key() == Qt::Key_Return) {
                return true;
            } else if(e->key() == Qt::Key_Tab) {
                return true;
            } else if(e->key() == Qt::Key_Escape) {
                exit(0);
                return true;
            };
        };
        return QApplication::notify(receiver,ev);
    };
};


int main(int argc, char **argv) {
    ros::init(argc, argv, "point_cloud_to_image");
    int argc1=0;
    char *argv1[0];
    myApp * a = new myApp(argc1, argv1);
    a->exec();
    delete a;
}