lakshmen's profile - activity

I was referred to this code previously. I have some doubts about this code, like to clarify some of them.

#include "ros/ros.h"
#include "pluginlib/class_list_macros.h"
#include "nodelet/nodelet.h"
#include "sensor_msgs/LaserScan.h"
#include "pcl/point_cloud.h"
#include "pcl_ros/point_cloud.h"
#include "pcl/point_types.h"
#include "pcl/ros/conversions.h"




namespace pointcloud_to_laserscan
{
typedef pcl::PointCloud<pcl::PointXYZ> PointCloud;

class CloudToScan : public nodelet::Nodelet
{
public:
  //Constructor
  CloudToScan(): min_height_(0.10), max_height_(0.15), u_min_(100), u_max_(150), output_frame_id_("/openi_depth_frame")
  {
  };

private:
  virtual void onInit()
  {
    ros::NodeHandle& nh = getNodeHandle();
    ros::NodeHandle& private_nh = getPrivateNodeHandle();

    private_nh.getParam("min_height", min_height_);
    private_nh.getParam("max_height", max_height_);

    private_nh.getParam("row_min", u_min_);
    private_nh.getParam("row_max", u_max_);

    private_nh.getParam("output_frame_id", output_frame_id_);
    pub_ = nh.advertise<sensor_msgs::LaserScan>("scan", 10);
    sub_ = nh.subscribe<PointCloud>("cloud", 10, &CloudToScan::callback, this);
  };

  void callback(const PointCloud::ConstPtr& cloud)
  {
    sensor_msgs::LaserScanPtr output(new sensor_msgs::LaserScan());
    NODELET_DEBUG("Got cloud");
    //Copy Header
    output->header = cloud->header;
    output->header.frame_id = output_frame_id_;
    output->angle_min = -M_PI/2;
    output->angle_max = M_PI/2;
    output->angle_increment = M_PI/180.0/2.0;
    output->time_increment = 0.0;
    output->scan_time = 1.0/30.0;
    output->range_min = 0.45;
    output->range_max = 10.0;

    uint32_t ranges_size = std::ceil((output->angle_max - output->angle_min) / output->angle_increment);
    output->ranges.assign(ranges_size, output->range_max + 1.0);

    //pcl::PointCloud<pcl::PointXYZ> cloud;
    //pcl::fromROSMsg(*input, cloud);

    for (PointCloud::const_iterator it = cloud->begin(); it != cloud->end(); ++it)
    {
      const float &x = it->x;
      const float &y = it->y;
      const float &z = it->z;

    /*    for (uint32_t u = std::max((uint32_t)u_min_, 0U); u < std::min((uint32_t)u_max_, cloud.height - 1); u++)
      for (uint32_t v = 0; v < cloud.width -1; v++)
      {
        //NODELET_ERROR("%d %d,  %d %d", u, v, cloud.height, cloud.width);
        pcl::PointXYZ point = cloud.at(v,u); ///WTF Column major?
        const float &x = point.x;
        const float &y = point.y;
        const float &z = point.z;
    */  

      if ( std::isnan(x) || std::isnan(y) || std::isnan(z) )
      {
        NODELET_DEBUG("rejected for nan in point(%f, %f, %f)\n", x, y, z);
        continue;  
      }
      if (-y > max_height_ || -y < min_height_)
      {
        NODELET_DEBUG("rejected for height %f not in range (%f, %f)\n", x, min_height_, max_height_);
        continue;
      }
      double angle = -atan2(x, z);
      if (angle < output->angle_min || angle > output->angle_max)
      {
        NODELET_DEBUG("rejected for angle %f not in range (%f, %f)\n", angle, output->angle_min, output->angle_max);
        continue;
      }
      int index = (angle - output->angle_min) / output->angle_increment;
      //printf ("index xyz( %f %f %f) angle %f index %d\n", x, y, z, angle, index);
      double range_sq = z*z+x*x;
      if (output->ranges[index] * output->ranges[index] > range_sq)
        output->ranges[index] = sqrt(range_sq);


      }

    pub_.publish(output);
  }


  double min_height_, max_height_;
  int32_t u_min_, u_max_;
  std::string output_frame_id_;

  ros::Publisher pub_;
  ros::Subscriber sub_;

};


PLUGINLIB_DECLARE_CLASS(pointcloud_to_laserscan, CloudToScan, pointcloud_to_laserscan::CloudToScan, nodelet::Nodelet);
}

1) where does the M_PI/2 in the angle_min and angle_max come from?

2) why is it range_sq = x^2 + z^2? shldnt it be range_sq = y^2 + z^2?

