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