ROS sensor_msgs PointCloud2 from cv::Mat

snippet.cpp

void reverse_memcpy(unsigned char* dst, const unsigned char* src, size_t n)
{
  for (size_t i=0; i<n; ++i)
    dst[n-1-i] = src[i];
}

// ----------------------------------------------------------------------------

sensor_msgs::PointCloud2 cloudmsg;
cloudmsg.header.stamp = _img->header.stamp;
cloudmsg.header.frame_id = _mpts->header.frame_id;
cloudmsg.width = image.rows*image.cols;
cloudmsg.height = 1;
cloudmsg.is_dense = true;

sensor_msgs::PointCloud2Modifier modifier(cloudmsg);
modifier.setPointCloud2FieldsByString(1, "xyz");

std::string format_str = "rgb";

cloudmsg.point_step = addPointField(cloudmsg, format_str.c_str(), 1, sensor_msgs::PointField::UINT32, cloudmsg.point_step);
cloudmsg.row_step = cloudmsg.width * cloudmsg.point_step;
cloudmsg.data.resize(cloudmsg.height * cloudmsg.row_step);

sensor_msgs::PointCloud2Iterator<float> iter_x(cloudmsg, "x");
sensor_msgs::PointCloud2Iterator<float> iter_y(cloudmsg, "y");
sensor_msgs::PointCloud2Iterator<float> iter_z(cloudmsg, "z");
sensor_msgs::PointCloud2Iterator<uint8_t> iter_color(cloudmsg, format_str);

uint8_t* color_data = out.data;

int num_colors = 3;

for (size_t r=0; r<image.rows; ++r) {
  for (size_t c=0; c<image.cols; ++c) {

    *iter_x = c*0.010;
    *iter_y = r*0.010;
    *iter_z = *iter_x + *iter_y;

    int offset = (r * image.cols + c) * num_colors;
    reverse_memcpy(&(*iter_color), color_data+offset, num_colors);

    ++iter_x; ++iter_y; ++iter_z;
    ++iter_color;
  }
}

pub_cloud_.publish(cloudmsg);

see https://github.com/IntelRealSense/realsense-ros/blob/development/realsense2_camera/src/base_realsense_node.cpp#L1886