void StereoProcessor::processPoints(const stereo_msgs::DisparityImage& disparity,
const cv::Mat& color, const std::string& encoding,
const image_geometry::StereoCameraModel& model,
sensor_msgs::PointCloud& points) const
{
// Calculate dense point cloud
const sensor_msgs::Image& dimage = disparity.image;
const cv::Mat_<float> dmat(dimage.height, dimage.width, (float*)&dimage.data[0], dimage.step);
model.projectDisparityImageTo3d(dmat, dense_points_, true);
// Fill in sparse point cloud message
points.points.resize(0);
points.channels.resize(3);
points.channels[0].name = "rgb";
points.channels[0].values.resize(0);
points.channels[1].name = "u";
points.channels[1].values.resize(0);
points.channels[2].name = "v";
points.channels[2].values.resize(0);
for (int32_t u = 0; u < dense_points_.rows; ++u) {
for (int32_t v = 0; v < dense_points_.cols; ++v) {
if (isValidPoint(dense_points_(u,v))) {
// x,y,z
geometry_msgs::Point32 pt;
pt.x = dense_points_(u,v)[0];
pt.y = dense_points_(u,v)[1];
pt.z = dense_points_(u,v)[2];
points.points.push_back(pt);
// u,v
points.channels[1].values.push_back(u);
points.channels[2].values.push_back(v);
}
}
}
// Fill in color
namespace enc = sensor_msgs::image_encodings;
points.channels[0].values.reserve(points.points.size());
if (encoding == enc::MONO8) {
for (int32_t u = 0; u < dense_points_.rows; ++u) {
for (int32_t v = 0; v < dense_points_.cols; ++v) {
if (isValidPoint(dense_points_(u,v))) {
uint8_t g = color.at<uint8_t>(u,v);
int32_t rgb = (g << 16) | (g << 8) | g;
points.channels[0].values.push_back(*(float*)(&rgb));
}
}
}
}
else if (encoding == enc::RGB8) {
for (int32_t u = 0; u < dense_points_.rows; ++u) {
for (int32_t v = 0; v < dense_points_.cols; ++v) {
if (isValidPoint(dense_points_(u,v))) {
const cv::Vec3b& rgb = color.at<cv::Vec3b>(u,v);
int32_t rgb_packed = (rgb[0] << 16) | (rgb[1] << 8) | rgb[2];
points.channels[0].values.push_back(*(float*)(&rgb_packed));
}
}
}
}
else if (encoding == enc::BGR8) {
for (int32_t u = 0; u < dense_points_.rows; ++u) {
for (int32_t v = 0; v < dense_points_.cols; ++v) {
if (isValidPoint(dense_points_(u,v))) {
const cv::Vec3b& bgr = color.at<cv::Vec3b>(u,v);
int32_t rgb_packed = (bgr[2] << 16) | (bgr[1] << 8) | bgr[0];
points.channels[0].values.push_back(*(float*)(&rgb_packed));
}
}
}
}
else {
ROS_WARN("Could not fill color channel of the point cloud, unrecognized encoding '%s'", encoding.c_str());
}
}
void PointCloud2Nodelet::imageCb(const ImageConstPtr& l_image_msg,
const CameraInfoConstPtr& l_info_msg,
const CameraInfoConstPtr& r_info_msg,
const DisparityImageConstPtr& disp_msg)
{
// Update the camera model
model_.fromCameraInfo(l_info_msg, r_info_msg);
// Calculate point cloud
const Image& dimage = disp_msg->image;
const cv::Mat_<float> dmat(dimage.height, dimage.width, (float*)&dimage.data[0], dimage.step);
model_.projectDisparityImageTo3d(dmat, points_mat_, true);
cv::Mat_<cv::Vec3f> mat = points_mat_;
// Fill in new PointCloud2 message (2D image-like layout)
PointCloud2Ptr points_msg = boost::make_shared<PointCloud2>();
points_msg->header = disp_msg->header;
points_msg->height = mat.rows;
points_msg->width = mat.cols;
points_msg->fields.resize (4);
points_msg->fields[0].name = "x";
points_msg->fields[0].offset = 0;
points_msg->fields[0].count = 1;
points_msg->fields[0].datatype = PointField::FLOAT32;
points_msg->fields[1].name = "y";
points_msg->fields[1].offset = 4;
points_msg->fields[1].count = 1;
points_msg->fields[1].datatype = PointField::FLOAT32;
points_msg->fields[2].name = "z";
points_msg->fields[2].offset = 8;
points_msg->fields[2].count = 1;
points_msg->fields[2].datatype = PointField::FLOAT32;
points_msg->fields[3].name = "rgb";
points_msg->fields[3].offset = 12;
points_msg->fields[3].count = 1;
points_msg->fields[3].datatype = PointField::FLOAT32;
//points_msg->is_bigendian = false; ???
static const int STEP = 16;
points_msg->point_step = STEP;
points_msg->row_step = points_msg->point_step * points_msg->width;
points_msg->data.resize (points_msg->row_step * points_msg->height);
points_msg->is_dense = false; // there may be invalid points
float bad_point = std::numeric_limits<float>::quiet_NaN ();
int offset = 0;
for (int v = 0; v < mat.rows; ++v)
{
for (int u = 0; u < mat.cols; ++u, offset += STEP)
{
if (isValidPoint(mat(v,u)))
{
// x,y,z,rgba
memcpy (&points_msg->data[offset + 0], &mat(v,u)[0], sizeof (float));
memcpy (&points_msg->data[offset + 4], &mat(v,u)[1], sizeof (float));
memcpy (&points_msg->data[offset + 8], &mat(v,u)[2], sizeof (float));
}
else
{
memcpy (&points_msg->data[offset + 0], &bad_point, sizeof (float));
memcpy (&points_msg->data[offset + 4], &bad_point, sizeof (float));
memcpy (&points_msg->data[offset + 8], &bad_point, sizeof (float));
}
}
}
// Fill in color
namespace enc = sensor_msgs::image_encodings;
const std::string& encoding = l_image_msg->encoding;
offset = 0;
if (encoding == enc::MONO8)
{
const cv::Mat_<uint8_t> color(l_image_msg->height, l_image_msg->width,
(uint8_t*)&l_image_msg->data[0],
l_image_msg->step);
for (int v = 0; v < mat.rows; ++v)
{
for (int u = 0; u < mat.cols; ++u, offset += STEP)
{
if (isValidPoint(mat(v,u)))
{
uint8_t g = color(v,u);
int32_t rgb = (g << 16) | (g << 8) | g;
memcpy (&points_msg->data[offset + 12], &rgb, sizeof (int32_t));
}
else
{
memcpy (&points_msg->data[offset + 12], &bad_point, sizeof (float));
}
}
}
}
else if (encoding == enc::RGB8)
{
const cv::Mat_<cv::Vec3b> color(l_image_msg->height, l_image_msg->width,
(cv::Vec3b*)&l_image_msg->data[0],
l_image_msg->step);
for (int v = 0; v < mat.rows; ++v)
{
for (int u = 0; u < mat.cols; ++u, offset += STEP)
{
if (isValidPoint(mat(v,u)))
{
const cv::Vec3b& rgb = color(v,u);
int32_t rgb_packed = (rgb[0] << 16) | (rgb[1] << 8) | rgb[2];
memcpy (&points_msg->data[offset + 12], &rgb_packed, sizeof (int32_t));
}
else
{
memcpy (&points_msg->data[offset + 12], &bad_point, sizeof (float));
}
}
}
}
else if (encoding == enc::BGR8)
{
const cv::Mat_<cv::Vec3b> color(l_image_msg->height, l_image_msg->width,
(cv::Vec3b*)&l_image_msg->data[0],
l_image_msg->step);
for (int v = 0; v < mat.rows; ++v)
{
for (int u = 0; u < mat.cols; ++u, offset += STEP)
{
if (isValidPoint(mat(v,u)))
{
const cv::Vec3b& bgr = color(v,u);
int32_t rgb_packed = (bgr[2] << 16) | (bgr[1] << 8) | bgr[0];
memcpy (&points_msg->data[offset + 12], &rgb_packed, sizeof (int32_t));
}
else
{
memcpy (&points_msg->data[offset + 12], &bad_point, sizeof (float));
}
}
}
}
else
{
NODELET_WARN_THROTTLE(30, "Could not fill color channel of the point cloud, "
"unsupported encoding '%s'", encoding.c_str());
}
pub_points2_.publish(points_msg);
}
void StereoProcessor::processPoints2(const stereo_msgs::DisparityImage& disparity,
const cv::Mat& color, const std::string& encoding,
const image_geometry::StereoCameraModel& model,
sensor_msgs::PointCloud2& points) const
{
// Calculate dense point cloud
const sensor_msgs::Image& dimage = disparity.image;
const cv::Mat_<float> dmat(dimage.height, dimage.width, (float*)&dimage.data[0], dimage.step);
model.projectDisparityImageTo3d(dmat, dense_points_, true);
// Fill in sparse point cloud message
points.height = dense_points_.rows;
points.width = dense_points_.cols;
points.fields.resize (4);
points.fields[0].name = "x";
points.fields[0].offset = 0;
points.fields[0].count = 1;
points.fields[0].datatype = sensor_msgs::PointField::FLOAT32;
points.fields[1].name = "y";
points.fields[1].offset = 4;
points.fields[1].count = 1;
points.fields[1].datatype = sensor_msgs::PointField::FLOAT32;
points.fields[2].name = "z";
points.fields[2].offset = 8;
points.fields[2].count = 1;
points.fields[2].datatype = sensor_msgs::PointField::FLOAT32;
points.fields[3].name = "rgb";
points.fields[3].offset = 12;
points.fields[3].count = 1;
points.fields[3].datatype = sensor_msgs::PointField::FLOAT32;
//points.is_bigendian = false; ???
points.point_step = 16;
points.row_step = points.point_step * points.width;
points.data.resize (points.row_step * points.height);
points.is_dense = false; // there may be invalid points
float bad_point = std::numeric_limits<float>::quiet_NaN ();
int i = 0;
for (int32_t u = 0; u < dense_points_.rows; ++u) {
for (int32_t v = 0; v < dense_points_.cols; ++v, ++i) {
if (isValidPoint(dense_points_(u,v))) {
// x,y,z,rgba
memcpy (&points.data[i * points.point_step + 0], &dense_points_(u,v)[0], sizeof (float));
memcpy (&points.data[i * points.point_step + 4], &dense_points_(u,v)[1], sizeof (float));
memcpy (&points.data[i * points.point_step + 8], &dense_points_(u,v)[2], sizeof (float));
}
else {
memcpy (&points.data[i * points.point_step + 0], &bad_point, sizeof (float));
memcpy (&points.data[i * points.point_step + 4], &bad_point, sizeof (float));
memcpy (&points.data[i * points.point_step + 8], &bad_point, sizeof (float));
}
}
}
// Fill in color
namespace enc = sensor_msgs::image_encodings;
i = 0;
if (encoding == enc::MONO8) {
for (int32_t u = 0; u < dense_points_.rows; ++u) {
for (int32_t v = 0; v < dense_points_.cols; ++v, ++i) {
if (isValidPoint(dense_points_(u,v))) {
uint8_t g = color.at<uint8_t>(u,v);
int32_t rgb = (g << 16) | (g << 8) | g;
memcpy (&points.data[i * points.point_step + 12], &rgb, sizeof (int32_t));
}
else {
memcpy (&points.data[i * points.point_step + 12], &bad_point, sizeof (float));
}
}
}
}
else if (encoding == enc::RGB8) {
for (int32_t u = 0; u < dense_points_.rows; ++u) {
for (int32_t v = 0; v < dense_points_.cols; ++v, ++i) {
if (isValidPoint(dense_points_(u,v))) {
const cv::Vec3b& rgb = color.at<cv::Vec3b>(u,v);
int32_t rgb_packed = (rgb[0] << 16) | (rgb[1] << 8) | rgb[2];
memcpy (&points.data[i * points.point_step + 12], &rgb_packed, sizeof (int32_t));
}
else {
memcpy (&points.data[i * points.point_step + 12], &bad_point, sizeof (float));
}
}
}
}
else if (encoding == enc::BGR8) {
for (int32_t u = 0; u < dense_points_.rows; ++u) {
for (int32_t v = 0; v < dense_points_.cols; ++v, ++i) {
if (isValidPoint(dense_points_(u,v))) {
const cv::Vec3b& bgr = color.at<cv::Vec3b>(u,v);
int32_t rgb_packed = (bgr[2] << 16) | (bgr[1] << 8) | bgr[0];
memcpy (&points.data[i * points.point_step + 12], &rgb_packed, sizeof (int32_t));
}
else {
memcpy (&points.data[i * points.point_step + 12], &bad_point, sizeof (float));
}
}
}
}
else {
ROS_WARN("Could not fill color channel of the point cloud, unrecognized encoding '%s'", encoding.c_str());
}
}