Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
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
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
00067 case 0:
00068 {
00069 int r = cloud.points.size();
00070 return r;
00071 }
00072 break;
00073
00074
00075
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
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())))
00123 {
00124 while ((entry = readdir(pDIR)))
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))
00130 {
00131 if (S_ISDIR(s.st_mode))
00132 {
00133 cout << stst.str() << ":" << endl;
00134 getFiles(stst.str());
00135 }
00136 else
00137 {
00138 string file(entry->d_name);
00139 if (strcmp(file.substr(file.size() - 3, 3).c_str(), "pcd") == 0)
00140 {
00141
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
00152 for (int i = 0; i < 3; i++)
00153 {
00154 val[i].push_back(compare(i));
00155 }
00156 }
00157 }
00158 }
00159 }
00160
00161
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
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