Revision history [back]

Not sure there is a package, I implemented a version of what you describe myself though using the 'RangeImagePlanar' point cloud type, as that preserves the locality information (x,y) of points and you only have to iterate over the image once this way. Don't have the code in a public repo, but here are the relevant bits (cobbled together, to give an idea on how one could do it quickly):

We fill a range image for every depth image coming from a RGB-D camera (given as depth_image_msg):

    range_image_planar->setDepthImage(reinterpret_cast<const float*> (&depth_image_msg->data[0]),
                                      depth_image_msg->width, depth_image_msg->height,
                                      camera_info_msg->P[2],  camera_info_msg->P[6],
                                      camera_info_msg->P[0],  camera_info_msg->P[5]);

    range_image_planar->header = depth_image_msg->header;

If we're in calibration mode, we update our reference image as needed (if the new distance value is closer or was invalid before we store that in the reference image):

  void calibrateReferenceImage(RangeImagePlanar* range_image_planar_in)
  {
    //if first image..
    if(range_image_planar_reference == 0){
      range_image_planar_reference = new RangeImagePlanar();
      *range_image_planar_reference = *range_image_planar_in;
    }else{
      if ( pointcloudsSameSize(*range_image_planar_reference, *range_image_planar_in)){

        int width_ref = range_image_planar_reference->width;
        int height_ref = range_image_planar_reference->height;

        for (int x = 0; x < width_ref; ++x){
          for (int y = 0; y < height_ref; ++y){

            if ( range_image_planar_in->isValid(x,y)){
              const pcl::PointWithRange& tmpCurr ( range_image_planar_in->getPoint(x,y));

              pcl::PointWithRange& tmpRef (range_image_planar_reference->getPoint(x,y) );

              if ( (tmpRef.range > tmpCurr.range) || (tmpRef.range < 0.3)){
                tmpRef = tmpCurr;
              }
            }
          }
        }
      }else{
        ROS_INFO("Reference and current cloud not same size! Not merging.\n");
      }
    }
  }

For getting pointclouds of only the foreground we just compare the distance values between the current and the reference image:

int width_ref = range_image_planar_reference->width;
int height_ref = range_image_planar_reference->height;

pc_foreground_->clear();
pc_foreground_->header = range_image_planar->header;

//get Pointcloud with objects in foreground (into pc_foreground_)
for (int x = 0; x < width_ref; ++x){
  for (int y = 0; y < height_ref; ++y){

    if ( range_image_planar->isValid(x,y) && range_image_planar_reference->isValid(x,y)){
      const pcl::PointWithRange& tmpCurr ( range_image_planar->getPoint(x,y));

      const pcl::PointWithRange& tmpRef (range_image_planar_reference->getPoint(x,y) );

      if ((tmpRef.range - threshold) > tmpCurr.range){
        pc_foreground_->push_back(tmpCurr);
      }
    }
  }
}

Not sure this is the "best" way, but it works pretty well and is reasonably fast. It of course only works with the special case of a fixed RGB-D camera and a static scene, apart from the moving objects of interest that one is interested in.

Not sure there is a package, package. I implemented a version of what you describe myself though using the 'RangeImagePlanar' point cloud type, as that preserves the locality information (x,y) of points and you only have to iterate over the image once this way. Don't have the code in a public repo, but here are the relevant bits (cobbled together, to give an idea on how one could do it quickly):

We fill a range image for every depth image coming from a RGB-D camera (given as depth_image_msg):

    range_image_planar->setDepthImage(reinterpret_cast<const float*> (&depth_image_msg->data[0]),
                                      depth_image_msg->width, depth_image_msg->height,
                                      camera_info_msg->P[2],  camera_info_msg->P[6],
                                      camera_info_msg->P[0],  camera_info_msg->P[5]);

    range_image_planar->header = depth_image_msg->header;

If we're in calibration mode, we update our reference image as needed (if the new distance value is closer or was invalid before we store that in the reference image):

  void calibrateReferenceImage(RangeImagePlanar* range_image_planar_in)
  {
    //if first image..
    if(range_image_planar_reference == 0){
      range_image_planar_reference = new RangeImagePlanar();
      *range_image_planar_reference = *range_image_planar_in;
    }else{
      if ( pointcloudsSameSize(*range_image_planar_reference, *range_image_planar_in)){

        int width_ref = range_image_planar_reference->width;
        int height_ref = range_image_planar_reference->height;

        for (int x = 0; x < width_ref; ++x){
          for (int y = 0; y < height_ref; ++y){

            if ( range_image_planar_in->isValid(x,y)){
              const pcl::PointWithRange& tmpCurr ( range_image_planar_in->getPoint(x,y));

              pcl::PointWithRange& tmpRef (range_image_planar_reference->getPoint(x,y) );

              if ( (tmpRef.range > tmpCurr.range) || (tmpRef.range < 0.3)){
                tmpRef = tmpCurr;
              }
            }
          }
        }
      }else{
        ROS_INFO("Reference and current cloud not same size! Not merging.\n");
      }
    }
  }

For getting pointclouds of only the foreground we just compare the distance values between the current and the reference image:

int width_ref = range_image_planar_reference->width;
int height_ref = range_image_planar_reference->height;

pc_foreground_->clear();
pc_foreground_->header = range_image_planar->header;

//get Pointcloud with objects in foreground (into pc_foreground_)
for (int x = 0; x < width_ref; ++x){
  for (int y = 0; y < height_ref; ++y){

    if ( range_image_planar->isValid(x,y) && range_image_planar_reference->isValid(x,y)){
      const pcl::PointWithRange& tmpCurr ( range_image_planar->getPoint(x,y));

      const pcl::PointWithRange& tmpRef (range_image_planar_reference->getPoint(x,y) );

      if ((tmpRef.range - threshold) > tmpCurr.range){
        pc_foreground_->push_back(tmpCurr);
      }
    }
  }
}

Not sure this is the "best" way, but it works pretty well and is reasonably fast. It of course only works with the special case of a fixed RGB-D camera and a static scene, apart from the moving objects of interest that one is interested in.