segmenter.cc

Go to the documentation of this file.

/*
 * segmenter.cc
 * Mac Mason <mmason@willowgarage.com>
 *
 * Implementation. See segmenter.hh.
 */

#include <sstream>
#include <boost/foreach.hpp>
#define foreach BOOST_FOREACH
#include <boost/format.hpp>

#include "ros/ros.h"
#include "pcl16_ros/transforms.h"
#include "pcl16/filters/passthrough.h"
#include "pcl16/filters/voxel_grid.h"
#include "pcl16/filters/project_inliers.h"
#include "pcl16/filters/extract_indices.h"
#include "pcl16/filters/radius_outlier_removal.h"
#include "pcl16/segmentation/extract_clusters.h"
#include "pcl16/segmentation/extract_polygonal_prism_data.h"
#include "pcl16/kdtree/kdtree.h"
#include "pcl16/surface/convex_hull.h"
#include "pcl16/ros/conversions.h"

#include "semanticmodel/Plane.h"
#include "semanticmodel/centroid.hh"
#include "semanticmodel/segmenter.hh"
#include "semanticmodel/Blobs.h"
#include "semanticmodel/PlaneExchange.h"

typedef pcl16::PointCloud<pcl16::Normal> NormalCloud;

// These are for debugging; I should get rid of them.
std::string CloudsToString(
    const std::vector< pcl16::PointCloud<pcl16::PointXYZRGB>::Ptr >& vec)
{
  std::stringstream ss;
  ss << vec.size() << ": [ ";
  for (size_t i = 0 ; i < vec.size() ; ++i)
  {
    ss << vec[i]->size() << " ";
  }
  ss << "]";
  return ss.str();
}

double degrees(double radians)
{
  return radians * 180.0 / M_PI;
}

// See above, re: debugging.
class ScopedTickTock
{
  public:
    std::string start;
    std::string end;
    ros::Time starttime;
    ScopedTickTock(const std::string& _start, const std::string& _end)
      : start(_start), end(_end), starttime(ros::Time::now())
    {
      ROS_INFO_STREAM("TickTock Start: " << start);
    }
    ~ScopedTickTock()
    {
      ROS_INFO_STREAM("TickTock Stop: " << 
                      end << 
                      " (" << 
                      (ros::Time::now() - starttime).toSec() <<
                      ") sec");

    }
};

