Weird looking PointCloud in RViz

asked 2015-08-07 10:56:36 -0500

rbaleksandar gravatar image

updated 2015-08-12 17:59:08 -0500

Hi!

This is more or less a follow-up to a previous question of mine.

In short: I used the pcd_to_pointcloud tool to create a PointCloud publisher.

Now I have extended my publisher to work with the PMD Nano camboard I'm using. The cloud is displayed in RViz however it looks weird:

image description

If I look at it from the side I get this peculiar view:

image description

I work with the rgbd-grabber library and if I use sample provided in it I get this:

image description

I had great difficulties figuring out how to publish and display the cloud inside RViz in the first and the way it looks like tells me I am doing some configuration wrong. You can see on the top of this...pyramid...turned upside down I get a view of my hand (though it looks pretty bad). It seems that the depth image I'm getting is somehow projected onto the bottom of this pyramid (bottom here is the "top" side of the cloud) and in 2D. All I'm doing is use the pcl::toROSMsg(...) to convert my pcl::PointCloud<pcl::pointxyz>::Ptr to a ros::sensor_msgs::PointCloud2 message, which a publish to the ROS network. I do not do any sort of transformation to its points.

Any advice how to correct this behaviour? I can also provide the code though as I said I just take the points from the camera, convert them to a ROS-compatible message and display them inside RViz.

Thanks!

EDIT: The code is as follows:

#include <ros/ros.h>
#include <string>
#include <memory>
#include <opencv2/core/core.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <pcl/visualization/cloud_viewer.h>
#include <pcl_ros/point_cloud.h>
//#include <sensor_msgs/point_cloud_conversion.h>
#include <pcl/point_types.h>
#include <image_transport/image_transport.h>
#include <cv_bridge/cv_bridge.h>
#include "../include/pmd_nano/PMDNano.h"
#include <pcl/io/io.h>
#include <pcl/io/pcd_io.h>
#include <pcl_ros/publisher.h>
#include <pcl_conversions/pcl_conversions.h>
#include <ros/publisher.h>

using std::string;

class PMDPublisher
{
protected:
  string ppp_file, pap_file;
  ros::NodeHandle nh;
  std::shared_ptr<DepthCamera> camera;
  string cloud_topic;
  string imgDepth_topic, imgAmplitude_topic;
  string tf_frame;
  ros::NodeHandle private_nh;
  pcl_ros::Publisher<sensor_msgs::PointCloud2> pubCloud;
  image_transport::Publisher pubDepth, pubAmplitude;
public:
  sensor_msgs::PointCloud2 cloud;
  //sensor_msgs::PointCloud2Ptr cloud;

  PMDPublisher(string _ppp_file, string _pap_file, string _cloud_topic="pmd_cloud", string _imgDepth_topic="pmd_depth", string _imgAmplitude_topic="pmd_amplitude")
    : ppp_file(_ppp_file), pap_file(_pap_file),
      cloud_topic(_cloud_topic),
      imgDepth_topic(_imgDepth_topic), imgAmplitude_topic(_imgAmplitude_topic),
      tf_frame("/base_link"),
      private_nh("~")
  {
    ROS_INFO("Initializing PMD Publisher");
    if(ppp_file == "" || pap_file == "")
      exit(EXIT_FAILURE);

    ROS_INFO_STREAM("Following PMD configuration files were loaded:\n"
                    << "PPP file: \"" << ppp_file << "\"\n"
                    << "PAP file: \"" << pap_file << "\"\n");

    camera = std::shared_ptr<DepthCamera>(new PMDNano(pap_file, ppp_file));

    pubCloud.advertise(nh, cloud_topic, 1);
    image_transport::ImageTransport imgTransportDepth(nh), imgTransportAmplitude(nh);
    pubDepth = imgTransportDepth.advertise(imgDepth_topic, 1);
    pubAmplitude = imgTransportAmplitude.advertise(imgAmplitude_topic, 1);

    private_nh.param("frame_id", tf_frame, string("/base_link"));
    ROS_INFO_STREAM("Publishing data on topic \"" << nh.resolveName(cloud_topic) << "\" with frame_id \"" << tf_frame << "\"");
  }

  int start()
  {
    try {
     camera->start();
    }
    catch(std::exception e) {
      ROS_ERROR("Node unable to start camera");
      return -1;
    }
    ROS_INFO_STREAM("Depth: " << camera->depthSize().width << " by " << camera->depthSize().height);
    return 0;
  }

  bool spin ()
  {
    ROS_INFO("Allocating memory ...
(more)
edit retag flag offensive close merge delete

Comments

You should link to, or post, your code. You may have to open an issue against pcl_conversions if no one finds something wrong with your code: https://github.com/ros-perception/pcl...

William gravatar imageWilliam ( 2015-08-07 12:20:50 -0500 )edit

Okay, will do that tomorrow since I don't have access to the computer right now.

rbaleksandar gravatar imagerbaleksandar ( 2015-08-07 12:27:51 -0500 )edit

@William There I added the code. I didn't have access to the computer till today.

rbaleksandar gravatar imagerbaleksandar ( 2015-08-10 10:07:08 -0500 )edit

Can you log some of the pcd files and post them? Have you tried http://wiki.ros.org/pcl_ros#pcd_to_po...

William gravatar imageWilliam ( 2015-08-10 23:20:30 -0500 )edit

Phew, took me a while to get access to the damn computer. Please see EDIT2 in my post. It seems the problem starts before the conversion step.

rbaleksandar gravatar imagerbaleksandar ( 2015-08-12 06:49:42 -0500 )edit