void robot_self_filter_color::SelfMask::maskIntersection(const pcl::PointCloud<pcl::PointXYZRGB>& data_in, const tf::Vector3 &sensor_pos, const double min_sensor_dist,
						   std::vector<int> &mask, const boost::function<void(const tf::Vector3&)> &callback)
{
  mask.resize(data_in.points.size());
  if (bodies_.empty())
    std::fill(mask.begin(), mask.end(), (int)OUTSIDE);
  else
  {
    assumeFrame(data_in.header.frame_id, data_in.header.stamp, sensor_pos, min_sensor_dist);
    maskAuxIntersection(data_in, mask, callback);
  }
}
예제 #2
0
void robot_self_filter::SelfMask::maskIntersection(const pcl::PointCloud<pcl::PointXYZ>& data_in, const std::string &sensor_frame, const double min_sensor_dist,
               std::vector<int> &mask, const boost::function<void(const Eigen::Vector3d&)> &callback)
{
  mask.resize(data_in.points.size());
  if (bodies_.empty()) {
    std::fill(mask.begin(), mask.end(), (int)OUTSIDE);
  }
  else
  {
      assumeFrame(data_in.header.frame_id, pcl_conversions::fromPCL(data_in.header).stamp, sensor_frame, min_sensor_dist);
    if (sensor_frame.empty())
        maskAuxContainment(data_in, mask);
    else
        maskAuxIntersection(data_in, mask, callback);
  }
}