blob.cc

Go to the documentation of this file.

/*
 * blob.cc
 * Mac Mason <mmason@willowgarage.com>
 *
 * Implementation. See include/semanticmodel/blob.hh.
 */

#include <limits>
#include <vector>
#include <boost/foreach.hpp>
#define foreach BOOST_FOREACH
#include "ros/ros.h"  // For debugging.
#include "pcl16/surface/convex_hull.h"
#include "semanticmodel/blob.hh"

namespace semanticmodel
{
  Blob::Blob(const PointCloud::ConstPtr& cluster)
    : cloud(new PointCloud(*cluster)), hull(new PointCloud()), id(next_id++)
  {
    /*
     * TODO TODO TODO:
     * We really should be giving each blob its own coordinate frame, and
     * using tf to keep track of where they are.
     */
    hull->header.frame_id = cloud->header.frame_id;
    hullerize(cluster);
  }

  bool Blob::overlaps(const Blob& rhs) const
  {
    return oneSidedIntersect(hull, rhs.hull) ||
           oneSidedIntersect(rhs.hull, hull);
  }

  void Blob::mergeFrom(const Blob& rhs)
  {
    // For the moment, give merged blobs the lesser of their unique ids.
    id = std::min(id, rhs.id);
    (*cloud) += *(rhs.cloud);
    (*hull) += *(rhs.hull);
    hullerize(hull);
  }

  void Blob::RGB(float& r, float& g, float& b) const
  {
    r = g = b = 0.0;
    foreach(Point& p, *cloud)
    {
      // PCL, you are so very odd.
      uint32_t rgb = *reinterpret_cast<int*>(&p.rgb);
      uint8_t rb = (rgb >> 16) & 0x0000ff;
      uint8_t gb = (rgb >> 8)  & 0x0000ff;
      uint8_t bb = (rgb)       & 0x0000ff;

      r += rb / 255.0;
      g += gb / 255.0;
      b += bb / 255.0;
    }

    r /= cloud->points.size();
    g /= cloud->points.size();
    b /= cloud->points.size();
  }

  void Blob::size(double& x, double& y, double& z) const
  {
    float minx = cloud->points[0].x;
    float maxx = cloud->points[0].x;
    float miny = cloud->points[0].y;
    float maxy = cloud->points[0].y;
    float minz = cloud->points[0].z;
    float maxz = cloud->points[0].z;
    foreach(const Point& p, *cloud)
    {
      minx = std::min(minx, p.x);
      maxx = std::max(maxx, p.x);
      miny = std::min(miny, p.y);
      maxy = std::max(maxy, p.y);
      minz = std::min(minz, p.z);
      maxz = std::max(maxz, p.z);
    }
    x = maxx - minx;
    y = maxy - miny;
    z = maxz - minz;
  }

  // Note abuse of Blob::PointCloud typedef to mean pcl16::PointCloud<...>. This
  // way, we can keep our typedefs private.
  Blob::PointCloud::Ptr Blob::flatten(PointCloud::Ptr& in, double z)
  {
    PointCloud::Ptr res(new PointCloud(*in));
    foreach(Point& p, *res)
      p.z = z;
    return res;
  }

  void Blob::MergeBlobWithSet(std::vector<Blob*>& objects, Blob* blob)
  {
    // Hooray for helper functions.
    objects.push_back(blob);
    while (MergeBlobWithSetHelper(objects)) {};
  }

  bool Blob::MergeBlobWithSetHelper(std::vector<Blob*>& objects)
  {
    Blob* end = objects.back();
    for (size_t i = 0 ; i < objects.size() - 1 ; ++i)
    {
      if (objects[i]->overlaps(*end))
      {
        ROS_DEBUG_STREAM_NAMED ("merge_blobs", "Merging blob " << end->id <<
                                " into " << objects[i]->id);
        objects[i]->mergeFrom(*end);
        delete end;
        objects.pop_back();
        std::swap(objects[i], objects.back());
        return true;
      }
    }
    return false;
  }

  // See above comment about typedef abuse.
  void Blob::hullerize(const Blob::PointCloud::ConstPtr& cloud)
  {
    (*hull) += (*cloud);  // Not necessarily flat, or a hull, after this.
    PointCloud::Ptr flat(Blob::flatten(hull, 0.0));
    pcl16::ConvexHull<Point> huller;
    huller.setInputCloud(flat);
    huller.setDimension(2); // Ok since we flattened it
    huller.reconstruct(*hull); // Now it's flat and a hull.
  }

  // Yet more typedef abuse.
  bool Blob::oneSidedIntersect(const Blob::PointCloud::ConstPtr& p1,
                               const Blob::PointCloud::ConstPtr& p2) const
  {
    size_t n = p1->points.size();
    for (size_t i = 0 ; i < n ; ++i)
    {
      size_t j;
      if (i > 0)
        j = i - 1;
      else
        j = n - 1;

      double min1, max1, min2, max2;
      projectOntoNormal(p1, p1->points[i], p1->points[j], min1, max1);
      projectOntoNormal(p2, p1->points[i], p1->points[j], min2, max2);
      if (max2 < min1 || max1 < min2)
        return false;
    }
    return true;
  }

  // Yet yet more typedef abuse.
  void Blob::projectOntoNormal(const Blob::PointCloud::ConstPtr& poly, 
                               const Point& q1, const Point& q2,
                               double& inf, double& sup) const
  {
    Eigen::Vector2f d;
    d[0] = q2.x - q1.x;
    d[1] = q2.y - q1.y;
    Eigen::Vector2f perp(d[1], -d[0]);
    perp.normalize();
    inf = std::numeric_limits<double>::infinity();
    sup = std::numeric_limits<double>::infinity();
    foreach(const Point& p, *poly)
    {
      Eigen::Vector2f v(p.x, p.y);
      float proj = perp.dot(v);
      if (!pcl_isfinite(inf) || proj < inf)
        inf = proj;
      if (!pcl_isfinite(sup) || proj > sup)
        sup = proj;
    }
    assert (pcl_isfinite(inf));
    assert (pcl_isfinite(sup));
  }

  unsigned int Blob::next_id = 0;

}