namespace semanticmodel
{

Segmenter::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_name)
  : handle(name_space),
    config(),
    dynparam_srv(),
    dynparam_func(),
    config_mutex(),
    listener(NULL),
    broadcaster(NULL),
    sync(NULL),
    cloud_sub(),
    rgb_transport(handle),
    depth_transport(handle),
    cam_sub(),
    depth_sub(),
    map_points_pub(),
    planecloud_pub(),
    plane_pub(),
    object_pub(),
    planetracker_srv(),
    latest_planes_(10),
    short_circuit(false),
    processed(0),
    skipped(0)
{
  // Enforce our input requirements.
  ROS_FATAL_COND(
      cloud_input_topic == "" || camera_input_topic == "",
      "You must provide both cloud_input_topic and image_input_topic!");
  ROS_ASSERT(cloud_input_topic != "" && camera_input_topic != "");

  // Set up dynamic_reconfigure.
  dynparam_func = 
    boost::bind(&Segmenter::dynamic_reconfigure_callback, this, _1, _2);
  dynparam_srv.setCallback(dynparam_func);

  listener.reset(new tf::TransformListener(ros::Duration(600)));
  broadcaster.reset(new tf::TransformBroadcaster());
  
  // Set up normal estimator
  normal_estimator_.setNormalEstimationMethod(
      normal_estimator_.COVARIANCE_MATRIX);
  normal_estimator_.setMaxDepthChangeFactor(0.02f);
  normal_estimator_.setNormalSmoothingSize(20.0f);
  
  // Segmenter
  plane_segmenter_.setMinInliers(1000);
  plane_segmenter_.setAngularThreshold(0.05); // About 3 degrees
  plane_segmenter_.setDistanceThreshold(0.02); // 2 cm
  comp_.reset(new pcl16::EdgeAwarePlaneComparator<Point, pcl16::Normal>());
  plane_segmenter_.setComparator(comp_);

  // See if we're short-circuiting.
  if (!handle.getParam("/short_circuit", short_circuit))
    ROS_DEBUG("short_circuit parameter is missing; using false.");
  else
    ROS_DEBUG_STREAM("Setting short_circuit to " << short_circuit);

  // Get the synchronizer spun up.
  sync.reset(new message_filters::Synchronizer<Sync>(Sync(QUEUE_SIZE)));
  sync->registerCallback(
      boost::bind(&Segmenter::synchronized_pipeline_callback, 
                  this, _1, _2, _3, _4, _5));

  // Subscribe to the topics we want.
  cloud_sub = handle.subscribe(cloud_input_topic, 
                               QUEUE_SIZE, 
                               &Segmenter::cloud_cb,
                               this);
  cam_sub = rgb_transport.subscribeCamera(camera_input_topic, 
                                          QUEUE_SIZE,
                                          &Segmenter::camera_cb,
                                          this);
  depth_sub = depth_transport.subscribeCamera(depth_input_topic, 
                                              QUEUE_SIZE,
                                              &Segmenter::depth_cb,
                                              this);
  // Advertise our outputs.
  map_points_pub = 
    handle.advertise<PointCloud>("/map_points", QUEUE_SIZE);
  planecloud_pub = 
    handle.advertise<PointCloud>(planecloud_output_topic, QUEUE_SIZE);
  plane_pub = 
    handle.advertise<Plane>(plane_output_topic, QUEUE_SIZE);
  object_pub = handle.advertise<Blobs>(object_output_topic, QUEUE_SIZE);
  latest_plane_pub_ =
    handle.advertise<PointCloud>("latest_planes", 10);
  latest_plane_timer_ = handle.createWallTimer(ros::WallDuration(1.0),
                                               &Segmenter::publishLatestPlanes,
                                               this);

  ros::service::waitForService(handle.resolveName(plane_service_name));
  planetracker_srv = 
    handle.serviceClient<semanticmodel::PlaneExchange>(plane_service_name);
  ROS_INFO_NAMED("segmenter", "Segmenter initialized");
}

void Segmenter::passthrough(PointCloud::Ptr& in, PointCloud::Ptr& out)
{
  pcl16::PassThrough<Point> passfilter;
  passfilter.setFilterFieldName("z");
  passfilter.setFilterLimits(config.min_z, config.max_z);
  passfilter.setInputCloud(in);
  passfilter.filter(*out);
}

bool Segmenter::isHorizontal(const pcl16::ModelCoefficients& coeffs) const
{
  return fabs(coeffs.values[2]) >= config.horizontal_dot;
}

bool Segmenter::isGroundPlane (const pcl16::ModelCoefficients& coeffs) const
{
  return (isHorizontal(coeffs) && 
          fabs(coeffs.values[3]) < config.ground_plane_threshold); 
}

pcl16::ModelCoefficients
Segmenter::transformCoeffs (const btTransform& trans,
                            const pcl16::ModelCoefficients& in)
{
  ROS_ASSERT(in.values.size()==4);
  btVector3 normal(in.values[0], in.values[1], in.values[2]); 
  btTransform inv = trans.inverse();
  const btVector3& origin = inv.getOrigin();
  const btQuaternion rot = inv.getRotation();
  btVector3 new_normal = inv.getBasis().transpose()*normal;
  pcl16::ModelCoefficients trans_coeffs;
  trans_coeffs.values.resize(4);
  trans_coeffs.values[0] = new_normal.x();
  trans_coeffs.values[1] = new_normal.y();
  trans_coeffs.values[2] = new_normal.z();
  trans_coeffs.values[3] = in.values[3] + normal.dot(origin); 
 
  /*
  ROS_INFO("Transformed coefficients %.4f, %.4f, %.4f, %.4f based on transform "
           "(%.4f, %.4f, %.4f), (%.4f, %.4f, %.4f, %.4f).  New coeffs are "
           "%.4f, %.4f, %.4f, %.4f", in.values[0], in.values[1], in.values[2],
           in.values[3], origin.x(), origin.y(), origin.z(), rot.x(), rot.y(), 
           rot.z(), rot.w(), trans_coeffs.values[0], trans_coeffs.values[1], 
           trans_coeffs.values[2], trans_coeffs.values[3]);
  */
                   
                   
  
  return trans_coeffs;
}

