45 #include <boost/filesystem.hpp>
48 #include <Eigen/Dense>
73 ss << scan.stem().string() << extension;
74 return boost::filesystem::path(ss.str());
85 using boost::filesystem::path;
86 using boost::filesystem::directory_iterator;
90 std::cout <<
options << std::endl;
94 path transformPath(
options.getTransformFile());
95 if(!exists(transformPath) || !is_regular_file(transformPath))
97 std::cout <<
timestamp <<
"Could not open transformation file " <<
options.getTransformFile() << std::endl;
107 std::cout <<
timestamp <<
"Transforming: " << std::endl << std::endl;
108 std::cout <<
transform << std::endl << std::endl;
111 if(!is_directory(inputDir))
118 if(!is_directory(inputDir))
124 if(inputDir == outputDir)
126 std::cout <<
timestamp <<
"Input directory and output directory should not be equal." << std::endl;
130 path mergeDir(
options.getMergeDir());
131 if(!is_directory(mergeDir))
133 std::cout <<
timestamp <<
"Merge directory is not valid: " <<
options.getMergeDir() << std::endl;
137 if(mergeDir == outputDir)
139 std::cout <<
timestamp <<
"Merge directory and output directory should not be equal." << std::endl;
146 vector<path> input_scans;
147 vector<path> merge_scans;
149 directory_iterator end;
150 for(directory_iterator it(inputDir); it != end; ++it)
152 string extension = it->path().extension().string();
153 if(extension ==
".3d")
155 input_scans.push_back(it->path());
159 for(directory_iterator it(mergeDir); it != end; ++it)
161 string extension = it->path().extension().string();
162 if(extension ==
".3d")
164 merge_scans.push_back(it->path());
168 std::sort(input_scans.begin(), input_scans.end());
169 std::sort(merge_scans.begin(), merge_scans.end());
173 int scan_counter = 0;
174 char name_buffer[256];
175 for(
auto current_path : input_scans)
178 sprintf(name_buffer,
"scan%03d.3d", scan_counter);
179 path target_path = outputDir / path(name_buffer);
180 std::cout <<
timestamp <<
"Copying " << current_path.string() <<
" to " << target_path.string() <<
"." << std::endl;
181 boost::filesystem::copy(current_path, target_path);
188 sprintf(name_buffer,
"scan%03d.oct", scan_counter);
189 path oct_out = outputDir / path(name_buffer);
190 std::cout <<
timestamp <<
"Copying " << oct_in.string() <<
" to " << oct_out.string() <<
"." << std::endl;
191 boost::filesystem::copy(oct_in, oct_out);
200 sprintf(name_buffer,
"scan%03d.frames", scan_counter);
201 path frames_out = outputDir / path(name_buffer);
204 if(!exists(frames_in))
206 std::cout <<
timestamp <<
"Warning: Could not find " << frames_in.string() << std::endl;
210 std::cout <<
timestamp <<
"Copying " << frames_in.string() <<
" to " << frames_out.string() <<
"." << std::endl;
211 boost::filesystem::copy(frames_in, frames_out);
220 sprintf(name_buffer,
"scan%03d.pose", scan_counter);
221 path pose_out = outputDir / path(name_buffer);
226 std::cout <<
timestamp <<
"Warning: Could not find " << pose_in.string() << std::endl;
230 std::cout <<
timestamp <<
"Copying " << pose_in.string() <<
" to " << pose_out.string() <<
"." << std::endl;
231 boost::filesystem::copy(pose_in, pose_out);
238 for(
auto current_path : merge_scans)
242 sprintf(name_buffer,
"scan%03d.3d", scan_counter);
243 path target_path = outputDir / path(name_buffer);
244 std::cout <<
timestamp <<
"Copying " << current_path.string() <<
" to " << target_path.string() <<
"." << std::endl;
245 boost::filesystem::copy(current_path, target_path);
252 sprintf(name_buffer,
"scan%03d.oct", scan_counter);
253 path oct_out = outputDir / path(name_buffer);
254 std::cout <<
timestamp <<
"Copying " << oct_in.string() <<
" to " << oct_out.string() <<
"." << std::endl;
255 boost::filesystem::copy(oct_in, oct_out);
264 sprintf(name_buffer,
"scan%03d.frames", scan_counter);
265 path frames_out = outputDir / path(name_buffer);
268 if(!exists(frames_in))
270 std::cout <<
timestamp <<
"Warning: Could not find " << frames_in.string() << std::endl;
275 std::cout <<
timestamp <<
"Transforming " << frames_in.string() << std::endl;
276 Transformf registration = getTransformationFromFrames<float>(frames_in);
280 std::cout <<
timestamp <<
"Writing transformed registration to " << frames_out.string() << std::endl;
290 sprintf(name_buffer,
"scan%03d.pose", scan_counter);
291 path pose_out = outputDir / path(name_buffer);
294 if(!exists(frames_in))
296 std::cout <<
timestamp <<
"Warning: Could not find " << frames_in.string() << std::endl;
301 std::cout <<
timestamp <<
"Transforming " << pose_in.string() << std::endl;
306 pos += transform_position;
307 ang += transform_angles;
309 std::cout <<
timestamp <<
"Writing transformed pose estimat to " << pose_out.string() << std::endl;