From PointXYZRGB to Mat: gives Segmentation fault

edit

Hello all.

I am using a Kinect to get PointXYZRGB data. After that, I would like to get the RGB elements and put them in a Mat so I can do some image processing.

After getting PointXYZRGB from my Kinect (that I can do successfully), I wanted to put the RGB data in to a Mat (OpenCV) format. Found these usefull linkes:

http://answers.ros.org/question/16859... http://stackoverflow.com/questions/15...

The problem is, when ever I try to run it, it gives me an error.

My code:

#include "ros/ros.h"
#include "std_msgs/String.h"
#include "sensor_msgs/PointCloud2.h"
#include "pcl_ros/point_cloud.h"
#include <iostream>
#include <fstream>
#include <stdio.h>
#include <opencv2/opencv.hpp>
using namespace cv;
using namespace std;
ofstream myfile;

void pointCallback(const pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr& cloud)
{   
        Mat imageFrame;

    if(cloud->isOrganized())
    {

        imageFrame = Mat(cloud->height, cloud->width, CV_8UC3);
        for(int h=0; h<imageFrame.rows; h++)
        {
            for(int w=0; w<imageFrame.cols; w++)
            {
                cout<< "h=" << h << " and w=" << w << endl;              
                pcl::PointXYZRGB point = cloud->at(w,h);
                Eigen::Vector3i rgb = point.getRGBVector3i();

                imageFrame.at<Vec3d>(h,w)[0] = rgb[2];
                imageFrame.at<Vec3d>(h,w)[1] = rgb[1];
                imageFrame.at<Vec3d>(h,w)[2] = rgb[0];
            }
        }
    }


}

int main(int argc, char **argv)
{

    ros::init(argc, argv, "front_end");
    ros::NodeHandle n;
    string topic = n.resolveName("/camera/depth_registered/points");
    uint32_t queue_size = 5;

    ros::Subscriber sub = n.subscribe<pcl::PointCloud<pcl::PointXYZRGB> > (topic, queue_size, pointCallback);

    ros::spin();

  return 0;
}

The error:

Segmentation fault (core dumped)

The error appears after a while it runs BUT before the "pointCallback" runs one complete time. As you can notice, in the two for loops I have put a "cout" code to see until which pixel it goes before displaying the error message. The last cout is "h=475 and w=570".

Any idea why this is happening and how I can fix this?