segmenter.hh

Go to the documentation of this file.

/*
 * segmenter.hh
 * Mac Mason <mmason@willowgarage.com>
 *
 * Segment out planes & objects & whatnot. 
 *
 * This object needs two things as input: a plane (PointCloud) and an Image.
 * It outputs several things:
 *
 * - A pointcloud of the current plane. 
 * - Planes, as Plane messages.
 * - Objects, as PointClouds.
 *
 * This object maintains all of its state internally; the NodeHandle, the
 * publishers and subscribers, and everything else. So, build one, configure
 * it, start it up, and spin.
 */

#pragma once

#include <string>
#include <boost/utility.hpp>  // For noncopyable
#include <boost/scoped_ptr.hpp> 
#include <boost/thread.hpp>
#include <boost/circular_buffer.hpp>

#include "ros/ros.h"

#include "pcl16/point_types.h"
#include "pcl16_ros/point_cloud.h"
#include "pcl16/ModelCoefficients.h"
#include "tf/transform_listener.h"
#include "tf/transform_broadcaster.h"
#include "image_transport/image_transport.h"
#include "sensor_msgs/CameraInfo.h"
#include "message_filters/synchronizer.h"
#include "message_filters/sync_policies/approximate_time.h"
#include <pcl16/features/integral_image_normal.h>
#include <pcl16/segmentation/organized_multi_plane_segmentation.h>
#include <pcl16/segmentation/edge_aware_plane_comparator.h>

#include "dynamic_reconfigure/server.h"
#include "semanticmodel/FilterConfig.h"
#include "semanticmodel/Plane.h"
#include <cstddef>

using semanticmodel::FilterConfig;
using std::vector;

namespace semanticmodel
{

class Segmenter : public boost::noncopyable
{

private:
  // Save some _serious_ typing.
  typedef pcl16::PointXYZRGB Point;
  typedef pcl16::PointCloud<Point> PointCloud;

  // Time here is tricky. However, openni_camera does approximate
  // synchronization for us, and then sets the cloud's timestamp to the
  // depth image's stamp. Sadly, that isn't (necessarily) the same as the
  // RGB image's stamp. Instead, we approximately synchronize all three,
  // knowing that (we hope) the first two are already exactly
  // synchronized, and the third has already passed through an
  // ApproximateTime policy.
  typedef message_filters::sync_policies::
  ApproximateTime<sensor_msgs::Image, 
                  sensor_msgs::CameraInfo, 
                  sensor_msgs::Image,  // Second pair are the depths.
                  sensor_msgs::CameraInfo,
                  PointCloud> Sync;
                  

public:
  // Any of the output topics can be the empty string, in which case we
  // don't publish them. The input topics, however, must both be provided.
  //
  // camera_input_topic is the (partial) namespace of the camera. For
  // example, the Kinect publishes /camera/rgb/image_color and
  // /camera/rgb/camera_info; camera_input_topic should be /camera/rgb.
  // (camera_info and image_color are hardcoded.)
  //
  // The passed-in topic names are appended to name_space (this permits
  // private naming).
  Segmenter(const std::string& name_space,
            const std::string& cloud_input_topic,
            const std::string& camera_input_topic,
            const std::string& depth_input_topic,
            const std::string& planecloud_output_topic,
            const std::string& plane_output_topic,
            const std::string& object_output_topic,
            const std::string& plane_service_topic);

  // No need for a destructor; our members are smart. No need for other
  // stuff; noncopyable keeps us well-behaved.
      
  /*
   * Our processing pipeline. This has several steps:
   *
   * 0. Transform the input into the config.canonical_frame tf frame;
   * this is (probably) /map, but could generally be any trustworthy frame
   * where +z is "up". This allows us to think about "horizontal" in very
   * simple terms.
   *
   * 1. A passthrough filter. This filters out NaNs, but (more
   * importantly) truncates out the floor and ceiling.
   *
   * 2. A plane fitter (using region-growing). This looks for
   * _horizontal_ planes, not just any planes.  The output from this
   * function is zero or more planes, as PointClouds, already projected
   * onto their plane equations, and a vector of ModelCoefficients
   * objects, representing those equations.
   *
   * 3. Get the convex hull of the extracted plane. 
   *
   * 4. The convex hulls we've just built aren't quite big enough; we want
   * to search for objects atop those hulls merged with existing ones
   * (which is what plane_tracker does). So, send them to the tracker, and
   * get back the hulls to use for the next step.
   *
   * 5. Collect the points above the convex hull; these are potential
   * objects.
   *
   * 6. Cluster those points to generate actual objects.
   *
   * These functions are public, for the moment, because they might be
   * generally useful, or something. They might not really deserve
   * publicity.
   */
  void passthrough(PointCloud::Ptr& in, PointCloud::Ptr& out);

  void fit_planes(PointCloud::ConstPtr in, 
                  pcl16::PointCloud<pcl16::Normal>::ConstPtr normals,
                  const tf::Transform& trans,
                  vector<PointCloud::Ptr>& good_planes,
                  vector<pcl16::ModelCoefficients>& good_models,
                  vector<pcl16::ModelCoefficients>& bad_models);

  // We need the contents of both planes and hulls, so planes isn't
  // modified here.
  void convex_hulls(const vector<PointCloud::Ptr>& planes,
                    vector<PointCloud::Ptr>& hulls);

