1 #include "object_recognition.h"
2 
3 #include <string>
4 #include <sstream>
5 #include <iostream>
6 #include <pcl/console/parse.h>
7 #include <pcl/io/pcd_io.h>
8 #include <pcl/visualization/pcl_visualizer.h>
9 #include <boost/filesystem.hpp>
10 #include <boost/algorithm/string.hpp> // for split, is_any_of
11 namespace bf = boost::filesystem;
12 
13 inline void
getModelsInDirectory(bf::path & dir,std::string & rel_path_so_far,std::vector<std::string> & relative_paths)14 getModelsInDirectory (bf::path & dir, std::string & rel_path_so_far, std::vector<std::string> & relative_paths)
15 {
16   for (const auto& dir_entry : bf::directory_iterator(dir))
17   {
18     //check if its a directory, then get models in it
19     if (bf::is_directory (dir_entry))
20     {
21       std::string so_far = rel_path_so_far + dir_entry.path ().filename ().string () + "/";
22       bf::path curr_path = dir_entry.path ();
23       getModelsInDirectory (curr_path, so_far, relative_paths);
24     }
25     else
26     {
27       std::vector<std::string> strs;
28       std::string file = dir_entry.path ().filename ().string ();
29       boost::split (strs, file, boost::is_any_of ("."));
30       std::string extension = strs[strs.size () - 1];
31 
32       if((file.compare (0, 3, "raw") == 0) && extension == "pcd") {
33         std::string path = rel_path_so_far + dir_entry.path ().filename ().string ();
34         relative_paths.push_back (path);
35       }
36     }
37   }
38 }
39 
40 int
main(int argc,char ** argv)41 main (int argc, char ** argv)
42 {
43   if (argc < 3)
44   {
45     pcl::console::print_info ("Syntax is: %s input_directory <options>\n", argv[0]);
46     pcl::console::print_info ("  where options are:\n");
47     pcl::console::print_info ("    --filter parameters.txt ............ Threshold, downsample, and denoise\n");
48     pcl::console::print_info ("    --segment parameters.txt  .......... Remove dominant plane and cluster\n");
49     pcl::console::print_info ("    --feature parameters.txt ........... Compute normals, keypoints, and descriptors\n");
50     pcl::console::print_info ("    --registration parameters.txt ...... Align best fitting model to query\n");
51     pcl::console::print_info ("  and where the parameters files must contain the following values (one per line):\n");
52     pcl::console::print_info ("    filter parameters:\n");
53     pcl::console::print_info ("      * min_depth\n");
54     pcl::console::print_info ("      * max_depth\n");
55     pcl::console::print_info ("      * downsample_leaf_size\n");
56     pcl::console::print_info ("      * outlier_rejection_radius\n");
57     pcl::console::print_info ("      * outlier_rejection_min_neighbors\n");
58     pcl::console::print_info ("    segmentation parameters:\n");
59     pcl::console::print_info ("      * plane_inlier_distance_threshold\n");
60     pcl::console::print_info ("      * max_ransac_iterations\n");
61     pcl::console::print_info ("      * cluster_tolerance\n");
62     pcl::console::print_info ("      * min_cluster_size\n");
63     pcl::console::print_info ("      * max_cluster_size\n");
64     pcl::console::print_info ("    feature estimation parameters:\n");
65     pcl::console::print_info ("      * surface_normal_radius\n");
66     pcl::console::print_info ("      * keypoints_min_scale\n");
67     pcl::console::print_info ("      * keypoints_nr_octaves\n");
68     pcl::console::print_info ("      * keypoints_nr_scales_per_octave\n");
69     pcl::console::print_info ("      * keypoints_min_contrast\n");
70     pcl::console::print_info ("      * local_descriptor_radius\n");
71     pcl::console::print_info ("    registration parameters:\n");
72     pcl::console::print_info ("      * initial_alignment_min_sample_distance\n");
73     pcl::console::print_info ("      * initial_alignment_max_correspondence_distance\n");
74     pcl::console::print_info ("      * initial_alignment_nr_iterations\n");
75     pcl::console::print_info ("      * icp_max_correspondence_distance\n");
76     pcl::console::print_info ("      * icp_rejection_threshold\n");
77     pcl::console::print_info ("      * icp_transformation_epsilon\n");
78     pcl::console::print_info ("      * icp_max_iterations\n");
79     pcl::console::print_info ("Note: The output's base filename must be specified without the .pcd extension\n");
80     pcl::console::print_info ("      Four output files will be created with the following suffixes:\n");
81     pcl::console::print_info ("        * '_points.pcd'\n");
82     pcl::console::print_info ("        * '_keypoints.pcd'\n");
83     pcl::console::print_info ("        * '_localdesc.pcd'\n");
84     pcl::console::print_info ("        * '_globaldesc.pcd'\n");
85 
86     return (1);
87   }
88 
89   ObjectRecognitionParameters params;
90   std::ifstream params_stream;
91 
92   //Parse filter parameters
93   std::string filter_parameters_file;
94   if (pcl::console::parse_argument (argc, argv, "--filter", filter_parameters_file) < 0)
95   {
96     pcl::console::print_error ("Missing option --filter\n");
97     return (1);
98   }
99   params_stream.open (filter_parameters_file.c_str ());
100   if (params_stream.is_open())
101   {
102     params_stream >> params.min_depth;
103     params_stream >> params.max_depth;
104     params_stream >> params.downsample_leaf_size;
105     params_stream >> params.outlier_rejection_radius;
106     params_stream >> params.outlier_rejection_min_neighbors;
107     params_stream.close ();
108   }
109   else
110   {
111     pcl::console::print_info ("Failed to open the filter parameters file (%s)\n", filter_parameters_file.c_str ());
112     return (1);
113   }
114 
115   // Parse segmentation parameters
116   std::string segmentation_parameters_file;
117   if (pcl::console::parse_argument (argc, argv, "--segment", segmentation_parameters_file) < 0)
118   {
119     pcl::console::print_error ("Missing option --segment\n");
120     return (1);
121   }
122   params_stream.open (segmentation_parameters_file.c_str ());
123   if (params_stream.is_open())
124   {
125     params_stream >> params.plane_inlier_distance_threshold;
126     params_stream >> params.max_ransac_iterations;
127     params_stream >> params.cluster_tolerance;
128     params_stream >> params.min_cluster_size;
129     params_stream >> params.max_cluster_size;
130     params_stream.close ();
131   }
132   else
133   {
134     pcl::console::print_info ("Failed to open the segmentation parameters file (%s)\n",
135                               segmentation_parameters_file.c_str ());
136     return (1);
137   }
138 
139   // Parse feature estimation parameters
140   std::string feature_estimation_parameters_file;
141   if (pcl::console::parse_argument (argc, argv, "--feature", feature_estimation_parameters_file) < 0)
142   {
143     pcl::console::print_error ("Missing option --feature\n");
144     return (1);
145   }
146   params_stream.open (feature_estimation_parameters_file.c_str ());
147   if (params_stream.is_open())
148   {
149     params_stream >> params.surface_normal_radius;
150     params_stream >> params.keypoints_min_scale;
151     params_stream >> params.keypoints_nr_octaves;
152     params_stream >> params.keypoints_nr_scales_per_octave;
153     params_stream >> params.keypoints_min_contrast;
154     params_stream >> params.local_descriptor_radius;
155     params_stream.close ();
156   }
157   else
158   {
159     pcl::console::print_info ("Failed to open the feature estimation parameters file (%s)\n",
160                               feature_estimation_parameters_file.c_str ());
161     return (1);
162   }
163 
164   // Parse the registration parameters
165   std::string registration_parameters_file;
166   if (pcl::console::parse_argument (argc, argv, "--registration", registration_parameters_file) < 0)
167   {
168     pcl::console::print_error ("Missing option --registration\n");
169     return (1);
170   }
171   params_stream.open (registration_parameters_file.c_str ());
172   if (params_stream.is_open())
173   {
174     params_stream >> params.initial_alignment_min_sample_distance;
175     params_stream >> params.initial_alignment_max_correspondence_distance;
176     params_stream >> params.initial_alignment_nr_iterations;
177     params_stream >> params.icp_max_correspondence_distance;
178     params_stream >> params.icp_outlier_rejection_threshold;
179     params_stream >> params.icp_transformation_epsilon;
180     params_stream >> params.icp_max_iterations;
181     params_stream.close ();
182   }
183   else
184   {
185     pcl::console::print_info ("Failed to open the registration parameters file (%s)\n",
186                               registration_parameters_file.c_str ());
187     return (1);
188   }
189 
190   std::string directory (argv[1]);
191   //Find all raw* files in input_directory
192   bf::path dir_path = directory;
193   std::vector < std::string > files;
194   std::string start = "";
195   getModelsInDirectory (dir_path, start, files);
196 
197   for(std::size_t i=0; i < files.size(); i++) {
198     // Load input file
199 
200     std::string filename = directory;
201     filename.append("/");
202     filename.append(files[i]);
203     PointCloudPtr input (new PointCloud);
204     pcl::io::loadPCDFile (filename, *input);
205     pcl::console::print_info ("Loaded %s (%lu points)\n", filename.c_str(), input->size ());
206 
207     std::cout << files[i] << std::endl;
208     // Construct the object model
209     ObjectRecognition obj_rec (params);
210     ObjectModel model;
211     obj_rec.constructObjectModel (input, model);
212 
213     //get directory name
214     std::vector < std::string > strs;
215     boost::split (strs, files[i], boost::is_any_of ("/\\"));
216 
217     std::string id = strs[0];
218     std::string raw_file = strs[1];
219 
220     strs.clear();
221     boost::split (strs, raw_file, boost::is_any_of ("_"));
222 
223     std::stringstream base_filestream;
224     base_filestream << directory << "/" << id << "/" << id << strs[1].substr(0,1);
225     // Save the model files
226     std::string base_filename (base_filestream.str()), output_filename;
227 
228     output_filename = base_filename;
229     output_filename.append ("_points.pcd");
230     pcl::io::savePCDFile (output_filename, *(model.points));
231     pcl::console::print_info ("Saved points as %s\n", output_filename.c_str ());
232 
233     output_filename = base_filename;
234     output_filename.append ("_keypoints.pcd");
235     pcl::io::savePCDFile (output_filename, *(model.keypoints));
236     pcl::console::print_info ("Saved keypoints as %s\n", output_filename.c_str ());
237 
238     output_filename = base_filename;
239     output_filename.append ("_localdesc.pcd");
240     pcl::io::savePCDFile (output_filename, *(model.local_descriptors));
241     pcl::console::print_info ("Saved local descriptors as %s\n", output_filename.c_str ());
242 
243     output_filename = base_filename;
244     output_filename.append ("_globaldesc.pcd");
245     pcl::io::savePCDFile (output_filename, *(model.global_descriptor));
246     pcl::console::print_info ("Saved global descriptor as %s\n", output_filename.c_str ());
247   }
248 
249   return (0);
250 }
251