Cloud to Scan: Some Points won't show up in laser scan

edit

 void callback(const PointCloud::ConstPtr& cloud)
 {
sensor_msgs::LaserScanPtr output(new sensor_msgs::LaserScan());
output->header = cloud->header;
output->header.frame_id = output_frame_id_; // Set output frame. Point clouds come from "optical" frame, scans come from corresponding mount frame
output->angle_min = angle_min_;
output->angle_max = angle_max_;
output->angle_increment = angle_increment_;
output->time_increment = 0.0;
output->scan_time = scan_time_;
output->range_min = range_min_;
output->range_max = range_max_;

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);

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

  if ( std::isnan(x) || std::isnan(y) || std::isnan(z) )
  {
    NODELET_INFO("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 range_sq = z*z+x*x;

  if (range_sq < range_min_sq_) {
    NODELET_DEBUG("rejected for range %f below minimum value %f. Point: (%f, %f, %f)", range_sq, range_min_sq_, x, y, z);
    continue;
  }

  double angle = -atan2(x, z) + 1.57;
  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;


  if (output->ranges[index] * output->ranges[index] > range_sq)
    output->ranges[index] = sqrt(range_sq);
  }

pub_.publish(output);
}