37 #include <boost/filesystem/fstream.hpp>
62 size_t num_points = model->m_pointCloud->numPoints();
70 for(
size_t i=0; i<num_points; i++)
76 template <
typename ValueType>
79 if (!force_overwrite && fs::exists(dest)) {
80 std::cout <<
"[write_params_to_file] Info: Skipping writing to file " << dest <<
" because the file already exists. (If already existing files should be overwritten, use --force.)" << std::endl;
84 fs::ofstream out(dest);
86 if (!out.is_open() || !out.good()) {
87 std::cout <<
"[write_params_to_file] Error: Unable to write to file " << dest << std::endl;
92 for (
int i = 0; i < count; i++) {
93 out << std::setprecision(std::numeric_limits<float>::digits10 + 1) <<
values[i] <<
" ";
102 return ((
int) in) - 48;
105 template <
typename T>
108 if (!force_overwrite && fs::exists(dest)) {
109 std::cout <<
"[write_matrix4_to_file] Info: Skipping writing to file " << dest <<
" because the file already exists. (If already existing files should be overwritten, use --force.)" << std::endl;
113 fs::ofstream out(dest);
115 if (!out.is_open() || !out.good()) {
116 std::cout <<
"[write_matrix4_to_file] Error: Unable to write to file " << dest << std::endl;
120 for (
int i = 0; i < 16; i++) {
121 out << std::setprecision(std::numeric_limits<T>::digits10 + 1 ) << mat(i);
122 out << (i%4 != 3 || i == 0 ?
' ' :
'\n');
130 bool copy_file(
const fs::path from,
const fs::path to,
bool force_overwrite) {
132 if (!force_overwrite && fs::exists(to)) {
133 std::cout <<
"[copy_file] Info: Skipping copy from file " << from <<
" because the file " << to <<
"already exists. (If already existing files should be overwritten, use --force.)" << std::endl;
137 fs::copy_option co = fs::copy_option::overwrite_if_exists;
141 }
catch (fs::filesystem_error &e) {
142 std::cout <<
"[convert_riegl_project] Error: " << e.what() << std::endl;
151 if (!force_overwrite && fs::exists(
file)) {
152 std::cout <<
"[write_mat4_to_pose_file] Info: Skipping writing " <<
file <<
" because it already exists. (If already existing files should be overwritten, use --force.)" << std::endl;
169 fs::ofstream pose(
file);
170 if (!pose.is_open() || !pose.good()) {
171 std::cout <<
"[writing_mat4_to_pose_file] Error: Error while writing " <<
file << std::endl;
175 for (
int i = 0; i < 6; i++) {
176 pose << pose_data[i];
178 if (i == 2 || i == 5) {
191 std::vector<lvr2::ScanPosition> *work,
192 int *read_file_count,
193 int *current_file_idx,
194 const fs::path *scans_dir,
197 bool force_overwrite,
198 unsigned int reduction,
199 std::string inputformat =
"rxp",
200 std::string outputcoords =
"slam6d")
207 while (*current_file_idx < work->size()) {
209 int scan_nr = *current_file_idx + 1;
215 char out_file_buf[2048];
216 std::snprintf(out_file_buf, 2048,
"scan%.3d.3d", scan_nr);
217 fs::path out_file = *scans_dir / out_file_buf;
218 if (!force_overwrite && fs::exists(out_file)) {
220 std::cout <<
"[read_rxp_per_thread] Info: Skipping conversion from file " << pos.scan_file <<
" because the file " << out_file <<
"already exists. (If already existing files should be overwritten, use --force.)" << std::endl;
221 (*read_file_count)++;
228 riegl_to_slam_transform(4) = -100.0;
229 riegl_to_slam_transform(9) = 100.0;
230 riegl_to_slam_transform(5) = 0.0;
231 riegl_to_slam_transform(2) = 100.0;
232 riegl_to_slam_transform(10) = 0.0;
236 if(inputformat ==
"rxp")
238 if(outputcoords ==
"slam6d")
241 pos.scan_file.string(),
243 riegl_to_slam_transform
246 }
else if(outputcoords ==
"lvr") {
248 pos.scan_file.string(),
258 inv_transform.transpose();
266 if(outputcoords ==
"slam6d")
269 std::cout <<
"[read_rxp_per_thread] Transform to slam6d" << std::endl;
277 std::cout <<
"[read_rxp_per_thread] Info: Thread " <<
id <<
": read data from file " << pos.scan_file <<
" and wrote to file " << out_file <<
" ("
278 << (*read_file_count)++ + 1 <<
"/" << work->size() <<
")" << std::endl;
286 const fs::path &out_scan_dir,
287 bool force_overwrite,
288 unsigned int reduction,
289 std::string output_coords =
"slam6d"
292 fs::path scans_dir = out_scan_dir /
"scans/";
293 fs::path images_dir = out_scan_dir /
"images/";
298 fs::create_directory(out_scan_dir);
300 fs::create_directory(scans_dir);
301 fs::create_directory(images_dir);
303 }
catch (fs::filesystem_error &e) {
304 std::cout <<
"[convert_riegl_project] Error: " << e.what() << std::endl;
310 std::chrono::time_point<std::chrono::high_resolution_clock> start = std::chrono::high_resolution_clock::now();
313 unsigned int num_cores = std::thread::hardware_concurrency();
319 std::thread *threads =
new std::thread[num_cores];
320 std::vector<lvr2::ModelPtr> clouds;
322 int read_file_count = 0;
323 int current_file_idx = 0;
325 for (
int j = 0; j < num_cores; j++) {
326 threads[j] = std::thread(
341 for (
int j = 0; j < num_cores; j++) {
347 std::chrono::time_point<std::chrono::high_resolution_clock> end = std::chrono::high_resolution_clock::now();
348 std::chrono::seconds diff = std::chrono::duration_cast<std::chrono::seconds>(end - start);
350 std::cout <<
"Reading " << read_file_count <<
" files took " << diff.count() <<
" sec." << std::endl;
357 char out_file_buf[2048];
358 std::snprintf(out_file_buf, 2048,
"scan%.3d.pose", scan_nr);
359 fs::path out_pose_file = scans_dir / out_file_buf;
361 std::cout <<
"[convert_riegl_project] Error: Error while trying to write file " << out_pose_file << std::endl;
366 for (
const lvr2::ImageFile &image : pos.images) {
368 char out_file_buf[2048];
369 std::snprintf(out_file_buf, 2048,
"scan%.3d_%.2d.jpg", scan_nr, image_nr);
370 fs::path out_img_file = images_dir / out_file_buf;
372 if (!
copy_file(image.image_file, out_img_file, force_overwrite)) {
373 std::cout <<
"[convert_riegl_project] Error: Error while trying to copy file "
374 << image.image_file <<
" to " << out_img_file << std::endl;
379 std::snprintf(out_file_buf, 2048,
"scan%.3d_%.2d_extrinsic.dat", scan_nr, image_nr);
380 if (!write_mat4_to_file<double>(image.extrinsic_transform,
381 images_dir / out_file_buf,
383 std::cout <<
"[convert_riegl_project] Error: Error while writing image extrinsic \
384 matrix to " << (images_dir / out_file_buf) << std::endl;
388 std::snprintf(out_file_buf, 2048,
"scan%.3d_%.2d_orientation.dat", scan_nr, image_nr);
389 if (!write_mat4_to_file<double>(image.orientation_transform,
390 images_dir / out_file_buf,
392 std::cout <<
"[convert_riegl_project] Error: Error while writing image orientation \
393 matrix to " << (images_dir / out_file_buf)<< std::endl;
397 std::snprintf(out_file_buf, 2048,
"scan%.3d_%.2d_intrinsic.txt", scan_nr, image_nr);
400 image.intrinsic_params,
402 std::cout <<
"[convert_riegl_project] Error: Error while writing image intrinsic \
403 params to " << (images_dir / out_file_buf) << std::endl;
407 std::snprintf(out_file_buf, 2048,
"scan%.3d_%.2d_distortion.txt", scan_nr, image_nr);
410 image.distortion_params,
412 std::cout <<
"[convert_riegl_project] Error: Error while writing image distortion \
413 params to " << (images_dir / out_file_buf) << std::endl;
440 std::cout <<
"[main] Error: The directory \'" << opt.
getInputDir() <<
"\' is NOT a Riegl Scanproject directory." << std::endl;
446 std::cout <<
"[main] Error: It occured an error while converting the Riegl Scan Project." << std::endl;
452 scan_dir = scan_dir /
"scans/";
453 std::regex files_3d_reg(
"scan\\d{3}.3d");
454 std::regex files_4x4_reg(
"scan\\d{3}.4x4");
456 std::vector<fs::path> cloud_files;
457 std::vector<fs::path> transform_files;
459 fs::directory_iterator it_begin = fs::directory_iterator(scan_dir);
460 fs::directory_iterator it_end = fs::directory_iterator();
461 for (
auto it = it_begin; it != it_end; it++) {
463 fs::path current_file = *it;
465 if (std::regex_match(current_file.filename().string(), files_3d_reg)) {
466 cloud_files.push_back(current_file);
469 if (std::regex_match(current_file.filename().string(), files_4x4_reg)) {
470 transform_files.push_back(current_file);
474 std::sort(cloud_files.begin(), cloud_files.end(), [](
const fs::path &a,
const fs::path &b) { return a.compare(b) < 0; });
475 std::sort(transform_files.begin(), transform_files.end(), [](
const fs::path &a,
const fs::path &b) { return a.compare(b) < 0; });
477 std::vector<lvr2::ModelPtr> clouds;
478 std::vector<lvr2::Matrix4<Vec>> transforms;
480 for (
int i = 0; i < cloud_files.size(); i++) {
485 transforms.push_back(trans);
488 for (
int i = 0; i < clouds.size(); i++) {
489 if (!clouds[i]->m_pointCloud)
493 std::cout << n << std::endl;
494 for (
int j = 0; j < n; j++) {
495 lvr2::Vector<Vec> vert(cloud[j*3], cloud[j*3 + 1], cloud[j*3 + 2]);
496 lvr2::Vector<Vec> vert2(vert.z/100.0, -vert.x/100.0, vert.y/100.0);
497 vert2 = transforms[i] * vert2;
498 vert2.transformRM(transforms[i]);
499 cloud[j*3] = -100*vert2.y;
500 cloud[j*3+1] = 100*vert2.z;
501 cloud[j*3+2] = 100*vert2.x;