Ask Your Question
0

Doubts about pointcloud to laser scan

asked 2011-07-14 05:41:47 -0600

lakshmen gravatar image

updated 2011-07-14 23:47:35 -0600

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)

edit retag flag offensive close merge delete

1 Answer

Sort by » oldest newest most voted
3

answered 2011-07-14 06:59:00 -0600

Chad Rockey gravatar image

updated 2011-07-15 01:17:04 -0600

1.) M_PI was likely #defined as 3.141592 (pi) in one of the header files. Somewhere in ROS.

2.) I think the input cloud is defined in a camera frame. Camera frames are (annoyingly) different than the standard ROS frame. X is horizontal along the image, Y is vertical along the image (often facing down), and Z is positive going into the image plane. So here distance to point ahead is z and distance from center is x.

In response to your edit, as stated in REP 103, the camera coordinate frame is "z forward, x right, y down", assuming you are looking from the image sensor to the image plane and are centered at the optical focus point of the camera.

3.) No, it's not. However, if you are running these nodes on a slower computer, the throttling can help a lot. It's just a very timing-imprecise node to slow things down so that you don't have to process every pointcloud.

edit flag offensive delete link more

Comments

M_PI should actually be in a std library header, likely math.h or cmath. It is indeed a macro that is #define'd to 3.14159... I believe some ROS header somewhere pulls in math.h.
Eric Perko gravatar imageEric Perko ( 2011-07-14 08:09:37 -0600 )edit
I edited my question.have a look..
lakshmen gravatar imagelakshmen ( 2011-07-14 17:23:51 -0600 )edit
Thanks a lot for correcting my mistake. Now i understand why it is z*z + x*x
lakshmen gravatar imagelakshmen ( 2011-07-17 14:18:57 -0600 )edit
1 more qn: why is it -atan2(x, z)? and not atan2(x,z)?
lakshmen gravatar imagelakshmen ( 2011-07-17 15:21:08 -0600 )edit
The "y" axis is technically the one we're revolving around in camera frame. Since it's pointing down, the angle rotates the wrong direction.
Chad Rockey gravatar imageChad Rockey ( 2011-07-17 15:31:37 -0600 )edit
1 more qn:if (output->ranges[index] * output->ranges[index] > range_sq) output->ranges[index] = sqrt(range_sq);, what is this statement trying to do.. not that sure...
lakshmen gravatar imagelakshmen ( 2011-07-18 15:29:50 -0600 )edit

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

Stats

Asked: 2011-07-14 05:41:47 -0600

Seen: 892 times

Last updated: Jul 15 '11