Can't get ground points for depth-image-encoding.
Hello everybody,
I'm trying to convert a depth-image from a stereocamera to a point-cloud like shown in the following code:
void DepthImageCallback(const sensor_msgs::ImageConstPtr& msg)
{
if (!cam_model.initialized())
return;
sensor_msgs::PointCloud2::Ptr cloud_msg(new sensor_msgs::PointCloud2);
cloud_msg->header = msg->header;
cloud_msg->header.frame_id = "zed3d";
cloud_msg->header.stamp = ros::Time::now() + delay;
cloud_msg->height = msg->height;
cloud_msg->width = msg->width;
cloud_msg->is_dense = false;
cloud_msg->is_bigendian = false;
sensor_msgs::PointCloud2Modifier pcd_modifier(*cloud_msg);
pcd_modifier.setPointCloud2FieldsByString(1, "xyz");
if (msg->encoding == sensor_msgs::image_encodings::TYPE_16UC1)
{
convert<uint16_t>(msg, cloud_msg, cam_model);
}
else if (msg->encoding == sensor_msgs::image_encodings::TYPE_32FC1)
{
convert<float>(msg, cloud_msg, cam_model);
}
else
{
ROS_ERROR("Depth image has unsupported encoding");
return;
}
cloud_publisher.publish(cloud_msg);
The converting process is successfull and i get a point-cloud of type PointCloud2.
As my goal is to create a ground-model out of the point-cloud data, I need to see the "ground-points".
That's where the problem is.
Do you have any idea, what I'm doing wrong and how I can achieve to see the ground-points?
Thank you very much in advance!