48 #include <boost/program_options.hpp>
49 #include <boost/filesystem.hpp>
50 #include <boost/optional.hpp>
57 using boost::filesystem::path;
61 size_t size = snprintf(
nullptr, 0, format.c_str(), index) + 1;
63 snprintf(buff, size, format.c_str(), index);
74 else if (format ==
"riegl_txt")
76 return "scan%03i.txt";
78 else if (format ==
"ply")
80 return "scan%03i.ply";
96 string format =
"uos";
97 string pose_format =
"pose";
100 bool write_scans =
false;
101 string output_format;
102 bool write_pose =
false;
103 string output_pose_format;
104 bool no_frames =
false;
111 using namespace boost::program_options;
113 options_description general_options(
"General Options");
114 options_description icp_options(
"ICP Options");
115 options_description loopclosing_options(
"Loopclosing Options");
117 general_options.add_options()
118 (
"start,s", value<int>(&start)->default_value(start),
119 "The first Scan to process.\n"
120 "-1 (default): Search for first Scan.")
122 (
"end,e", value<int>(&end)->default_value(end),
123 "The last Scan to process.\n"
124 "-1 (default): Continue until no more Scans found.")
126 (
"format,f", value<string>(&format)->default_value(format),
127 "The format of the Scans in <dir>.\n"
128 "This can be a predefined Format (uos, ply, riegl_txt), or a printf Format String like \"scan%03i.3d\",\n"
129 "containing one %i or %d that will be replaced with the Scan Index.")
131 (
"pose-format", value<string>(&pose_format)->default_value(pose_format),
132 "The File extension of the Pose files.\n"
133 "Currently supported are: pose, dat, frames.")
135 (
"reduction,r", value<double>(&
options.reduction)->default_value(
options.reduction),
136 "The Voxel size for Octree based reduction.\n"
137 "-1 (default): No reduction.")
139 (
"min,m", value<double>(&
options.minDistance)->default_value(
options.minDistance),
140 "Ignore all Points closer than <value> to the origin of the Scan.\n"
141 "-1 (default): No filter.")
143 (
"max,M", value<double>(&
options.maxDistance)->default_value(
options.maxDistance),
144 "Ignore all Points farther away than <value> from the origin of the Scan.\n"
145 "-1 (default): No filter.")
147 (
"trustPose,p", bool_switch(&
options.trustPose),
148 "Use the unmodified Pose for ICP. Useful for GPS Poses or unordered Scans.\n"
149 "false (default): Apply the relative refinement of previous Scans.")
151 (
"metascan", bool_switch(&
options.metascan),
152 "Match Scans to the combined Pointcloud of all previous Scans instead of just the last Scan.")
154 (
"noFrames,F", bool_switch(&no_frames),
155 "Don't write \".frames\" files.")
157 (
"writePose,w", value<string>(&output_pose_format)->implicit_value(
"<pose-format>"),
158 "Write Poses to directory specified by --output.")
160 (
"writeScans,W", value<string>(&output_format)->implicit_value(
"<format>"),
161 "Write Scans to directory specified by --output.")
163 (
"output,o", value<path>(&output_dir),
164 "Changes output directory of --writePose and --writeScans. Does not affect \".frames\" files\n"
165 "default: <dir>/output.")
167 (
"verbose,v", bool_switch(&
options.verbose),
168 "Show more detailed output. Useful for fine-tuning Parameters or debugging.")
170 (
"hdf,H", bool_switch(&
options.useHDF),
171 "Opens the given hdf5 file. Then registrates all scans in '/raw/scans/'\nthat are named after the scheme: 'position_00001' where '1' is the scans number.\nAfter registration the calculated poses are written to the finalPose dataset in the hdf5 file.\n")
173 (
"help,h", bool_switch(&help),
174 "Print this help. Seriously how are you reading this if you don't know the --help Option?")
177 icp_options.add_options()
178 (
"icpIterations,i", value<int>(&
options.icpIterations)->default_value(
options.icpIterations),
179 "Number of iterations for ICP.\n"
180 "ICP should ideally converge before this number is met, but this number places an upper Bound on calculation time.")
182 (
"icpMaxDistance,d", value<double>(&
options.icpMaxDistance)->default_value(
options.icpMaxDistance),
183 "The maximum distance between two points during ICP.")
185 (
"maxLeafSize", value<int>(&
options.maxLeafSize)->default_value(
options.maxLeafSize),
186 "The maximum number of Points in a Leaf of a KDTree.")
188 (
"epsilon", value<double>(&
options.epsilon)->default_value(
options.epsilon),
189 "The epsilon difference between ICP-errors for the stop criterion of ICP.")
192 loopclosing_options.add_options()
193 (
"loopClosing,L", bool_switch(&
options.doLoopClosing),
194 "Use simple Loopclosing.\n"
195 "At least one of -L and -G must be specified for Loopclosing to take place.")
197 (
"graphSlam,G", bool_switch(&
options.doGraphSLAM),
198 "Use complex Loop Closing with GraphSLAM.\n"
199 "At least one of -L and -G must be specified for Loopclosing to take place.")
201 (
"closeLoopDistance,c", value<double>(&
options.closeLoopDistance)->default_value(
options.closeLoopDistance),
202 "The maximum distance between two poses to consider a closed loop in Loopclosing or an Edge in the GraphSLAM Graph.\n"
203 "Mutually exclusive to --closeLoopPairs.")
205 (
"closeLoopPairs,C", value<int>(&
options.closeLoopPairs)->default_value(
options.closeLoopPairs),
206 "The minimum pair overlap between two poses to consider a closed loop in Loopclosing or an Edge in the GraphSLAM Graph.\n"
207 "Mutually exclusive to --closeLoopDistance.\n"
208 "Pairs are judged using slamMaxDistance.\n"
209 "-1 (default): use --closeLoopDistance instead.")
211 (
"loopSize,l", value<int>(&
options.loopSize)->default_value(
options.loopSize),
212 "The minimum number of Scans to be considered a Loop to prevent Loopclosing from triggering on adjacent Scans.\n"
213 "Also used in GraphSLAM when considering other Scans for Edges\n"
214 "For Loopclosing, this value needs to be at least 6, for GraphSLAM at least 1.")
216 (
"slamIterations,I", value<int>(&
options.slamIterations)->default_value(
options.slamIterations),
217 "Number of iterations for SLAM.")
219 (
"slamMaxDistance,D", value<double>(&
options.slamMaxDistance)->default_value(
options.slamMaxDistance),
220 "The maximum distance between two points during SLAM.")
222 (
"slamEpsilon", value<double>(&
options.slamEpsilon)->default_value(
options.slamEpsilon),
223 "The epsilon difference of SLAM corrections for the stop criterion of SLAM.")
226 options_description hidden_options(
"hidden_options");
227 hidden_options.add_options()
228 (
"dir", value<path>(&dir))
231 positional_options_description pos;
234 options_description all_options(
"options");
235 all_options.add(general_options).add(icp_options).add(loopclosing_options).add(hidden_options);
237 variables_map variables;
238 store(command_line_parser(
argc,
argv).
options(all_options).positional(pos).run(), variables);
243 cout <<
"The Scan Registration Tool" << endl;
244 cout <<
"Usage: " << endl;
245 cout <<
"\tlvr2_registration [OPTIONS] <dir>" << endl;
247 general_options.print(cout);
249 icp_options.print(cout);
251 loopclosing_options.print(cout);
253 cout <<
"<dir> is the directory to search scans in" << endl;
257 if (variables.count(
"dir") != 1)
259 throw error(
"Missing <dir> Parameter");
262 if (variables.count(
"output") == 0)
264 output_dir = dir /
"output";
267 if (format.find(
'%') == string::npos)
272 if (variables.count(
"writePose") == 1)
275 if (output_pose_format[0] ==
'<')
277 output_pose_format = pose_format;
280 if (variables.count(
"writeScans") == 1)
283 if (output_format[0] ==
'<')
285 output_format = format;
287 else if (output_format.find(
'%') == string::npos)
293 options.createFrames = !no_frames;
297 std::cerr << ex.what() << endl;
299 std::cerr <<
"Use '--help' to see the list of possible options" << endl;
310 std::shared_ptr<HDF5PCIO> h5_ptr(
new HDF5PCIO());
314 cout <<
"HDF5 is used!" << endl;
315 ifstream f(dir.c_str());
319 boost::filesystem::path pathToHDF(dir.c_str());
320 h5_ptr->open(pathToHDF.string());
324 cerr <<
"The given HDF5 file could not be opened! Oben" << endl;
334 for (
int i = 0; i < 100; i++)
340 cout <<
"First scan: " <<
file.filename() << endl;
346 cerr <<
"Could not find a starting scan. are you using the right format?" << endl;
354 for (
int i = start; end == -1 || i <= end; i++)
359 if (end != -1 || i == start)
361 cerr <<
"Missing scan " <<
file.filename() << endl;
365 cout <<
"Last scan: \"" <<
format_name(format, end) <<
'"' << endl;
368 file.replace_extension(pose_format);
371 cerr <<
"Missing pose file " <<
file.filename() << endl;
378 vector<SLAMScanPtr> scans;
384 int count = end - start + 1;
386 vector<string> numOfScansInHDF;
387 if(h5_ptr->m_hdf5_file)
393 vector<lvr2::ScanPtr> rawScans;
396 for (
int i = 0; i < numOfScansInHDF.size(); i++)
402 boost::shared_array<float> bb_array = h5_ptr->loadArray<
float>(
"raw/scans/" + numOfScansInHDF[i],
"boundingBox", six);
406 tempScan->boundingBox = bb;
408 boost::shared_array<float> fov_array = h5_ptr->loadArray<
float>(
"raw/scans/" + numOfScansInHDF[i],
"fov", six);
413 boost::shared_array<float> res_array = h5_ptr->loadArray<
float>(
"raw/scans/" + numOfScansInHDF[i],
"resolution", six);
415 tempScan->hResolution = res_array[0];
416 tempScan->vResolution = res_array[1];
418 boost::shared_array<float> point_array = h5_ptr->loadArray<
float>(
"raw/scans/"+ numOfScansInHDF[i],
"points", pointsNum);
420 pointsNum = pointsNum / 3;
422 tempScan->points = pointPointer;
424 tempScan->pointsLoaded =
true;
426 tempScan->poseEstimation = h5_ptr->loadMatrix<
Transformd>(
"raw/scans/" + numOfScansInHDF[i],
"initialPose").get();
427 tempScan->positionNumber = i;
429 tempScan->scanRoot =
"raw/scans/" + numOfScansInHDF[i];
433 tempScan->registration = Transformd::Identity();
438 pos.
scans.push_back(tempScan);
439 proj.
project->positions.push_back(std::make_shared<ScanPosition>(pos));
449 cout <<
"vor Pipe Konstruktor" << endl;
453 cout <<
"Nach doRegistration" << endl;
454 for (
size_t i = 0; i < projPtr->changed.size(); i++)
456 cout <<
"Reconstruct indivcator ans Stelle: " << i <<
" ist: " << projPtr->changed.at(i)<< endl;
459 cout <<
"Eine Pose aus dem Project:" << endl << projPtr->project->positions.at(1)->scans[0]->registration << endl;
466 for (
int i = 0; i < count; i++)
473 cerr <<
"Unable to read Model from: " <<
file.string() << endl;
476 if (!model->m_pointCloud)
478 cerr <<
"file does not contain Points: " <<
file.string() << endl;
482 file.replace_extension(pose_format);
486 scan->points = model->m_pointCloud;
487 scan->poseEstimation = pose;
490 scans.push_back(slamScan);
496 auto start_time = chrono::steady_clock::now();
500 auto required_time = chrono::steady_clock::now() - start_time;
501 cout <<
"SLAM finished in " << required_time.count() / 1e9 <<
" seconds" << endl;
503 if (write_pose || write_scans)
505 create_directories(output_dir);
513 for(
int i = 0; i < scans.size(); i++)
516 cout <<
"Main:Pose Scan Nummer " << i << endl << pose << endl;
520 pose.transposeInPlace();
521 h5_ptr->MatrixIO::save(
"raw/scans/" + numOfScansInHDF[i],
"finalPose", pose);
525 for (
int i = 0; i < count; i++)
527 auto& scan = scans[i];
532 file.replace_extension(
"frames");
534 scan->writeFrames(
file.string());
539 file = output_dir /
format_name(write_scans ? output_format : format, start + i);
540 file.replace_extension(output_pose_format);
541 ofstream out(
file.string());
543 auto pose = scan->pose();
544 for (
int y = 0; y < 4; y++)
546 for (
int x = 0; x < 4; x++)
562 size_t n = scan->numPoints();
564 auto model = make_shared<Model>();
565 auto pointCloud = make_shared<PointBuffer>();
568 #pragma omp parallel for schedule(static)
569 for (
size_t i = 0; i < n; i++)
571 auto point = scan->rawPoint(i);
572 points[i * 3] = point[0];
573 points[i * 3 + 1] = point[1];
574 points[i * 3 + 2] = point[2];
577 pointCloud->setPointArray(points, n);
578 model->m_pointCloud = pointCloud;