compare_point_clouds.cpp
Go to the documentation of this file.
00001 /* 
00002  * Copyright (c) 2010, Florian Zacherl <Florian.Zacherl1860@mytum.de>, Dejan Pangercic <dejan.pangercic@cs.tum.edu>
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  * 
00008  *     * Redistributions of source code must retain the above copyright
00009  *       notice, this list of conditions and the following disclaimer.
00010  *     * Redistributions in binary form must reproduce the above copyright
00011  *       notice, this list of conditions and the following disclaimer in the
00012  *       documentation and/or other materials provided with the distribution.
00013  *     * Neither the name of the Intelligent Autonomous Systems Group/
00014  *       Technische Universitaet Muenchen nor the names of its contributors 
00015  *       may be used to endorse or promote products derived from this software 
00016  *       without specific prior written permission.
00017  * 
00018  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00019  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00020  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00021  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00022  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00023  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00024  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00025  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00026  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00027  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00028  * POSSIBILITY OF SUCH DAMAGE.
00029  */
00030 
00031 #include <ros/ros.h>
00032 #include <sensor_msgs/PointCloud.h>
00033 #include <sensor_msgs/PointCloud2.h>
00034 #include <opencv/cv.h>
00035 #include <pcl/io/pcd_io.h>
00036 #include <pcl/point_types.h>
00037 #include <pcl/registration/icp.h>
00038 #include <sensor_msgs/point_cloud_conversion.h>
00039 #include <dirent.h>
00040 #include <sys/stat.h>
00041 #include <string.h>
00042 #include <math.h>
00043 #include <stdio.h>
00044 
00045 //#define OUTPUT_RAW; //If defined, just the pure values without any description are returned
00046 
00047 using namespace std;
00048 
00049 class PCLCompare
00050 {
00051 protected:
00052   pcl::PointCloud<pcl::PointXYZ> model;
00053   pcl::PointCloud<pcl::PointXYZ> cloud;
00054 
00055   const static float tol = 0.008;
00056 
00057   int output;
00058 
00059   int number;
00060 
00061 public:
00062   int compare(int compare_mode)
00063   {
00064     switch (compare_mode)
00065     {
00066       //Just returns the size of the point cloud
00067       case 0:
00068       {
00069         int r = cloud.points.size();
00070         return r;
00071       }
00072         break;
00073 
00074         //Returns the number of points that are equal with a given tolerance
00075         //Therefore the point clouds have to be in the exactly same coordinate frame!!
00076       case 1:
00077       {
00078         int point_count = 0;
00079         for (unsigned int i = 0; i < cloud.points.size(); i++)
00080         {
00081           for (unsigned int j = 0; j < model.points.size(); j++)
00082           {
00083             if (abs(model.points[j].x - cloud.points[i].x) < tol && abs(model.points[j].y - cloud.points[i].y) < tol
00084                 && abs(model.points[j].z - cloud.points[i].z) < tol)
00085             {
00086               point_count++;
00087               break;
00088             }
00089           }
00090         }
00091         return point_count;
00092       }
00093         break;
00094 
00095         //Returns the ICP-fitness score:
00096       case 2:
00097       {
00098         pcl::IterativeClosestPoint < pcl::PointXYZ, pcl::PointXYZ > icp;
00099         icp.setMaximumIterations(100);
00100         icp.setTransformationEpsilon(1e-6);
00101         icp.setInputCloud(boost::make_shared<pcl::PointCloud<pcl::PointXYZ> >(cloud));
00102         icp.setInputTarget(boost::make_shared<pcl::PointCloud<pcl::PointXYZ> >(model));
00103         pcl::PointCloud < pcl::PointXYZ > res;
00104         icp.align(res);
00105         return 10e6 * icp.getFitnessScore(numeric_limits<double>::max());
00106       }
00107         break;
00108 
00109       default:
00110         ROS_INFO("Invalid compare mode!");
00111         return -1;
00112     }
00113   }
00114 
00115   void getFiles(string path)
00116   {
00117     vector<int> val[3];
00118     DIR *pDIR;
00119     struct dirent *entry;
00120     struct stat s;
00121 
00122     if ((pDIR = opendir(path.c_str()))) //Given folder was found
00123     {
00124       while ((entry = readdir(pDIR))) //Read all entries in the folder
00125       {
00126         stringstream stst;
00127         stst << path << "/" << entry->d_name;
00128         stat(stst.str().c_str(), &s);
00129         if ((strcmp(entry->d_name, ".") != 0 && strcmp(entry->d_name, "..") != 0)) //Ignore . and ..
00130         {
00131           if (S_ISDIR(s.st_mode)) //If entry is a folder:
00132           {
00133             cout << stst.str() << ":" << endl; //Return folder name
00134             getFiles(stst.str()); //Recursive call
00135           }
00136           else //If entry is no folder
00137           {
00138             string file(entry->d_name);
00139             if (strcmp(file.substr(file.size() - 3, 3).c_str(), "pcd") == 0) //Use just .pcd files
00140             {
00141               //Read point cloud:
00142               sensor_msgs::PointCloud2 cloud_mess1;
00143               if (pcl::io::loadPCDFile(stst.str(), cloud_mess1) == -1)
00144               {
00145                 ROS_ERROR("Couldn't read file!");
00146                 return;
00147               }
00148               pcl::fromROSMsg(cloud_mess1, cloud);
00149               number++;
00150 
00151               //Save values for all three compare modes:
00152               for (int i = 0; i < 3; i++)
00153               {
00154                 val[i].push_back(compare(i));
00155               }
00156             }
00157           }
00158         }
00159       }
00160 
00161       //Compute average value and standard deviation:
00162       if (val[0].size() > 0)
00163       {
00164         int avg[3] = {0, 0, 0};
00165         int sd[3] = {0, 0, 0};
00166         for (int i = 0; i < 3; i++)
00167         {
00168           int sum = 0;
00169           for (unsigned int j = 0; j < val[0].size(); j++)
00170           {
00171             sum += val[i][j];
00172           }
00173           avg[i] = sum / val[0].size();
00174 
00175           for (unsigned int j = 0; j < val[0].size(); j++)
00176           {
00177             sd[i] += (avg[i] - val[i][j]) * (avg[i] - val[i][j]);
00178           }
00179           sd[i] = sqrt(sd[i] / val[0].size());
00180         }
00181 
00182 #ifdef OUTPUT_RAW
00183         if (output == 0 || output == 3)
00184         cout << avg[0] << endl;
00185         if (output == 1 || output == 3)
00186         cout << avg[1] << endl;
00187         if (output == 2 || output == 3)
00188         cout << avg[2] << endl;
00189 #else
00190         //Output:
00191         if (output == 0 || output == 3)
00192           cout << "Average number of points: " << avg[0] << ", Standard deviation: " << sd[0] << endl;
00193         if (output == 1 || output == 3)
00194           cout << "Average matching points: " << avg[1] << "(" << (int)((float)avg[1] / avg[0] * 100) << "%)"
00195               << ", Standard deviation: " << sd[1] << endl;
00196         if (output == 2 || output == 3)
00197           cout << "Average ICP-Value: " << avg[2] << ", Standard deviation: " << sd[2] << "\n" << endl;
00198 #endif
00199       }
00200     }
00201     else
00202     {
00203       ROS_ERROR("Couldn't find folder %s!", path.c_str());
00204       return;
00205     }
00206   }
00207 
00208   PCLCompare(pcl::PointCloud<pcl::PointXYZ>& m, int o) :
00209     model(m), output(o), number(0)
00210   {
00211 
00212   }
00213 };
00214 
00215 int main(int argc, char** argv)
00216 {
00217   string model_path;
00218   if (argc < 4)
00219   {
00220     cout << "Usage " << argv[0] << " [Path to model] [Point cloud folder] [Compare mode]" << endl;
00221     return -1;
00222   }
00223 
00224   sensor_msgs::PointCloud2 cloud_mess2;
00225   pcl::PointCloud < pcl::PointXYZ > cloud2;
00226 
00227   if (pcl::io::loadPCDFile(argv[1], cloud_mess2) == -1)
00228   {
00229     ROS_ERROR("Couldn't read file %s!", argv[1]);
00230 
00231     return (-1);
00232   }
00233 
00234   pcl::fromROSMsg(cloud_mess2, cloud2);
00235   PCLCompare t(cloud2, atoi(argv[3]));
00236 
00237   t.getFiles(argv[2]);
00238 
00239 }
00240 
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Properties Friends


ias_projected_light
Author(s): Florian Zacherl, Dejan Pangercic
autogenerated on Sun Oct 6 2013 12:09:47