detailedplane.cc
Go to the documentation of this file.
00001 /*
00002  * detailedplane.cc
00003  * Mac Mason <mmason@willowgarage.com>
00004  *
00005  * Implementation. See semanticmodel/detailedplane.hh.
00006  */
00007 
00008 #include <boost/foreach.hpp>
00009 #define foreach BOOST_FOREACH
00010 #include "ros/ros.h"
00011 #include "opencv2/opencv.hpp"
00012 #include "pcl16_ros/transforms.h"
00013 #include "pcl16/segmentation/extract_polygonal_prism_data.h"
00014 #include "pcl16/sample_consensus/method_types.h"
00015 #include "pcl16/sample_consensus/model_types.h"
00016 #include "pcl16/segmentation/sac_segmentation.h"
00017 #include "pcl16/surface/convex_hull.h"
00018 #include "pcl16/filters/project_inliers.h"
00019 #include "semanticmodel/centroid.hh"
00020 #include "semanticmodel/detailedplane.hh"
00021 
00022 namespace semanticmodel
00023 {
00024 
00025 DetailedPlane::DetailedPlane(const semanticmodel::Plane& plane,
00026                              tf::TransformListener* listener)
00027   : a(plane.a), b(plane.b), c(plane.c), d(plane.d),
00028     center(plane.center),
00029     hull(new PointCloud()), cloud(new PointCloud()),
00030     color(), frame_id("/map")
00031 {
00032   PointCloud::Ptr tempcl16oud(new PointCloud());
00033   pcl16::fromROSMsg(plane.cloud, *tempcl16oud);
00034   pcl16_ros::transformPointCloud(frame_id,
00035                                *tempcl16oud, *cloud, *listener);
00036   /*
00037   ROS_INFO ("Created detailed plane with coeffs (%f, %f, %f, %f) in frame %s",
00038             a,b,c,d, tempcl16oud->header.frame_id.c_str());
00039   */
00040 
00041   // Get the hull, in case we don't get it from the message.
00042   pcl16::ConvexHull<Point> huller;
00043   // Flatten the cloud since we know z is fixed in the map frame
00044   ROS_ASSERT(fabs(c-1)<1e-2);
00045   BOOST_FOREACH (Point& p, cloud->points)
00046     p.z = -d;
00047   huller.setInputCloud(cloud);
00048   huller.setDimension(2);
00049   huller.reconstruct(*hull);
00050 
00051   static cv::RNG prng(2);
00052   color.r = prng.uniform(0.0, 1.0);
00053   color.g = prng.uniform(0.0, 1.0);
00054   color.b = prng.uniform(0.0, 1.0);
00055   color.a = 1.0;
00056 }
00057 
00058 bool DetailedPlane::overlaps(const DetailedPlane& rhs) const
00059 {
00060   /*ROS_INFO ("Overlap check: coeffs are (%.2f, %.2f, %.2f, %.2f)"
00061             " and (%.2f, %.2f, %.2f, %.2f)", a, b, c, d,
00062             rhs.a, rhs.b, rhs.c, rhs.d);
00063   */
00064   foreach(const Point& p, rhs.hull->points)
00065   {
00066     // Point-to-plane distance; inlined dot product, sort of.
00067     double ppdist = (p.x * a) + (p.y * b) + (p.z * c) + d;
00068     if (fabs(ppdist) > PLANE_ERROR_TOL) {
00069       return false;
00070     }
00071   }
00072 
00073   // I'm not sure I need to go the other direction; let's be safe.
00074   foreach(const Point& p, hull->points)
00075   {
00076     double ppdist = (p.x * rhs.a) + (p.y * rhs.b) + (p.z * rhs.c) + rhs.d;
00077     if (fabs(ppdist) > PLANE_ERROR_TOL) {
00078       return false;
00079     }
00080   }
00081 
00082   // Now that we've guaranteed that these two planes are equationally similar,
00083   // we need to do the second step: guaranteeing that I contain a part of your
00084   // hull, or vice versa.
00085   if (rhs.hull->points.size()>3)
00086   {
00087     foreach(const Point& p, hull->points)
00088     {
00089       if (pcl16::isXYPointIn2DXYPolygon(p, *rhs.hull)) {
00090         return true;
00091       }
00092     }
00093   }
00094 
00095   if (hull->points.size()>3)
00096   {
00097     // This time I know we need to go both ways.
00098     foreach(const Point& p, rhs.hull->points)
00099     {
00100       if (pcl16::isXYPointIn2DXYPolygon(p, *hull)) {
00101         return true;
00102       }
00103     }
00104   }
00105   return false;
00106 }
00107 
00108 double DetailedPlane::area() const
00109 {
00110   // Adapted from http://alienryderflex.com/polygon_area/, which lists it as
00111   // public domain.
00112   double area = 0.0;
00113   ROS_ASSERT_MSG(fabs(c-1.0)<1e-1, "Invalid plane coefficients %.2f, %.2f, %.2f, %.2f",
00114                  a, b, c, d);
00115   int j = hull->size() - 1;
00116   for (size_t i = 0 ; i < hull->size() ; ++i)
00117   {
00118     area += (hull->points[j].x + hull->points[i].x) * 
00119             (hull->points[j].y - hull->points[i].y);
00120     j = i;
00121   }
00122   return fabs(area * 0.5);
00123 }
00124 
00125 // Merging is a bit tricky; we don't want either plane to be priviledged, so
00126 // we pick the larger one. That's imperfect, but it's a start.
00127 void DetailedPlane::mergeFrom(const DetailedPlane& rhs)
00128 {
00129   if (rhs.cloud->points.size() > cloud->points.size())  // Need to make changes.
00130   {
00131     a = rhs.a;
00132     b = rhs.b;
00133     c = rhs.c;
00134     d = rhs.d;
00135 
00136     color = rhs.color;
00137   }
00138 
00139   // No matter which is larger, we need all the points.
00140   *cloud += *rhs.cloud;
00141 
00142   // Project onto our preferred plane, and re-compute the convex hull.
00143   pcl16::ProjectInliers<Point> projector;
00144   projector.setModelType(pcl16::SACMODEL_PLANE);
00145   projector.setInputCloud(cloud);
00146 
00147   pcl16::ModelCoefficients::Ptr model(new pcl16::ModelCoefficients());
00148   model->values.push_back(a);
00149   model->values.push_back(b);
00150   model->values.push_back(c);
00151   model->values.push_back(d);
00152 
00153   projector.setModelCoefficients(model);
00154   projector.filter(*cloud);
00155 
00156   PointCloud::Ptr newhull(new PointCloud());
00157   pcl16::ConvexHull<Point> huller;
00158   huller.setInputCloud(cloud);
00159   huller.setDimension(2);
00160   huller.reconstruct(*newhull);
00161   std::swap(hull, newhull);
00162 
00163   Eigen::Vector4f newcentroid = centroid(cloud);
00164   center.x = newcentroid[0];
00165   center.y = newcentroid[1];
00166   center.z = newcentroid[2];
00167 }
00168 
00169 semanticmodel::Plane DetailedPlane::toPlaneMsg()
00170 {
00171   semanticmodel::Plane out;
00172   out.header.frame_id = frame_id;
00173   out.header.stamp = ros::Time(424242.42); // it's not used
00174   out.a = a;
00175   out.b = b;
00176   out.c = c;
00177   out.d = d;
00178   out.center = center;
00179   pcl16::toROSMsg(*hull, out.hull);
00180   pcl16::toROSMsg(*cloud, out.cloud);
00181   return out;
00182 }
00183 
00184 void DetailedPlane::toHullAndEquationPlaneMsg(semanticmodel::Plane& output)
00185 {
00186   output.a = a;
00187   output.b = b;
00188   output.c = c;
00189   output.d = d;
00190   pcl16::toROSMsg(*hull, output.hull);
00191 }
00192 
00193 }


semanticmodel
Author(s): Julian ("Mac") Mason
autogenerated on Thu Dec 12 2013 12:39:10