  // Send in some information, get back out new planes.
  void publish_planes(const vector<PointCloud::Ptr>& planes,
                      const vector<PointCloud::Ptr>& hulls,
                      const vector<pcl16::ModelCoefficients>& coeffs,
                      vector<semanticmodel::Plane>& outputs);

  // Note that we start working with the un-voxelized cloud again.
  void get_aboves(const vector<PointCloud::Ptr>& hulls,
                  const vector<pcl16::ModelCoefficients>& coeffs,
                  const PointCloud::Ptr& densecloud,
                  PointCloud::Ptr& out);

  // Throw out those "above" points that fit one of the bad_coeffs planes
  // too well. This helps us get rid of extraneous "wall" objects.
  void lose_bads(const PointCloud::ConstPtr& aboves,
                 PointCloud::Ptr good_aboves,
                 const vector<pcl16::ModelCoefficients>& bad_coeffs);

  // Finally, the callback that actually drives everything.
  void synchronized_pipeline_callback(
      const sensor_msgs::ImageConstPtr& rgb_img,
      const sensor_msgs::CameraInfoConstPtr& rgb_info,
      const sensor_msgs::ImageConstPtr& depth_image,
      const sensor_msgs::CameraInfoConstPtr& depth_info,
      const PointCloud::ConstPtr& cloud);
  
  // Timer to publish latest detected planes
  void publishLatestPlanes(const ros::WallTimerEvent& e);

private:
  // How big are our queues?
  static const int QUEUE_SIZE = 100;
  
  // We are a node.
  ros::NodeHandle handle;

  // For dynamic_reconfigure support.
  FilterConfig config;
  dynamic_reconfigure::Server<FilterConfig> dynparam_srv;
  dynamic_reconfigure::Server<FilterConfig>::CallbackType dynparam_func;
  boost::mutex config_mutex;

  // Finally, the dynamic_reconfigure callback.
  void dynamic_reconfigure_callback(const FilterConfig& config, 
                                    uint32_t level);
  
  // is a plane horizontal to within a threshold?
  bool isHorizontal(const pcl16::ModelCoefficients& coeffs) const;
  
  // is a plane within GROUND_PLANE_THRESHOLD (m) of the ground?
  bool isGroundPlane(const pcl16::ModelCoefficients& coeffs) const;
  
  // Transform coefficients into map frame
  pcl16::ModelCoefficients transformCoeffs(const btTransform& trans,
                                           const pcl16::ModelCoefficients& in);
      
  // tf support; pointers are needed here to enforce that we have a
  // nodehandle before these are built.
  boost::scoped_ptr<tf::TransformListener> listener;
  boost::scoped_ptr<tf::TransformBroadcaster> broadcaster;

  // Squeeze all of our messages together. See the long comment below,
  // too.
  boost::scoped_ptr< message_filters::Synchronizer<Sync> > sync;

  /*
   * We need image_transport to play nicely with ApproximateTime, which it
   * doesn't (automatically), because the ApproximateTime stuff requires
   * message_filter subscriptions, not image_transport subscriptions. We
   * can get around this (cf openni_record_player) using .add<i> on our
   * synchronizer; this means we need barely-op callbacks for the three
   * channels we're watching, plus subscribers for them.
   */
  ros::Subscriber cloud_sub;
  image_transport::ImageTransport rgb_transport; // For the next one.
  image_transport::CameraSubscriber cam_sub; // Gets both.
  image_transport::ImageTransport depth_transport;
  image_transport::CameraSubscriber depth_sub; // Gets both.

  // The barely-op callbacks described above; these just push their
  // messages into the sync queues.
  void cloud_cb(const PointCloud::ConstPtr& cloud);
  void camera_cb(const sensor_msgs::ImageConstPtr& img, 
                 const sensor_msgs::CameraInfoConstPtr& info);
  void depth_cb(const sensor_msgs::ImageConstPtr& img, 
                const sensor_msgs::CameraInfoConstPtr& info);

  // The various things we push out.
  ros::Publisher map_points_pub;  // Just the points transformed to /map.
  ros::Publisher planecloud_pub;
  ros::Publisher plane_pub;
  ros::Publisher object_pub;
  ros::Publisher latest_plane_pub_;

  // For our plane-merging service
  ros::ServiceClient planetracker_srv;
    
  // Plane segmentation  
  pcl16::OrganizedMultiPlaneSegmentation<Point, pcl16::Normal, pcl16::Label> 
  plane_segmenter_;
  
  // Normal estimation
  pcl16::IntegralImageNormalEstimation<Point, pcl16::Normal> normal_estimator_;

  // Pairwise comparator used by segmentation
  boost::shared_ptr<pcl16::EdgeAwarePlaneComparator<Point, pcl16::Normal> > 
  comp_;
 
  // Latest detected planes
  boost::circular_buffer<PointCloud::Ptr> latest_planes_;
  ros::WallTimer latest_plane_timer_;

  // If we're on a robot, we can short-circuit early (as soon as planes
  // are published for the benefit of the head pointer). This is collected
  // from a param at construction-time.
  bool short_circuit;
      
  // Some debugging data.
  int processed;
  int skipped;
};

}  // namespace semanticmodel