detailedplane.cc

Go to the documentation of this file.

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

#include <boost/foreach.hpp>
#define foreach BOOST_FOREACH
#include "ros/ros.h"
#include "opencv2/opencv.hpp"
#include "pcl16_ros/transforms.h"
#include "pcl16/segmentation/extract_polygonal_prism_data.h"
#include "pcl16/sample_consensus/method_types.h"
#include "pcl16/sample_consensus/model_types.h"
#include "pcl16/segmentation/sac_segmentation.h"
#include "pcl16/surface/convex_hull.h"
#include "pcl16/filters/project_inliers.h"
#include "semanticmodel/centroid.hh"
#include "semanticmodel/detailedplane.hh"

namespace semanticmodel
{

DetailedPlane::DetailedPlane(const semanticmodel::Plane& plane,
                             tf::TransformListener* listener)
  : a(plane.a), b(plane.b), c(plane.c), d(plane.d),
    center(plane.center),
    hull(new PointCloud()), cloud(new PointCloud()),
    color(), frame_id("/map")
{
  PointCloud::Ptr tempcl16oud(new PointCloud());
  pcl16::fromROSMsg(plane.cloud, *tempcl16oud);
  pcl16_ros::transformPointCloud(frame_id,
                               *tempcl16oud, *cloud, *listener);
  /*
  ROS_INFO ("Created detailed plane with coeffs (%f, %f, %f, %f) in frame %s",
            a,b,c,d, tempcl16oud->header.frame_id.c_str());
  */

  // Get the hull, in case we don't get it from the message.
  pcl16::ConvexHull<Point> huller;
  // Flatten the cloud since we know z is fixed in the map frame
  ROS_ASSERT(fabs(c-1)<1e-2);
  BOOST_FOREACH (Point& p, cloud->points)
    p.z = -d;
  huller.setInputCloud(cloud);
  huller.setDimension(2);
  huller.reconstruct(*hull);

  static cv::RNG prng(2);
  color.r = prng.uniform(0.0, 1.0);
  color.g = prng.uniform(0.0, 1.0);
  color.b = prng.uniform(0.0, 1.0);
  color.a = 1.0;
}

bool DetailedPlane::overlaps(const DetailedPlane& rhs) const
{
  /*ROS_INFO ("Overlap check: coeffs are (%.2f, %.2f, %.2f, %.2f)"
            " and (%.2f, %.2f, %.2f, %.2f)", a, b, c, d,
            rhs.a, rhs.b, rhs.c, rhs.d);
  */
  foreach(const Point& p, rhs.hull->points)
  {
    // Point-to-plane distance; inlined dot product, sort of.
    double ppdist = (p.x * a) + (p.y * b) + (p.z * c) + d;
    if (fabs(ppdist) > PLANE_ERROR_TOL) {
      return false;
    }
  }

  // I'm not sure I need to go the other direction; let's be safe.
  foreach(const Point& p, hull->points)
  {
    double ppdist = (p.x * rhs.a) + (p.y * rhs.b) + (p.z * rhs.c) + rhs.d;
    if (fabs(ppdist) > PLANE_ERROR_TOL) {
      return false;
    }
  }

  // Now that we've guaranteed that these two planes are equationally similar,
  // we need to do the second step: guaranteeing that I contain a part of your
  // hull, or vice versa.
  if (rhs.hull->points.size()>3)
  {
    foreach(const Point& p, hull->points)
    {
      if (pcl16::isXYPointIn2DXYPolygon(p, *rhs.hull)) {
        return true;
      }
    }
  }

  if (hull->points.size()>3)
  {
    // This time I know we need to go both ways.
    foreach(const Point& p, rhs.hull->points)
    {
      if (pcl16::isXYPointIn2DXYPolygon(p, *hull)) {
        return true;
      }
    }
  }
  return false;
}

double DetailedPlane::area() const
{
  // Adapted from http://alienryderflex.com/polygon_area/, which lists it as
  // public domain.
  double area = 0.0;
  ROS_ASSERT_MSG(fabs(c-1.0)<1e-1, "Invalid plane coefficients %.2f, %.2f, %.2f, %.2f",
                 a, b, c, d);
  int j = hull->size() - 1;
  for (size_t i = 0 ; i < hull->size() ; ++i)
  {
    area += (hull->points[j].x + hull->points[i].x) * 
            (hull->points[j].y - hull->points[i].y);
    j = i;
  }
  return fabs(area * 0.5);
}

// Merging is a bit tricky; we don't want either plane to be priviledged, so
// we pick the larger one. That's imperfect, but it's a start.
void DetailedPlane::mergeFrom(const DetailedPlane& rhs)
{
  if (rhs.cloud->points.size() > cloud->points.size())  // Need to make changes.
  {
    a = rhs.a;
    b = rhs.b;
    c = rhs.c;
    d = rhs.d;

    color = rhs.color;
  }

  // No matter which is larger, we need all the points.
  *cloud += *rhs.cloud;

  // Project onto our preferred plane, and re-compute the convex hull.
  pcl16::ProjectInliers<Point> projector;
  projector.setModelType(pcl16::SACMODEL_PLANE);
  projector.setInputCloud(cloud);

  pcl16::ModelCoefficients::Ptr model(new pcl16::ModelCoefficients());
  model->values.push_back(a);
  model->values.push_back(b);
  model->values.push_back(c);
  model->values.push_back(d);

  projector.setModelCoefficients(model);
  projector.filter(*cloud);

  PointCloud::Ptr newhull(new PointCloud());
  pcl16::ConvexHull<Point> huller;
  huller.setInputCloud(cloud);
  huller.setDimension(2);
  huller.reconstruct(*newhull);
  std::swap(hull, newhull);

  Eigen::Vector4f newcentroid = centroid(cloud);
  center.x = newcentroid[0];
  center.y = newcentroid[1];
  center.z = newcentroid[2];
}

semanticmodel::Plane DetailedPlane::toPlaneMsg()
{
  semanticmodel::Plane out;
  out.header.frame_id = frame_id;
  out.header.stamp = ros::Time(424242.42); // it's not used
  out.a = a;
  out.b = b;
  out.c = c;
  out.d = d;
  out.center = center;
  pcl16::toROSMsg(*hull, out.hull);
  pcl16::toROSMsg(*cloud, out.cloud);
  return out;
}

void DetailedPlane::toHullAndEquationPlaneMsg(semanticmodel::Plane& output)
{
  output.a = a;
  output.b = b;
  output.c = c;
  output.d = d;
  pcl16::toROSMsg(*hull, output.hull);
}

}