void Segmenter::fit_planes(PointCloud::ConstPtr in,
                           NormalCloud::ConstPtr normals,
                           const tf::Transform& trans,
                           vector<PointCloud::Ptr>& planes,
                           vector<pcl16::ModelCoefficients>& good_coeffs,
                           vector<pcl16::ModelCoefficients>& bad_coeffs)
{
  ROS_DEBUG_NAMED("planes", "In plane callback");
  typedef pcl16::PointCloud<pcl16::Normal> NormalCloud;
  typedef pcl16::PointCloud<pcl16::Label> LabelCloud;
  
  // Set up inputs and outputs
  vector<pcl16::PlanarRegion<Point> > regions;
  vector<pcl16::ModelCoefficients> coeffs;
  vector<pcl16::PointIndices> inliers, label_inds, boundary_inds;
  LabelCloud::Ptr labels(new LabelCloud());
  plane_segmenter_.setInputNormals(normals);
  plane_segmenter_.setInputCloud(in);
  planes.clear();
  good_coeffs.clear();
  bad_coeffs.clear();
  pcl16::ExtractIndices<Point> extract;
  extract.setInputCloud(in);
  extract.setNegative(false);
  
  // Segment
  // I originally used the version below, which is better about getting the
  // entire plane including the edge.  But it was running into some
  // undersegmentation problems.  E.g., the pool table in
  // 2011-08-16-20-59-00.bag.  So switched to the other version which seems
  // more accurate.  Alex Trevor is supposed to look at this in the next few
  // weeks (April 2012).
  //plane_segmenter_.segmentAndRefine(regions, coeffs, inliers, labels,
  //label_inds, boundary_inds);
  std::vector<Eigen::Vector4f, Eigen::aligned_allocator<Eigen::Vector4f> > centroids;
  std::vector <Eigen::Matrix3f, Eigen::aligned_allocator<Eigen::Matrix3f> > covariances;
  plane_segmenter_.segment(coeffs, inliers, centroids, covariances,
                           *labels, label_inds);
  ROS_DEBUG_NAMED("planes", "Found %zu segments", regions.size());
  
  // Divide into good and bad
  for (size_t i=0; i<coeffs.size(); i++)
  {
    pcl16::ModelCoefficients transformed_coeffs = 
      transformCoeffs(trans.asBt(), coeffs[i]);

    if (isHorizontal(transformed_coeffs))
    {
      PointCloud::Ptr plane_cloud(new PointCloud());
      PointCloud::Ptr transformed(new PointCloud());
      pcl16::PointIndices::Ptr inds(new pcl16::PointIndices(inliers[i]));
      extract.setIndices(inds);
      extract.filter(*plane_cloud);
      pcl16_ros::transformPointCloud(*plane_cloud, *transformed, trans);
      transformed->header.frame_id = config.canonical_frame;
      
      // Correct the z term to deal with numerical issues caused by the 
      // interaction of errors in the coefficient estimate getting multiplied
      // by the potentially large translation term.  
      //
      // Note: This depends on our
      // assumption that horizontal planes have normal (0,0,1) — it would
      // need to be a bit more complicated otherwise.
      float sum=0.0;
      BOOST_FOREACH (const Point& p, transformed->points)
        sum += p.z;
      transformed_coeffs.values[0] = 0;
      transformed_coeffs.values[1] = 0;
      transformed_coeffs.values[2] = 1;
      transformed_coeffs.values[3] = -sum/transformed->points.size();
      
      if (isGroundPlane(transformed_coeffs))
      {
        //ROS_INFO ("Skipping ground plane");
        bad_coeffs.push_back(transformed_coeffs);
        continue;
      }
      planes.push_back(transformed);
      good_coeffs.push_back(transformed_coeffs);
      /*
        ROS_INFO ("Adding plane with z = %.2f in frame %s", transformed_coeffs.values[3],
        transformed->header.frame_id.c_str()); 
        ROS_INFO ("A point in the plane is %.2f, %.2f, %.2f",
        transformed->points[0].x, transformed->points[0].y,
        transformed->points[0].z);
      */
    }
    else
    {
      bad_coeffs.push_back(transformed_coeffs);
    }
  }
  //ROS_WARN("%zu good planes and %zu bad",
  //                good_coeffs.size(), bad_coeffs.size());
}

