blob_store.cc

Go to the documentation of this file.

/*
 * Copyright (c) 2008, Willow Garage, Inc.
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *
 *     * Redistributions of source code must retain the above copyright
 *       notice, this list of conditions and the following disclaimer.
 *     * Redistributions in binary form must reproduce the above copyright
 *       notice, this list of conditions and the following disclaimer in the
 *       documentation and/or other materials provided with the distribution.
 *     * Neither the name of the Willow Garage, Inc. nor the names of its
 *       contributors may be used to endorse or promote products derived from
 *       this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 *
 */

#include <semanticmodel/blob_store.h>
#include "blob_properties.h"
#include <boost/foreach.hpp>
#include <cv_bridge/cv_bridge.h>
#include <opencv/cv.h>
#include <pcl16/ros/conversions.h>
#include <sensor_msgs/image_encodings.h>
#include <boost/optional.hpp>
#include <tf/exceptions.h>
#include <pcl16/features/normal_3d.h>
#include <pcl16/filters/extract_indices.h>

namespace semanticmodel
{

namespace mr=mongo_ros;
namespace sm=sensor_msgs;
namespace enc=sensor_msgs::image_encodings;
namespace gm=geometry_msgs;

using std::vector;
using std::string;
using std::map;
using std::max;
using std::min;

typedef boost::mutex::scoped_lock Lock;
typedef vector<float> Color;
typedef map<string, Color> ColorMap;
typedef pcl16::PointXYZRGB Point;
typedef pcl16::PointCloud<Point> PointCloud;
typedef boost::mutex::scoped_lock Lock;

BlobStore::BlobStore (const string& canonical_frame,
                      const string& rgb_camera_frame,
                      const string& db_name,
                      const string& collection_namespace) 
  : canonical_frame_(canonical_frame),
    rgb_camera_frame_(rgb_camera_frame),
    base_frame_("/base_footprint"),
    db_name_(db_name), 
    max_head_angular_speed_(0.3), 
    it_(nh_), 
    tf_(ros::Duration(600)),
    pub_(it_.advertise("last_added_blob", 10)),
    switch_srv_(nh_.advertiseService("switch_blob_db", 
                                     &BlobStore::switchDb, this))
{
//  ROS_INFO_STREAM("storing collections in " << db_name << "/" <<
//                  collection_namespace);
  SwitchDB::Request req;
  SwitchDB::Response resp;
  req.collection_namespace = collection_namespace;
  switchDb(req, resp);
}

bool BlobStore::switchDb (SwitchDB::Request& req,
                          SwitchDB::Response& resp)
{
  Lock l(mutex_);
  images_.reset(
      new ImageCollection(db_name_, req.collection_namespace + "_images"));
  blobs_.reset(
      new BlobCollection(db_name_, req.collection_namespace + "_blobs"));
  ROS_DEBUG_STREAM_NAMED ("blob_store", "Successfully switched to " <<
                          req.collection_namespace);
  return true;
}

gm::PointStamped makePoint(const Point& p, const ros::Time& t, const string& f)
{
  gm::PointStamped pt;
  pt.point.x = p.x;
  pt.point.y = p.y;
  pt.point.z = p.z;
  pt.header.stamp = t;
  pt.header.frame_id = f;
  return pt;
}

btQuaternion BlobStore::headAngleAt (const ros::Time& t)
{
  const ros::Duration timeout(0.5);
  const string base = "/map";

  // no need to wait: see previous comment
  //tf_.waitForTransform(base, camera, t, timeout);
  tf::StampedTransform trans;
  tf_.lookupTransform(base, rgb_camera_frame_, t, trans);
  return trans.getRotation().asBt();
}

// To get a single number summarizing how fast the head is moving wrt the map,
// take the norm of the vector of the rates of change of the Euler angles
double BlobStore::headAngularSpeed (const ros::Time& t)
{
  try
  {
    const double DT=0.5;
    ros::Duration d(DT);
    btMatrix3x3 m0(headAngleAt(t-d));
    btMatrix3x3 m1(headAngleAt(t+d));
    double a0, b0, c0, a1, b1, c1;
    m0.getEulerYPR(a0, b0, c0);
    m1.getEulerYPR(a1, b1, c1);
    const double da = (a1-a0)/(2*DT);
    const double db = (b1-b0)/(2*DT);
    const double dc = (c1-c0)/(2*DT);
    return sqrt(da*da+db*db+dc*dc);
  }
  catch (tf::TransformException& e)
  {
    ROS_WARN_STREAM("Received tf exception when computing head speed for time " 
                     << t << ", so returning large value (42)."
                     << " Exception was " << e.what());
    return 42;
  }
  
}

sm::Image::Ptr BlobStore::highlightBlobInImage(const Blob& blob,
                                               const sm::Image& img,
                                               const sm::CameraInfo& info) 
{
  const ros::Time acquisition_time = img.header.stamp;

  ROS_DEBUG_STREAM_NAMED ("blob_store",
                          "Highlighting in image with acquisition time "
                          << acquisition_time);

  // Convert to open cv
  cv_bridge::CvImagePtr image;
  image = cv_bridge::toCvCopy(img, enc::BGR8);

  // Cam model initialize from info
  cam_model_.fromCameraInfo(info);
  const string target_frame = cam_model_.tfFrame();

  // Figure out corners of bounding box
  ROS_ASSERT(!blob.cloud->points.empty());
  boost::optional<double> min_x, min_y, max_x, max_y;

  // Wait for the transform that we'll need inside the loop
  // Actually not
  /*
  const ros::Duration timeout(0.5);
  ROS_DEBUG_STREAM_THROTTLE_NAMED (1.0, "blob_store",
                                   "Waiting for transform to " << target_frame
                                   << " from " << canonical_frame_ << " at "
                                   << acquisition_time);
  tf_.waitForTransform(target_frame, canonical_frame_,
                       acquisition_time, timeout);
  */

  tf::StampedTransform trans;
  tf_.lookupTransform(target_frame, canonical_frame_,
                      acquisition_time, trans);

  vector<cv::Point> points;
  BOOST_FOREACH (const Point& pcl16_pt, blob.cloud->points)
  {
    const tf::Point in(pcl16_pt.x, pcl16_pt.y, pcl16_pt.z);
    const tf::Point out = trans*in;

    // Convert to open cv
    cv::Point3d pt_cv(out.x(), out.y(), out.z());
    cv::Point2d image_pt = cam_model_.project3dToPixel(pt_cv);
    points.push_back(image_pt);

    if (!min_x || *min_x > image_pt.x)
      min_x = image_pt.x;
    if (!min_y || *min_y > image_pt.y)
      min_y = image_pt.y;
    if (!max_x || *max_x < image_pt.x)
      max_x = image_pt.x;
    if (!max_y || *max_y < image_pt.y)
      max_y = image_pt.y;
  }
  cv::Mat points_mat(points);
  vector<int> hull;
  cv::convexHull(points_mat, hull);
  const int n = hull.size();

  cv::Point* poly = new cv::Point[n];
  for (int i=0; i<n; i++)
    poly[i] = points[hull[i]];
  cv::polylines(image->image, (const cv::Point**) &poly, &n, 1, true,
                CV_RGB(0, 255, 0), 2);

  ROS_ASSERT(min_x && min_y && max_x && max_y);
  /*cv::rectangle(image->image, cv::Point2d(*min_x, *min_y),
    cv::Point2d(*max_x, *max_y), CV_RGB(0, 255, 0), 2);*/


  delete [] poly;

  // Convert back to ros message and return
  return image->toImageMsg();
}

double entropy(const vector<unsigned>& hist)
{
  double sum=0;
  const unsigned n = hist.size();
  BOOST_FOREACH (const unsigned c, hist)
    sum += c;
  double ent=0;
  for (unsigned i=0; i<n; i++)
  {
    const double p = (float)hist[i]/sum;
//    ROS_INFO ("Normalizing %u to %.2f", hist[i], p);
    if (hist[i]>0)
      ent -= p*log(p)/log(2);
  }
//  ROS_INFO ("Entropy is %.2f", ent);
  return ent;
}

void normalize (const pcl16::Normal& n, double* x, double* y, double* z)
{
  *x = n.normal[0];
  *y = n.normal[1];
  *z = n.normal[2];
  const double r = sqrt(pow(*x, 2) + pow(*y, 2) + pow(*z, 2));
  *x = n.normal[0]/r;
  *y = n.normal[1]/r;
  *z = n.normal[2]/r;
}

unsigned numNormalDirections (const vector<unsigned>& hist)
{
  unsigned n=0;
  const unsigned NORMAL_DIRECTION_THRESHOLD=50;
  BOOST_FOREACH (const unsigned i, hist)
    if (i>NORMAL_DIRECTION_THRESHOLD)
      n++;
  return n;
}

mr::Metadata blobMetadata (const Blob& blob)
{
//  ROS_INFO ("Computing blob metadata");
  // Set up normal estimation class
  pcl16::NormalEstimation<Point, pcl16::Normal> ne;
  ne.setInputCloud(blob.cloud);

  // We only want normals at a subset of the points for efficiency
  const unsigned MAX_NUMBER_POINTS = 1000;
  const unsigned num_points = blob.cloud->points.size();
  pcl16::PointIndices::Ptr indices(new pcl16::PointIndices());
  const unsigned step = ceil((double)num_points/MAX_NUMBER_POINTS);
//  ROS_INFO ("Num points is %u, using step %u", num_points, step);
  for (unsigned ind=0; ind<num_points; ind+=step)
    indices->indices.push_back(ind);
//  ROS_INFO ("Using %zu indices", indices->indices.size());
  ne.setIndices(indices);

  // Do the normal computation
  pcl16::search::KdTree<Point>::Ptr tree(new pcl16::search::KdTree<Point>);
  ne.setSearchMethod(tree);
  pcl16::PointCloud<pcl16::Normal>::Ptr normals(new pcl16::PointCloud<pcl16::Normal>());
  ne.setRadiusSearch(0.03);
  ne.compute(*normals);
//  ROS_INFO ("Computed %zu normals", normals->size());
  //const pcl16::Normal& n = normals->points[0];
//  ROS_INFO ("First normal is %.2f, %.2f %.2f, with curvature %.2f",
//      n.normal[0], n.normal[1], n.normal[2], n.curvature);

  // Geometry analysis
  unsigned num_vertical = 0;
  const double VERTICALITY_THRESHOLD=0.9;
  vector<unsigned> c(10);
  const unsigned NUM_LONGITUDE_BINS=10;
  vector<unsigned> longitude_hist(NUM_LONGITUDE_BINS);
  BOOST_FOREACH (const pcl16::Normal& n, normals->points)
  {
    double x=42, y=42, z=42;
    normalize(n, &x, &y, &z);
    unsigned i = n.curvature < 0.5 ? floor(n.curvature*20) : 9;
    c.at(i)++;
    if (z>VERTICALITY_THRESHOLD)
      num_vertical++;
    else
    {
      const double PI=3.1415;
      const double longitude = atan2(y, x);
      const double normalized = min(0.999, max((longitude+PI)/(2*PI), 0.0));
      const unsigned ind=floor(normalized*NUM_LONGITUDE_BINS);
      longitude_hist.at(ind)++;
    }
  }

  // Compute other info and return
  const GeometricInfo geom_info = centroidAndDiameter(*blob.cloud);
  const double np=normals->points.size();
  return mr::Metadata().append("color", blobColorName(blob)).
    append("id", blob.id).
    append("diameter", geom_info.d).
    append("x", geom_info.x).
    append("y", geom_info.y).
    append("z", geom_info.z).
    append("num_normals", np).
    append("num_vertical", num_vertical).
    append("c01", (c[0]+c[1])/np).
    append("c0", c[0]/np).
    append("c23", (c[2]+c[3])/np).
    append("c2", c[2]/np).
    append("c4", (c[4]+c[5]+c[6]+c[7]+c[8]+c[9])/np).
    append("num_normal_directions", numNormalDirections(longitude_hist));
}



mr::Metadata imageMetadata (const Blob& blob, const gm::Pose2D& viewpoint)
{
  return mr::Metadata("cluster_id", blob.id, "x",
                      viewpoint.x, "y", viewpoint.y,
                      "theta", viewpoint.theta);
}

vector<gm::Point> projectPoints (const Blob& blob)
{
  vector<gm::Point> points;
  BOOST_FOREACH (const Point& pt, blob.hull->points)
  {
    gm::Point p;
    p.x = pt.x;
    p.y = pt.y;
    p.z = -42.424242;
    points.push_back(p);
  }
  return points;
}

gm::Pose2D BlobStore::getViewpoint (const sm::Image& img)
{
  const ros::Time& acquisition_time = img.header.stamp;
  const ros::Duration timeout(0.5);
  // No need to wait: it's old, so if we don't have it now we won't get it later
  // tf_.waitForTransform(canonical_frame_, base_frame_, acquisition_time, timeout);
  
  tf::StampedTransform tr;
  tf_.lookupTransform(canonical_frame_, base_frame_, acquisition_time, tr);
  
  gm::Pose2D pose;
  pose.x = tr.getOrigin().x();
  pose.y = tr.getOrigin().y();
  pose.theta = tf::getYaw(tr.getRotation());
  return pose;
}

void BlobStore::update (const vector<Blob*>& blobs,
                        const sm::Image& img,
                        const sm::CameraInfo& img_info)

{
  Lock l(mutex_);
  ROS_ASSERT_MSG (blobs_ && images_, "Update was called on BlobStore, but"
                  " DB collections have not been initialized yet.  Skipping.");
  const double head_angular_speed = headAngularSpeed(img.header.stamp);
  
  typedef mr::MessageWithMetadata<BlobMessage>::ConstPtr BlobWithMetadata;
  std::set<int> seen_ids, existing_ids;
  ROS_DEBUG_STREAM_NAMED(
      "blob_store", 
      "Received update at " << img.header.stamp << " with " <<
      blobs.size() << " blobs; head angular speed is " <<
      head_angular_speed);
  BOOST_FOREACH (Blob* blob, blobs)
  {
    ROS_DEBUG_STREAM_NAMED ("blob_store", "Processing blob " << blob->id);
    seen_ids.insert(blob->id);
    mr::Query q("id", blob->id);
    vector<BlobWithMetadata> matches = blobs_->pullAllResults(q);
    if (matches.empty())
    {
      mr::Metadata metadata = blobMetadata(*blob);
      metadata.append("head_speed", head_angular_speed);

      try
      {
        sm::Image::Ptr updated = highlightBlobInImage(*blob, img, img_info);
        gm::Pose2D viewpoint = getViewpoint(img);
        BlobMessage m;
        pcl16::toROSMsg(*blob->cloud, m.points);
        m.image = *updated;
        m.hull = projectPoints(*blob);
        blobs_->insert(m, metadata);
        pub_.publish(updated);
        images_->insert(*updated, imageMetadata(*blob, viewpoint));
      }
      catch (tf::TransformException& e)
      {
        ROS_WARN_STREAM ("Transform exception when storing blob: " << e.what());
      }
    }

    // If there is already a cluster with the given id, possibly update its
    // info
    else
    {
      ROS_ASSERT(matches.size()==1);
      PointCloud existing;
      const int id = matches[0]->lookupInt("id");

      // Get metadata for the existing image so we can delete it later
      typedef mr::MessageWithMetadata<sm::Image>::ConstPtr ImageWithMetadata;
      vector<ImageWithMetadata> image_matches =
        images_->pullAllResults(mr::Query("cluster_id", id));
      ROS_ASSERT(image_matches.size()==1);

      // Convert cloud to pcl16.  Note pcl16 conversion dies on empty clouds
      if (matches[0]->points.data.empty())
      {
        ROS_DEBUG_NAMED ("blob_store", "Skipping empty cloud");
        continue;
      }
      pcl16::fromROSMsg(matches[0]->points, existing);
      
      // Quick check: if number of points has not changed, we won't update the
      // data for this object.  This also ensures that the object is visible
      // in the image
      const size_t num_points = blob->cloud->points.size();
      const size_t num_existing = existing.points.size();
      if (num_existing==num_points)
      {
        ROS_DEBUG_NAMED ("blob_store", "No new points for %d; skipping", id);
        continue;
      }

      // Try to update the metadata and store an image with the object
      // highlighted.  This can fail due to transform timeouts (between the
      // camera and map frames), in which case we just skip this object this
      // time.
      try
      {
        mr::Metadata metadata = blobMetadata(*blob); 
        const double prev_speed = matches[0]->lookupDouble("head_speed");

        // These next two could theoretically throw tf exceptions
        const sm::Image::Ptr updated = 
          highlightBlobInImage(*blob, img, img_info);
        const gm::Pose2D viewpoint = getViewpoint(img);
        
        BlobMessage m;
        pcl16::toROSMsg(*blob->cloud, m.points);
        m.image = *updated;
        m.hull = projectPoints(*blob);

        // If the head is moving fast relative to the base, we don't store the
        // image because it's likely to be blurred.   
        if (prev_speed < head_angular_speed - max_head_angular_speed_/5)
        {
          ROS_DEBUG_STREAM_NAMED ("blob_store", "For " << id << ", speed " <<
                                  head_angular_speed << " too high relative to "
                                  << prev_speed << "; not updating image");
          metadata.append("head_speed", prev_speed);
        }
        else
        {
          images_->insert(*updated, imageMetadata(*blob, viewpoint));
          metadata.append("head_speed", head_angular_speed);

          const unsigned num_removed =
            images_->removeMessages(image_matches[0]->metadata);
          ROS_ASSERT(num_removed==1);
        }

        // Regardless of the head speed, we'll update the semantic metadata,
        // by inserting the new cluster and removing the old one.  Note that
        // due to the lack of transactions in mongo, both messages coexist in
        // the db temporarily.  Queries by id should therefore always only use
        // the most recent message for a given id.
        blobs_->insert(m, metadata);
        const unsigned num_removed = 
          blobs_->removeMessages(matches[0]->metadata);
        ROS_ASSERT(num_removed==1);
        ROS_DEBUG_STREAM_NAMED ("blob_store", "Removed message " <<
                                matches[0]->metadata);
        
      }
      catch (tf::TransformException& e)
      {
        ROS_WARN_STREAM("Transform exception when updating blob: " << e.what());
      }
    }
  }
  
  // Remove dangling ids
  BOOST_FOREACH (BlobWithMetadata b, blobs_->queryResults(mr::Query(), true))
    existing_ids.insert(b->lookupInt("id"));

  BOOST_FOREACH (const int id, existing_ids)
  {
    if (seen_ids.find(id)==seen_ids.end())
    {
      ROS_DEBUG_STREAM_NAMED ("blob_store", "Removing id " << id);
      blobs_->removeMessages(mr::Query("id", id));
      images_->removeMessages(mr::Query("cluster_id", id));
    }      
  }
}

} // namespace