Revision history [back]

Not sure if this will help someone else but this is what I have for converting an OpenCV mat pointcloud into a sensor_msgs::PointCloud2 message

//convert point cloud image to ros message
sensor_msgs::PointCloud2 img_to_cloud(const cv::Mat& rgb, const cv::Mat &coords)
{
//rgb is a cv::Mat with 3 color channels of size 640x480
//coords is a cv::Mat with xyz channels of size 640x480, units in mm from calibration

//figure out number of points
int numpoints = rgb.size().width * rgb.size().height;

//declare message and sizes
sensor_msgs::PointCloud2 cloud;
cloud.header.frame_id = camera_frame_id;
cloud.header.stamp = ros::Time::now();
cloud.width  = numpoints;
cloud.height = 1;
cloud.is_bigendian = false;
cloud.is_dense = false; // there may be invalid points

//for fields setup
sensor_msgs::PointCloud2Modifier modifier(cloud);
modifier.setPointCloud2FieldsByString(2,"xyz","rgb");
modifier.resize(numpoints);

//iterators
sensor_msgs::PointCloud2Iterator<float> out_x(cloud, "x");
sensor_msgs::PointCloud2Iterator<float> out_y(cloud, "y");
sensor_msgs::PointCloud2Iterator<float> out_z(cloud, "z");
sensor_msgs::PointCloud2Iterator<uint8_t> out_r(cloud, "r");
sensor_msgs::PointCloud2Iterator<uint8_t> out_g(cloud, "g");
sensor_msgs::PointCloud2Iterator<uint8_t> out_b(cloud, "b");

for (int y=0; y<coords.rows; y++)
{
    for (int x=0; x<coords.cols; x++)
    {
        //get the image coordinate for this point and convert to mm
        cv::Vec3f pointcoord = coords.at<cv::Vec3f>(y, x);
        float X_World = pointcoord.val[0] * mm_to_m;
        float Y_World = pointcoord.val[1] * mm_to_m;
        float Z_World = pointcoord.val[2] * mm_to_m;

        //get the colors
        cv::Vec3b color = rgb.at<cv::Vec3b>(y,x);
        uint8_t r = (color[2]);
        uint8_t g = (color[1]);
        uint8_t b = (color[0]);

        //store xyz in point cloud, transforming from image coordinates, (Z Forward to X Forward)
        *out_x = Z_World;
        *out_y = -X_World;
        *out_z = -Y_World;

        // store colors
        *out_r = r;
        *out_g = g;
        *out_b = b;

        //increment
        ++out_x;
        ++out_y;
        ++out_z;
        ++out_r;
        ++out_g;
        ++out_b;
    }
}
return cloud;
}

Also, if you see something wrong with this code please let me know