void Segmenter::convex_hulls(const std::vector<PointCloud::Ptr>& planes,
                             std::vector<PointCloud::Ptr>& hulls)
{
  pcl16::ConvexHull<Point> hull;
  hull.setDimension(2);
  foreach(const PointCloud::Ptr& plane, planes)
  {
    PointCloud::Ptr out(new PointCloud());
    hull.setInputCloud(plane);
    hull.reconstruct(*out);
    hulls.push_back(out);
  }
}

void Segmenter::publish_planes(
    const std::vector<PointCloud::Ptr>& planes,
    const std::vector<PointCloud::Ptr>& hulls,
    const std::vector<pcl16::ModelCoefficients>& coeffs,
    std::vector<Plane>& outputs)
{
  ROS_ASSERT(planes.size() == hulls.size());
  ROS_ASSERT(hulls.size() == coeffs.size());
  // Pushing the plane messages is easy; the tricky part is that we also want
  // to push a tf frame for best_frame, for the benefit of the head pointer.
  // So, we need to know which one is the biggest before we start.
  //
  // At the moment, we're going to publish really boring names for the planes;
  // just the index of each plane.
  if (planes.size() == 0)
    return;

  size_t largest = planes[0]->points.size();
  size_t largest_idx = 0;
  for (size_t i = 0 ; i < planes.size() ; ++i)
  {
    if (planes[i]->points.size() > largest)
    {
      largest = planes[i]->points.size();
      largest_idx = i;
    }
  }

  // Now we have the index of the best plane; let's publish everything.
  PlaneExchange exchange;
  exchange.request.in_planes.planes.resize(planes.size());
  for (size_t i = 0 ; i < planes.size() ; ++i)
  {
    Plane planemsg;
    planemsg.header = planes[i]->header;

    planemsg.a = coeffs[i].values[0];
    planemsg.b = coeffs[i].values[1];
    planemsg.c = coeffs[i].values[2];
    planemsg.d = coeffs[i].values[3];

    Eigen::Vector4f center = centroid(planes[i]);
    planemsg.center.x = center[0];
    planemsg.center.y = center[1];
    planemsg.center.z = center[2];

    pcl16::toROSMsg(*hulls[i], planemsg.hull);
    pcl16::toROSMsg(*planes[i], planemsg.cloud);

    exchange.request.in_planes.planes[i] = planemsg;
    
    // planemsg is built and stored. Now, build the tf transform.
    tf::Transform transform;
    transform.setOrigin(tf::Vector3(center[0], center[1], center[2]));
    transform.setRotation(tf::Quaternion(0, 0, 1.0, 0.0));
    std::string name = (boost::format("plane_%02d") % i).str();
    tf::StampedTransform trans(transform, 
                               planes[i]->header.stamp,
                               config.canonical_frame,
                               name);
    broadcaster->sendTransform(trans);
    
    // An extra one for the best current plane.
    if (i == largest_idx)
    {
      tf::StampedTransform trans2(transform, 
                                  planes[i]->header.stamp,
                                  config.canonical_frame,
                                  "best_plane");
      broadcaster->sendTransform(trans2);
    }
  }
  // Now, transmit our planes, and get the new hulls back.
  planetracker_srv.call(exchange);
  outputs = exchange.response.out_planes.planes;
}

void Segmenter::get_aboves(
    const std::vector<PointCloud::Ptr>& hulls,
    const std::vector<pcl16::ModelCoefficients>& coeffs,
    const PointCloud::Ptr& densecloud,
    PointCloud::Ptr& out)
{
  out->header = densecloud->header;
  foreach(Point& p, *densecloud)
  {
    // We want to use every point that's "above enough" a plane, but isn't
    // _below_ any planes at all. That's not perfect, but it's a start.
    bool add = false;
    for (size_t i = 0 ; i < hulls.size() ; ++i)
    {
      // Point-to-plane distance.
      double ppdist = coeffs[i].values[0] * p.x + 
                      coeffs[i].values[1] * p.y + 
                      coeffs[i].values[2] * p.z + 
                      coeffs[i].values[3];
      if (hulls[i]->points.size()>3 && pcl16::isXYPointIn2DXYPolygon(p, *hulls[i]))
      {
        if (ppdist > config.plane_distance)
        {
          add = true;
        }
        else
        {
          add = false;
          break;
        }
      }
    }
    if (add)
    {
      out->points.push_back(p);
    }
  }
}

