plane.cpp
Go to the documentation of this file.
00001 #include <srs_env_model_percp/but_plane_detector/plane.h>
00002 
00003 namespace srs_env_model_percp {
00004 
00005 PlaneExt::PlaneExt(but_plane_detector::Plane<float> plane) : but_plane_detector::Plane<float> (0.0, 0.0, 0.0, 0.0), planeCoefficients(new pcl::ModelCoefficients())
00006 {
00007         a = plane.a;
00008         b = plane.b;
00009         c = plane.c;
00010         d = plane.d;
00011         norm = plane.norm;
00012 
00013         // Create a quaternion for rotation into XY plane
00014         Eigen::Vector3f current(a, b, c);
00015         Eigen::Vector3f target(0.0, 0.0, 1.0);
00016         Eigen::Quaternion<float> q;
00017         q.setFromTwoVectors(current, target);
00018         planeTransXY = q.toRotationMatrix();
00019 
00020         planeShift = -d;
00021 
00022         color.r = abs(a) / 2.0 + 0.2;
00023         color.g = abs(b) / 2.0 + 0.2;
00024         color.b = abs(d) / 5.0;
00025         color.a = 1.0;
00026 
00027         // Plane coefficients pre-calculation...
00028         planeCoefficients->values.push_back(a);
00029         planeCoefficients->values.push_back(b);
00030         planeCoefficients->values.push_back(c);
00031         planeCoefficients->values.push_back(d);
00032 
00033 }
00034 
00035 PlaneExt::tVertices PlaneExt::ComputeConcaveHull(pcl::PointCloud<pcl::PointXYZ>::Ptr &plane_cloud, pcl::PointCloud<pcl::PointXYZ>::Ptr &plane_hull)
00036 {
00037     pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_projected (new pcl::PointCloud<pcl::PointXYZ> ());
00038 
00039     // project all points onto current plane
00040     pcl::ProjectInliers<pcl::PointXYZ> proj;
00041     proj.setModelType (pcl::SACMODEL_PLANE);
00042     proj.setInputCloud (plane_cloud);
00043     proj.setModelCoefficients (planeCoefficients);
00044     proj.filter(*cloud_projected);
00045 
00046     // create the concave hull of the plane
00047     pcl::ConcaveHull<pcl::PointXYZ > chull;
00048     chull.setInputCloud (cloud_projected);
00049     chull.setAlpha(0.15);
00050 
00051     tVertices polys;
00052     chull.reconstruct(*plane_hull, polys);
00053 
00054     if (polys.size() > MAX_POLYS)
00055                 return tVertices();
00056 
00057     return polys;
00058 }
00059 
00060 ClipperLib::ExPolygons PlaneExt::PolygonizeConcaveHull(pcl::PointCloud<pcl::PointXYZ>::Ptr &plane_hull, tVertices &polygon_indices)
00061 {
00062         ClipperLib::ExPolygons polygon;
00063 
00064         // Create transformed point cloud (polygon is aligned with XY plane
00065         pcl::PointCloud<pcl::PointXYZ>::Ptr plane_hull_proj (new pcl::PointCloud<pcl::PointXYZ> ());
00066         pcl::transformPointCloud(*plane_hull, *plane_hull_proj, Eigen::Vector3f(-a*planeShift, -b*planeShift, -c*planeShift), Eigen::Quaternion<float>(0,0,0,0));
00067         pcl::transformPointCloud(*plane_hull_proj, *plane_hull_proj, planeTransXY);
00068 
00069         // save each point into Clipper lib polygon structure
00070         if (plane_hull_proj->points.size() > 0)
00071         {
00072                 for (unsigned int i = 0; i < polygon_indices.size(); ++i)
00073                 {
00074                         ClipperLib::ExPolygon clipperPoly;
00075                         clipperPoly.outer.resize(polygon_indices[i].vertices.size());
00076 
00077                         for (unsigned int j = 0; j < polygon_indices[i].vertices.size(); ++j)
00078                         {
00079                                 clipperPoly.outer[j].X = CONVERT_TO_LONG(plane_hull_proj->points[polygon_indices[i].vertices[j]].x);
00080                                 clipperPoly.outer[j].Y = CONVERT_TO_LONG(plane_hull_proj->points[polygon_indices[i].vertices[j]].y);
00081                         }
00082 
00083                         // Orientation check
00084                         if (!ClipperLib::Orientation(clipperPoly.outer))
00085                         {
00086                                 ClipperLib::ReversePolygon(clipperPoly.outer);
00087                         }
00088                 polygon.push_back(clipperPoly);
00089                 }
00090         }
00091         return polygon;
00092 }
00093 
00094 void PlaneExt::ConcaveHullRewrite(pcl::PointCloud<pcl::PointXYZ>::Ptr &plane_hull, tVertices &polygon_indices)
00095 {
00096         // Rewrite saved polygon (previous points will be deleted)
00097         planePolygonsClipper = PolygonizeConcaveHull(plane_hull, polygon_indices);
00098 }
00099 
00100 bool PlaneExt::ConcaveHullJoinCurrent(pcl::PointCloud<pcl::PointXYZ>::Ptr &plane_hull, tVertices &polygon_indices, int max_poly_size)
00101 {
00102         // Join new polygon with current
00103         ClipperLib::ExPolygons newPoly = PolygonizeConcaveHull(plane_hull, polygon_indices);
00104 
00105         std::cerr << "polygonized" << std::endl;
00106 
00107         if (newPoly.size() > max_poly_size) {
00108 
00109                 std::cerr << "too big (new) polygon" << std::endl;
00110                 return false;
00111 
00112         }
00113 
00114         if (planePolygonsClipper.size() > max_poly_size) {
00115 
00116                 std::cerr << "too big (exist) polygon" << std::endl;
00117                 return false;
00118 
00119         }
00120 
00121         ClipperLib::Clipper clipper;
00122         // insert all existing polygons
00123         std::cerr << "add existing pol " << planePolygonsClipper.size() << std::endl;
00124         for (unsigned int i = 0; i < planePolygonsClipper.size(); ++i) {
00125                 try {
00126 
00127                         clipper.AddPolygon(planePolygonsClipper[i].outer, ClipperLib::ptSubject);
00128 
00129                 } catch (const char* Message) {
00130 
00131                         std::cerr << "excp" << std::endl;
00132 
00133                 }
00134 
00135         }
00136 
00137         // insert all new polygons
00138         std::cerr << "add new pol " << newPoly.size() << std::endl;
00139         for (unsigned int i = 0; i < newPoly.size(); ++i) {
00140 
00141                 try {
00142 
00143                 clipper.AddPolygon(newPoly[i].outer, ClipperLib::ptClip);
00144 
00145                 } catch  (const char* Message) {
00146 
00147                         std::cerr << "excp" << std::endl;
00148 
00149                 }
00150 
00151         }
00152 
00153         // execute join operation
00154         std::cerr << "executing join... polygons: " << planePolygonsClipper.size() + newPoly.size() << std::endl;
00155         clipper.Execute(ClipperLib::ctUnion, planePolygonsClipper);
00156         std::cerr << "executed join!" << std::endl;
00157 
00158         return true;
00159 }
00160 
00161 void PlaneExt::TriangulatePlanePolygon()
00162 {
00163         // clear Marker and Shape messages
00164         planeTriangles.points.clear();
00165         planeTrianglesSRS.clear();
00166 
00167         // for all polygons representing this plane
00168         for (unsigned int polygon_i = 0; polygon_i < planePolygonsClipper.size(); ++polygon_i)
00169         {
00170                 planeTrianglesSRS.push_back(cob_3d_mapping_msgs::Shape());
00171                 planeTrianglesSRS.back().type = cob_3d_mapping_msgs::Shape::POLYGON;
00172                 planeTrianglesSRS.back().params.push_back(a);
00173                 planeTrianglesSRS.back().params.push_back(b);
00174                 planeTrianglesSRS.back().params.push_back(c);
00175                 planeTrianglesSRS.back().params.push_back(d);
00176                 planeTrianglesSRS.back().holes.push_back(false);
00177 
00178                 // convert each polygon to polygonizer DS
00179         TPPLPoly triPolygon;
00180         triPolygon.Init(planePolygonsClipper[polygon_i].outer.size());
00181         pcl::PointCloud<pcl::PointXYZ> current_point_cloud;
00182 
00183                 for (unsigned int i = 0; i < planePolygonsClipper[polygon_i].outer.size(); ++i)
00184                 {
00185                         triPolygon[i].x = CONVERT_FROM_LONG(planePolygonsClipper[polygon_i].outer[i].X);
00186                         triPolygon[i].y = CONVERT_FROM_LONG(planePolygonsClipper[polygon_i].outer[i].Y);
00187 
00188                         // additionaly, insert this point into shape message
00189                         pcl::PointXYZ point;
00190                         point.x = triPolygon[i].x;
00191                         point.y = triPolygon[i].y;
00192                         point.z = 0;
00193                         current_point_cloud.push_back(point);
00194                 }
00195 
00196                 // triangulate
00197                 TPPLPartition triangulation;
00198                 std::list<TPPLPoly> triangles;
00199                 triangulation.Triangulate_EC(&triPolygon, &triangles);
00200 
00201 
00202                 // create a message object
00203                 for (std::list<TPPLPoly>::iterator it = triangles.begin(); it != triangles.end(); ++it)
00204                 {
00205                         for (unsigned int j = 0; j < it->GetNumPoints(); ++j)
00206                         {
00207                                 Eigen::Vector3f vec;
00208                                 vec(0) = it->GetPoint(j).x;
00209                                 vec(1) = it->GetPoint(j).y;
00210                                 vec(2) = 0;
00211                                 vec = planeTransXY.inverse() * vec;
00212                                 geometry_msgs::Point p;
00213                                 p.x = vec(0) + planeShift*a;
00214                                 p.y = vec(1) + planeShift*b;
00215                                 p.z = vec(2) + planeShift*c;
00216 
00217                                 planeTriangles.points.push_back(p);
00218                         }
00219                 }
00220 
00221                 // insert polygon point cloud into Shape message
00222                 pcl::transformPointCloud(current_point_cloud, current_point_cloud, planeTransXY.inverse());
00223                 pcl::transformPointCloud(current_point_cloud, current_point_cloud, Eigen::Vector3f(a*planeShift, b*planeShift, c*planeShift), Eigen::Quaternion<float>(0,0,0,0));
00224 
00225                 sensor_msgs::PointCloud2 cloud;
00226                 pcl::toROSMsg(current_point_cloud, cloud);
00227                 planeTrianglesSRS.back().points.push_back(cloud);
00228                 planeTrianglesSRS.back().centroid.x = planeShift*a;
00229                 planeTrianglesSRS.back().centroid.y = planeShift*b;
00230                 planeTrianglesSRS.back().centroid.z = planeShift*c;
00231                 planeTrianglesSRS.back().color = color;
00232                 planeTriangles.color = color;
00233         }
00234 
00235         // compute centroid
00236 }
00237 
00238 visualization_msgs::Marker PlaneExt::NewPlanePoints(pcl::PointCloud<pcl::PointXYZ>::Ptr plane_cloud)
00239 {
00240         // Init a new plane hull (previous will be deleted
00241         pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_hull (new pcl::PointCloud<pcl::PointXYZ>());
00242         tVertices polygons = ComputeConcaveHull(plane_cloud, cloud_hull);
00243 
00244         // revrite hull
00245         ConcaveHullRewrite(cloud_hull, polygons);
00246 
00247         // triangulate
00248         TriangulatePlanePolygon();
00249 
00250         return planeTriangles;
00251 }
00252 
00253 visualization_msgs::Marker PlaneExt::AddPlanePoints(pcl::PointCloud<pcl::PointXYZ>::Ptr plane_cloud, int max_poly_size)
00254 {
00255         // Add new plane hull
00256         pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_hull (new pcl::PointCloud<pcl::PointXYZ>());
00257         std::cerr << "computing concave hull" << std::endl;
00258         tVertices polygons = ComputeConcaveHull(plane_cloud, cloud_hull);
00259 
00260         // Join with current
00261         std::cerr << "concave hull join" << std::endl;
00262         ConcaveHullJoinCurrent(cloud_hull, polygons, max_poly_size);
00263 
00264         // Triangulate
00265         std::cerr << "triangulating polygon" << std::endl;
00266         TriangulatePlanePolygon();
00267 
00268         return planeTriangles;
00269 }
00270 
00271 visualization_msgs::Marker &PlaneExt::getMeshMarker()
00272 {
00273         return planeTriangles;
00274 }
00275 
00276 PlaneExt::tShapeMarker &PlaneExt::getShapeMarker()
00277 {
00278         return planeTrianglesSRS;
00279 }
00280 
00281 void PlaneExt::setColor(std_msgs::ColorRGBA &new_color)
00282 {
00283         planeTriangles.color = new_color;
00284         for (unsigned int i = 0; i < planeTrianglesSRS.size(); ++i)
00285                 planeTrianglesSRS[i].color = new_color;
00286 
00287         color = new_color;
00288 }
00289 
00290 ClipperLib::ExPolygons &PlaneExt::getPolygons()
00291 {
00292         return planePolygonsClipper;
00293 }
00294 
00295 void PlaneExt::setPolygons(ClipperLib::ExPolygons &polys)
00296 {
00297         planePolygonsClipper = polys;
00298 }
00299 
00300 }


srs_env_model_percp
Author(s): Rostislav Hulik (ihulik@fit.vutbr.cz), Tomas Hodan, Michal Spanel (spanel@fit.vutbr.cz)
autogenerated on Sun Jan 5 2014 11:51:56