correct me if i am wrong.. if x is the horizontal and y is the vertical, then z shld be in the negative direction(coming out of the image ... (more)

Hi guys,

Although, i am able to find the depth pointer and find the depth value. Now i need to publish the data and want to visualize the point cloud in Rviz. I have written a code and would like to have your opinions.

    /* OpenCV Includes */
#include "opencv2\opencv.hpp"
#include "opencv2\highgui\highgui.hpp"
#include "opencv2\imgproc\imgproc.hpp"

/* OpenNI Includes */
#include <XnCppWrapper.h>
#include <XnOpenNI.h>
#include <XnLog.h>
#include <XnOS.h>

/* PCL Includes */
#include "pcl/ModelCoefficients.h"
#include "pcl/console/parse.h"
#include "pcl/console/print.h"
#include "pcl/io/pcd_io.h"
#include "pcl/point_types.h"
#include <pcl/features/cvfh.h>
#include <pcl/features/vfh.h>
#include <pcl/features/normal_3d.h>
#include "pcl/filters/extract_indices.h"
#include "pcl/filters/passthrough.h"
#include "pcl/filters/statistical_outlier_removal.h"
#include "pcl/kdtree/kdtree.h"
#include "pcl/sample_consensus/method_types.h"
#include "pcl/sample_consensus/model_types.h"
#include "pcl/segmentation/sac_segmentation.h"
#include "pcl/segmentation/extract_clusters.h"
#include <boost/filesystem.hpp>

/* ROS Includes */
#include <ros/ros.h>
#include <pcl_ros/point_cloud.h>
#include <pcl/point_types.h>

#define SAMPLE_XML_PATH "C:/Program Files (x86)/OpenNI/Data/SamplesConfig.xml"
#define X_RES 640
#define Y_RES 480
#define MAX_DEPTH 10000

using namespace std;
using namespace xn;
using namespace pcl::console;

/* Globals */
Context         context;
ImageGenerator      g_image;
DepthGenerator      d_image;
ImageMetaData       g_imageMD;
EnumerationErrors   errors;
XnStatus        nRetVal = XN_STATUS_OK;
unsigned short      depth[MAX_DEPTH];
char *depth_data;

/* Typedefs */
typedef pcl::PointCloud<pcl::PointXYZRGBNormal> PointCloud;

void raw2depth(){
  for (int i=0; i<MAX_DEPTH; i++) {
  float v = (float)i/MAX_DEPTH;//for visualization purposes only
  v = powf(v, 2);
  v = v*36*256;
  depth[i] = v;
  }
}

void depth2rgb(const XnDepthPixel* Xn_disparity){
  for (int i=0; i<307200; i++) {
    int pval = depth[Xn_disparity[i]];
    int lb = pval & 0xff;
    switch (pval>>8) {
      case 0:
        depth_data[3*i+0] = 255;
        depth_data[3*i+1] = 255;
        depth_data[3*i+2] = 255-lb;
        break;
      case 1:
        depth_data[3*i+0] = 255;
        depth_data[3*i+1] = lb;
        depth_data[3*i+2] = 0;
        break;
      case 2:
        depth_data[3*i+0] = 255-lb;
        depth_data[3*i+1] = 255;
        depth_data[3*i+2] = 0;
        break;
      case 3:
        depth_data[3*i+0] = 0;
        depth_data[3*i+1] = 255;
        depth_data[3*i+2] = lb;
        break;
      case 4:
        depth_data[3*i+0] = 0;
        depth_data[3*i+1] = 255-lb;
        depth_data[3*i+2] = 255;
        break;
      case 5:
        depth_data[3*i+0] = 0;
        depth_data[3*i+1] = 0;
        depth_data[3*i+2] = 255-lb;
        break;
      default:
        depth_data[3*i+0] = 0;
        depth_data[3*i+1] = 0;
        depth_data[3*i+2] = 0;
        break;
    }
  }
}


void rgbdInit(){
  //Initialize context object
  nRetVal = context.Init();
  if (nRetVal != XN_STATUS_OK){
    print_error("Failed to initialize context: %s\n", xnGetStatusString(nRetVal));
  }

  // Create a DepthGenerator node
  nRetVal = d_image.Create(context);
  if (nRetVal != XN_STATUS_OK){
    print_error("Failed to create depth generator: %s\n", xnGetStatusString(nRetVal));
  }

  // Create a ImageGenerator node
  nRetVal = g_image.Create(context);
  if (nRetVal != XN_STATUS_OK){
    print_error("Failed to create image generator: %s\n", xnGetStatusString(nRetVal));
  }

  // Make it start generating data
  nRetVal = context.StartGeneratingAll();
  if (nRetVal != XN_STATUS_OK){
    print_error("Failed generating data: %s\n", xnGetStatusString(nRetVal));
  }

/*  //Sync the DepthGenerator with the ImageGenerator
  nRetVal = d_image.GetFrameSyncCap().FrameSyncWith(g_image);
  if (nRetVal ...