Weird looking PointCloud in RViz

edit

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:

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

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

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 ...