main.cpp
Go to the documentation of this file.
00001 /*
00002 Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
00003 All rights reserved.
00004 
00005 Redistribution and use in source and binary forms, with or without
00006 modification, are permitted provided that the following conditions are met:
00007     * Redistributions of source code must retain the above copyright
00008       notice, this list of conditions and the following disclaimer.
00009     * Redistributions in binary form must reproduce the above copyright
00010       notice, this list of conditions and the following disclaimer in the
00011       documentation and/or other materials provided with the distribution.
00012     * Neither the name of the Universite de Sherbrooke nor the
00013       names of its contributors may be used to endorse or promote products
00014       derived from this software without specific prior written permission.
00015 
00016 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
00017 ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
00018 WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
00019 DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
00020 DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
00021 (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00022 LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
00023 ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
00024 (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
00025 SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
00026 */
00027 
00028 #include <pcl/point_types.h>
00029 #include <pcl/point_cloud.h>
00030 #include <pcl/io/pcd_io.h>
00031 #include <pcl/ModelCoefficients.h>
00032 #include <pcl/common/common.h>
00033 #include <pcl/common/pca.h>
00034 #include <pcl/filters/extract_indices.h>
00035 #include <pcl/segmentation/sac_segmentation.h>
00036 #include <pcl/segmentation/extract_clusters.h>
00037 
00038 #include <rtabmap/utilite/UConversion.h>
00039 
00040 void showUsage()
00041 {
00042         printf("\nUsage: extractObject [options] cloud.pcd\n"
00043                         "Options:\n"
00044                         "   -p #.#            plane distance threshold (default 0.02 m)\n"
00045                         "   -a #.#            plane angle tolerance from z axis (default PI/6)\n"
00046                         "   -c #.#            cluster tolerance (default 0.1 m)\n"
00047                         "   -s #              minimum cluster size (default 50 points)\n"
00048                         "   -center_obj       center the objects to their local reference\n"
00049                         "   -save_plane       save the plane inliers to \"extracted_plane.pcd\"\n");
00050 }
00051 
00052 int main(int argc, char *argv[])
00053 {
00054         if(argc < 2)
00055         {
00056                 showUsage();
00057                 return -1;
00058         }
00059 
00060         std::string cloudPath = argv[argc-1];
00061         double planeDistanceThreshold = 0.02f;
00062         double planeEpsAngle = 3.1416/6.0;
00063         double clusterTolerance = 0.1f;
00064         int minClusterSize = 50;
00065         bool centerObject = false;
00066         bool savePlane = false;
00067 
00068         for(int i=1; i<argc-1; ++i)
00069         {
00070                 if(strcmp(argv[i], "-p") == 0)
00071                 {
00072                         ++i;
00073                         if(i < argc)
00074                         {
00075                                 planeDistanceThreshold = uStr2Float(argv[i]);
00076                                 if(planeDistanceThreshold < 0.0f)
00077                                 {
00078                                         showUsage();
00079                                 }
00080                         }
00081                         else
00082                         {
00083                                 showUsage();
00084                         }
00085                         continue;
00086                 }
00087                 if(strcmp(argv[i], "-a") == 0)
00088                 {
00089                         ++i;
00090                         if(i < argc)
00091                         {
00092                                 planeEpsAngle = uStr2Float(argv[i]);
00093                                 if(planeEpsAngle < 0.0f)
00094                                 {
00095                                         showUsage();
00096                                 }
00097                         }
00098                         else
00099                         {
00100                                 showUsage();
00101                         }
00102                         continue;
00103                 }
00104                 if(strcmp(argv[i], "-c") == 0)
00105                 {
00106                         ++i;
00107                         if(i < argc)
00108                         {
00109                                 clusterTolerance = uStr2Float(argv[i]);
00110                                 if(clusterTolerance <= 0.0f)
00111                                 {
00112                                         showUsage();
00113                                 }
00114                         }
00115                         else
00116                         {
00117                                 showUsage();
00118                         }
00119                         continue;
00120                 }
00121                 if(strcmp(argv[i], "-s") == 0)
00122                 {
00123                         ++i;
00124                         if(i < argc)
00125                         {
00126                                 minClusterSize = std::atoi(argv[i]);
00127                                 if(minClusterSize < 1)
00128                                 {
00129                                         showUsage();
00130                                 }
00131                         }
00132                         else
00133                         {
00134                                 showUsage();
00135                         }
00136                         continue;
00137                 }
00138                 if(strcmp(argv[i], "-center_obj") == 0)
00139                 {
00140                         centerObject = true;
00141                         continue;
00142                 }
00143                 if(strcmp(argv[i], "-save_plane") == 0)
00144                 {
00145                         savePlane = true;
00146                         continue;
00147                 }
00148                 printf("Unrecognized option : %s\n", argv[i]);
00149                 showUsage();
00150         }
00151 
00152         printf("Parameters:\n"
00153                         "   planeDistanceThreshold=%f\n"
00154                         "   planeEpsAngle=%f\n"
00155                         "   clusterTolerance=%f\n"
00156                         "   minClusterSize=%d\n"
00157                         "   centerObject=%s\n"
00158                         "   savePlane=%s\n",
00159                         planeDistanceThreshold,
00160                         planeEpsAngle,
00161                         clusterTolerance,
00162                         minClusterSize,
00163                         centerObject?"true":"false",
00164                         savePlane?"true":"false");
00165 
00166         printf("Loading \"%s\"...", cloudPath.c_str());
00167         pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
00168         pcl::io::loadPCDFile(cloudPath, *cloud);
00169         printf("done! (%d points)\n", (int)cloud->size());
00170 
00171         // Extract plane
00172         pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients);
00173         pcl::PointIndices::Ptr inliers (new pcl::PointIndices);
00174         // Create the segmentation object
00175         pcl::SACSegmentation<pcl::PointXYZRGB> seg;
00176         // Optional
00177         seg.setOptimizeCoefficients (true);
00178         // Mandatory
00179         seg.setModelType (pcl::SACMODEL_PLANE);
00180         seg.setMethodType (pcl::SAC_RANSAC);
00181         seg.setMaxIterations(1000);
00182         seg.setDistanceThreshold (planeDistanceThreshold);
00183         seg.setAxis(Eigen::Vector3f(0,0,1));
00184         seg.setEpsAngle (planeEpsAngle);
00185 
00186         seg.setInputCloud (cloud);
00187         seg.segment (*inliers, *coefficients);
00188 
00189         printf("Plane coefficients: %f %f %f %f\n", coefficients->values[0],
00190                                                                                   coefficients->values[1],
00191                                                                                   coefficients->values[2],
00192                                                                                   coefficients->values[3]);
00193 
00194         float a = coefficients->values[0];
00195         float b = coefficients->values[1];
00196         float c = coefficients->values[2];
00197         float d = coefficients->values[3];
00198 
00199         // Create a quaternion for rotation into XY plane
00200         Eigen::Vector3f current(a, b, c);
00201         Eigen::Vector3f target(0.0, 0.0, 1.0);
00202         Eigen::Quaternion<float> q;
00203         q.setFromTwoVectors(current, target);
00204         Eigen::Matrix4f trans;
00205         trans.topLeftCorner<3,3>() = q.toRotationMatrix();
00206 
00207         float planeShift = -d;
00208 
00209         // Create transformed point cloud (polygon is aligned with XY plane)
00210         pcl::PointCloud<pcl::PointXYZRGB>::Ptr output (new pcl::PointCloud<pcl::PointXYZRGB> ());
00211         pcl::transformPointCloud(*cloud, *output, Eigen::Vector3f(-a*planeShift, -b*planeShift, -c*planeShift), q);
00212 
00213         if(savePlane)
00214         {
00215                 pcl::io::savePCDFile("extracted_plane.pcd", *output, inliers->indices);
00216                 printf("Saved extracted_plane.pcd (%d points)\n", (int)inliers->indices.size());
00217         }
00218         else
00219         {
00220                 printf("Plane size = %d points\n", (int)inliers->indices.size());
00221         }
00222 
00223         // remove plane inliers
00224         pcl::ExtractIndices<pcl::PointXYZRGB> extract;
00225         extract.setNegative (true);
00226         extract.setInputCloud (output);
00227         extract.setIndices(inliers);
00228         extract.filter (*output);
00229 
00230         // Get the biggest cluster
00231         pcl::search::KdTree<pcl::PointXYZRGB>::Ptr kdTree(new pcl::search::KdTree<pcl::PointXYZRGB>);
00232         kdTree->setInputCloud(output);
00233         std::vector<pcl::PointIndices> cluster_indices;
00234         pcl::EuclideanClusterExtraction<pcl::PointXYZRGB> ec;
00235         ec.setClusterTolerance (clusterTolerance);
00236         ec.setMinClusterSize (minClusterSize);
00237         ec.setMaxClusterSize (200000);
00238         ec.setSearchMethod (kdTree);
00239         ec.setInputCloud (output);
00240         ec.extract (cluster_indices);
00241 
00242         if(cluster_indices.size() == 0)
00243         {
00244                 printf("No object found! (minimum cluster size=%d)\n", minClusterSize);
00245                 return 1;
00246         }
00247 
00248         for(unsigned int i=0; i<cluster_indices.size(); ++i)
00249         {
00250                 pcl::PointCloud<pcl::PointXYZRGB>::Ptr object (new pcl::PointCloud<pcl::PointXYZRGB> ());
00251                 pcl::copyPointCloud(*output, cluster_indices.at(i), *object);
00252 
00253                 if(centerObject)
00254                 {
00255                         // recenter the object on xy plane, and set min z to 0
00256                         Eigen::Vector4f min, max;
00257                         pcl::getMinMax3D(*object, min, max);
00258 
00259                         pcl::transformPointCloud(*object, *object, Eigen::Vector3f(-(max[0]+min[0])/2.0f, -(max[1]+min[1])/2.0f, -min[2]), Eigen::Quaternion<float>(0,0,0,0));
00260                 }
00261 
00262                 pcl::io::savePCDFile(uFormat("extracted_object%d.pcd", (int)i+1), *object);
00263                 printf("Saved extracted_object%d.pcd (%d points)\n", (int)i+1, (int)object->size());
00264         }
00265 
00266         return 0;
00267 }


rtabmap
Author(s): Mathieu Labbe
autogenerated on Fri Aug 28 2015 12:51:31