void Segmenter::lose_bads(const PointCloud::ConstPtr& aboves,
                          PointCloud::Ptr good_aboves,
                          const vector<pcl16::ModelCoefficients>& bad_coeffs)
{
  good_aboves->header = aboves->header;
  foreach(const Point& p, *aboves)
  {
    bool add = true;
    foreach(pcl16::ModelCoefficients model, bad_coeffs)
    {
      double ppdist = model.values[0] * p.x + 
                      model.values[1] * p.y + 
                      model.values[2] * p.z + 
                      model.values[3];
      if (fabs(ppdist) <= config.plane_tolerance)  // Fits the plane.
      {
        add = false;
        break;
      }
    }
    if (add)
      good_aboves->points.push_back(p);
  }
}

void Segmenter::publishLatestPlanes (const ros::WallTimerEvent& e)
{
  static size_t i=0;
  boost::mutex::scoped_lock lock(config_mutex);

  if (latest_planes_.size()>0) 
  {
    i = (i+1)%latest_planes_.size();
    latest_plane_pub_.publish(*latest_planes_[i]);
  }
}

void Segmenter::synchronized_pipeline_callback(
    const sensor_msgs::ImageConstPtr& rgb_img,
    const sensor_msgs::CameraInfoConstPtr& rgb_info,
    const sensor_msgs::ImageConstPtr& depth_img,
    const sensor_msgs::CameraInfoConstPtr& depth_info,
    const PointCloud::ConstPtr& cloud)
{
  // No changes during a frame, please.
  boost::mutex::scoped_lock lock(config_mutex);

  if (processed + skipped > 0)
  {
    ROS_DEBUG_NAMED("score", 
                    "Processed %d frames. Skipped %d frames. (%f processed)",
                    processed, 
                    skipped, 
                    ((float)processed) / (processed + skipped));
  }
  
  ROS_DEBUG_STREAM_NAMED("timer",
                         "We are behind by " 
                         << ros::Time::now() - rgb_img->header.stamp);
  // Avoid falling too far back
  if (rgb_img->header.stamp + ros::Duration(config.filter_lag_cutoff) < 
      ros::Time::now())
  {
    ROS_WARN_STREAM_NAMED("timer",
                          "Too old! (" << 
                          ros::Time::now() - rgb_img->header.stamp << ")");
    skipped++;
    return;
  }
  else
  {
    ROS_DEBUG_STREAM_NAMED(
        "filter", 
        "Processing filter input at " << rgb_img->header.stamp);
    processed++;
  }
  
  
  // Get the transform between the camera and canonical frames
  tf::StampedTransform trans;
  bool found = 
    listener->waitForTransform(config.canonical_frame, cloud->header.frame_id,
                               cloud->header.stamp, ros::Duration(1.0));
  try 
  {
    if (found)
      listener->lookupTransform(config.canonical_frame, cloud->header.frame_id,
                                cloud->header.stamp, trans);
  }
  catch (tf::TransformException& e)
  {
    found = false;
  }
  if (!found)
  {
    ROS_WARN_STREAM("Couldn't find transform between " << 
                    cloud->header.frame_id << 
                    " and " << 
                    config.canonical_frame << 
                    " at " 
                    <<
                    cloud->header.stamp << "; skipping");
    return;
  }
  PointCloud::Ptr transformed(new PointCloud()); 
  pcl16_ros::transformPointCloud(*cloud, *transformed, trans);
  transformed->header.frame_id = config.canonical_frame;
//  btVector3 origin = trans.getOrigin().asBt();
//  btQuaternion rot = trans.getRotation().asBt();
  //ROS_INFO ("Transform is (%.4f, %.4f, %.4f), (%.4f, %.4f, %.4f, %.4f)",
  //         origin.x(), origin.y(), origin.z(), rot.x(), rot.y(), rot.z(), rot.w());
  
  // Step 0: estimate normals (for untransformed cloud)
  NormalCloud::Ptr normals(new NormalCloud());
  normal_estimator_.setInputCloud(cloud);
  normal_estimator_.compute(*normals);
  
  // Step 1: find horizontal planes 
  // Inputs are untransformed, outputs are transformed
  std::vector<PointCloud::Ptr> good_planes;
  std::vector<pcl16::ModelCoefficients> good_coeffs, bad_coeffs;
  float* distance_map = normal_estimator_.getDistanceMap();
  comp_->setDistanceMap(distance_map); // Needed by fit_planes
  fit_planes(cloud, normals, trans, good_planes, good_coeffs, bad_coeffs);

  // Step 3. Convex hulls. We need to keep the plane points, so we can't
  // clobber the planes variable.
  std::vector<PointCloud::Ptr> hulls;
  convex_hulls(good_planes, hulls); 
  if (!ros::ok())
    return;

  // Step 4. Send our planes to the tracker, and get back the new hulls.
  std::vector<Plane> returnedplanes;
  publish_planes(good_planes, hulls, good_coeffs, returnedplanes);
  BOOST_FOREACH (PointCloud::Ptr plane, good_planes) 
    latest_planes_.push_back(plane);
  if (short_circuit || !ros::ok())
    return;

  // Parse the newplanes out.
  hulls.clear();
  good_coeffs.clear();
  foreach(Plane& p, returnedplanes)
  {
    hulls.push_back(PointCloud::Ptr(new PointCloud()));
    pcl16::fromROSMsg(p.hull, *hulls.back());
    good_coeffs.push_back(pcl16::ModelCoefficients());
    good_coeffs.back().values.resize(4);
    good_coeffs.back().values[0] = p.a;
    good_coeffs.back().values[1] = p.b;
    good_coeffs.back().values[2] = p.c;
    good_coeffs.back().values[3] = p.d;
  }

  // Step 5: we go back to working in the dense plane, here.
  map_points_pub.publish(transformed);
  if (transformed->points.size() == 0 || !ros::ok())
  {
    ROS_ERROR("No transformed points in filter");
    return;
  }

  PointCloud::Ptr aboves(new PointCloud());
  get_aboves(hulls, good_coeffs, transformed, aboves);
  if (aboves->points.size() == 0 || !ros::ok())
  {
    ROS_DEBUG_NAMED("aboves", "No above points; skipping");
    return;
  }

  // Step 5.5. Given that we have points that are "above enough" the
  // horizontal planes in the scene, we now want to throw out those that are
  // too good of a fit to the "bad" (non-horizontal) planes. The idea here is
  // to get rid of points on walls.
  PointCloud::Ptr good_aboves(new PointCloud());
  lose_bads(aboves, good_aboves, bad_coeffs);

  aboves.reset();

  if (good_aboves->points.size() == 0 || !ros::ok())
  {
    ROS_WARN("No good above points; skipping.");
    return;
  }

  /*
   * NEW VERSION.
   * Doing the extra object processing is expensive. Rather than do all of it,
   * we punt: send the simple aboves over the wire, and have the object
   * tracker keep it all together. We'd do lose_bads too, but it requires the
   * bad_coeffs variable, which I don't want to (also) ship over the wire.
   */
  Blobs msg;
  msg.blobs.resize(1);  // No clustering on this end.
  msg.rgb_info = *rgb_info;
  msg.rgb_image = *rgb_img;
  msg.depth_info = *depth_info;
  msg.depth_image = *depth_img;
  pcl16::toROSMsg(*good_aboves, msg.blobs.at(0));
  object_pub.publish(msg);
}

void Segmenter::dynamic_reconfigure_callback(const FilterConfig& config, 
                                             uint32_t level)
{
  // We ignore level, because everybody does.
  boost::mutex::scoped_lock lock(config_mutex);
  this->config = config;  // Well, that was easy.
}

// These next two functions just add stuff to sync's queue; it handles the
// rest.
void Segmenter::cloud_cb(const PointCloud::ConstPtr& cloud)
{
  sync->add<4>(cloud); 
}

void Segmenter::camera_cb(const sensor_msgs::ImageConstPtr& img, 
                          const sensor_msgs::CameraInfoConstPtr& info)
{
  sync->add<0>(img);
  sync->add<1>(info);
}

void Segmenter::depth_cb(const sensor_msgs::ImageConstPtr& img, 
                         const sensor_msgs::CameraInfoConstPtr& info)
{
  sync->add<2>(img);
  sync->add<3>(info);
}

}  // namespace semanticmodel