main.cpp
Go to the documentation of this file.
1 
28 #include <chrono>
29 #include <iostream>
30 #include <math.h>
31 #include <mutex>
32 #include <regex>
33 #include <string>
34 #include <thread>
35 #include <vector>
36 
37 #include <boost/filesystem/fstream.hpp>
38 
40 #include "lvr2/io/ModelFactory.hpp"
41 #include "lvr2/io/RxpIO.hpp"
42 #include "lvr2/util/Util.hpp"
45 
46 #include "Options.hpp"
47 #include "RieglProject.hpp"
48 
49 /*
50 
51  TODO:
52  If RxpIO can't read the file, it probably gives back an empty modelptr. This case
53  crashes the program.
54 
55 
56 */
57 
59 
61 {
62  size_t num_points = model->m_pointCloud->numPoints();
63 
64  // float test = model->m_pointCloud->getPointArray().get()[0];
65 
66  lvr2::BaseVector<float>* pts_raw =
67  reinterpret_cast<lvr2::BaseVector<float>*>(&model->m_pointCloud->getPointArray().get()[0]);
68 
69  #pragma omp for
70  for(size_t i=0; i<num_points; i++)
71  {
72  pts_raw[i] = transform * pts_raw[i];
73  }
74 }
75 
76 template <typename ValueType>
77 bool write_params_to_file(fs::path dest, bool force_overwrite, ValueType *values, int count) {
78 
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;
81  return true;
82  }
83 
84  fs::ofstream out(dest);
85 
86  if (!out.is_open() || !out.good()) {
87  std::cout << "[write_params_to_file] Error: Unable to write to file " << dest << std::endl;
88  return false;
89  }
90 
91 
92  for (int i = 0; i < count; i++) {
93  out << std::setprecision(std::numeric_limits<float>::digits10 + 1) << values[i] << " ";
94  }
95 
96  out.close();
97 
98  return true;
99 }
100 
101 int char_to_int(char in) {
102  return ((int) in) - 48;
103 }
104 
105 template <typename T>
106 bool write_mat4_to_file(lvr2::Transformd mat, fs::path dest, bool force_overwrite) {
107 
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;
110  return true;
111  }
112 
113  fs::ofstream out(dest);
114 
115  if (!out.is_open() || !out.good()) {
116  std::cout << "[write_matrix4_to_file] Error: Unable to write to file " << dest << std::endl;
117  return false;
118  }
119 
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');
123  }
124 
125  out.close();
126 
127  return true;
128 }
129 
130 bool copy_file(const fs::path from, const fs::path to, bool force_overwrite) {
131 
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;
134  return true;
135  }
136 
137  fs::copy_option co = fs::copy_option::overwrite_if_exists;
138 
139  try {
140  fs::copy_file(from, to, co);
141  } catch (fs::filesystem_error &e) {
142  std::cout << "[convert_riegl_project] Error: " << e.what() << std::endl;
143  return false;
144  }
145 
146  return true;
147 }
148 
149 bool write_mat4_to_pose_file(const fs::path file, const lvr2::Transformd& transform, bool force_overwrite) {
150 
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;
153  return true;
154  }
155 
156  double pose_data[6];
157 
158  //lvr2::Util::riegl_to_slam6d_transform(transform).toPostionAngle(pose_data);
159 
161 
162  lvr2::eigenToEuler(trans_slam6d, pose_data);
163 
164  // .pose expects angles in degree and not radian
165  pose_data[3] = lvr2::Util::rad_to_deg(pose_data[3]);
166  pose_data[4] = lvr2::Util::rad_to_deg(pose_data[4]);
167  pose_data[5] = lvr2::Util::rad_to_deg(pose_data[5]);
168 
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;
172  return false;
173  }
174 
175  for (int i = 0; i < 6; i++) {
176  pose << pose_data[i];
177 
178  if (i == 2 || i == 5) {
179  pose << '\n';
180  } else {
181  pose << ' ';
182  }
183  }
184 
185  pose.close();
186 
187  return true;
188 }
189 
191  std::vector<lvr2::ScanPosition> *work,
192  int *read_file_count,
193  int *current_file_idx,
194  const fs::path *scans_dir,
195  std::mutex *mtx,
196  int id,
197  bool force_overwrite,
198  unsigned int reduction,
199  std::string inputformat = "rxp",
200  std::string outputcoords = "slam6d")
201 {
202 
203  lvr2::RxpIO rxpio;
204 
205  mtx->lock();
206 
207  while (*current_file_idx < work->size()) {
208 
209  int scan_nr = *current_file_idx + 1;
210  lvr2::ScanPosition &pos = (*work)[(*current_file_idx)++];
211 
212  mtx->unlock();
213 
214  // build output filename to check if it already exists...
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)) {
219  mtx->lock();
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)++;
222  continue;
223  }
224 
225  lvr2::Transformd identity;
226  lvr2::Transformd riegl_to_slam_transform;
227 
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;
233 
234  lvr2::ModelPtr tmp;
235 
236  if(inputformat == "rxp")
237  {
238  if(outputcoords == "slam6d")
239  {
240  tmp = rxpio.read(
241  pos.scan_file.string(),
242  reduction,
243  riegl_to_slam_transform
244  );
245 
246  } else if(outputcoords == "lvr") {
247  tmp = rxpio.read(
248  pos.scan_file.string(),
249  reduction,
250  identity
251  );
252  }
253  } else {
254  // ascii etc
255 
256 
257  lvr2::Transformd inv_transform = pos.transform.inverse();
258  inv_transform.transpose();
259 
260  tmp = lvr2::ModelFactory::readModel(pos.scan_file.string());
261 
262  // ascii export is already transformed, have to transform it back.
263  // or find the button to save the ascii clouds without transformation
264  transformModel(tmp, inv_transform);
265 
266  if(outputcoords == "slam6d")
267  {
268  // convert to slam6d
269  std::cout << "[read_rxp_per_thread] Transform to slam6d" << std::endl;
270  transformModel(tmp, riegl_to_slam_transform);
271  }
272  }
273 
274  lvr2::ModelFactory::saveModel(tmp, out_file.string());
275 
276  mtx->lock();
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;
279  }
280 
281  mtx->unlock();
282 }
283 
285  lvr2::RieglProject &ri_proj,
286  const fs::path &out_scan_dir,
287  bool force_overwrite,
288  unsigned int reduction,
289  std::string output_coords = "slam6d"
290 ) {
291  //sub folders
292  fs::path scans_dir = out_scan_dir / "scans/";
293  fs::path images_dir = out_scan_dir / "images/";
294 
295  // @TODO maybe don't create dirs if nothing will be stored in them...
296  // create directory structure
297  try {
298  fs::create_directory(out_scan_dir);
299 
300  fs::create_directory(scans_dir);
301  fs::create_directory(images_dir);
302 
303  } catch (fs::filesystem_error &e) {
304  std::cout << "[convert_riegl_project] Error: " << e.what() << std::endl;
305  return false;
306  }
307 
308  // read pointclouds
309 
310  std::chrono::time_point<std::chrono::high_resolution_clock> start = std::chrono::high_resolution_clock::now();
311 
312  // get number of cores...
313  unsigned int num_cores = std::thread::hardware_concurrency();
314  if (!num_cores) {
315  num_cores = 1;
316  }
317 
318  std::mutex mtx;
319  std::thread *threads = new std::thread[num_cores];
320  std::vector<lvr2::ModelPtr> clouds;
321 
322  int read_file_count = 0;
323  int current_file_idx = 0;
324 
325  for (int j = 0; j < num_cores; j++) {
326  threads[j] = std::thread(
328  &ri_proj.m_scan_positions,
329  &read_file_count,
330  &current_file_idx,
331  &scans_dir,
332  &mtx,
333  j,
334  force_overwrite,
335  reduction,
336  ri_proj.m_input_cloud_format,
337  output_coords
338  );
339  }
340 
341  for (int j = 0; j < num_cores; j++) {
342  threads[j].join();
343  }
344 
345  delete[] threads;
346 
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);
349 
350  std::cout << "Reading " << read_file_count << " files took " << diff.count() << " sec." << std::endl;
351 
352  int scan_nr = 1;
353 
354  for (lvr2::ScanPosition &pos : ri_proj.m_scan_positions) {
355 
356  // copy pose files
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;
360  if (!write_mat4_to_pose_file(out_pose_file, pos.transform, force_overwrite)) {
361  std::cout << "[convert_riegl_project] Error: Error while trying to write file " << out_pose_file << std::endl;
362  return false;
363  }
364 
365  int image_nr = 1;
366  for (const lvr2::ImageFile &image : pos.images) {
367  //copy image files
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;
371 
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;
375  return false;
376  }
377 
378  //write out extrinsic transform
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,
382  force_overwrite)) {
383  std::cout << "[convert_riegl_project] Error: Error while writing image extrinsic \
384  matrix to " << (images_dir / out_file_buf) << std::endl;
385  }
386 
387  //write out orientation transform
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,
391  force_overwrite)) {
392  std::cout << "[convert_riegl_project] Error: Error while writing image orientation \
393  matrix to " << (images_dir / out_file_buf)<< std::endl;
394  }
395 
396  //write out intrinsic params
397  std::snprintf(out_file_buf, 2048, "scan%.3d_%.2d_intrinsic.txt", scan_nr, image_nr);
398  if (!write_params_to_file(images_dir / out_file_buf,
399  force_overwrite,
400  image.intrinsic_params,
401  4)) {
402  std::cout << "[convert_riegl_project] Error: Error while writing image intrinsic \
403  params to " << (images_dir / out_file_buf) << std::endl;
404  }
405 
406  //write out distortion params
407  std::snprintf(out_file_buf, 2048, "scan%.3d_%.2d_distortion.txt", scan_nr, image_nr);
408  if (!write_params_to_file(images_dir / out_file_buf,
409  force_overwrite,
410  image.distortion_params,
411  6)) {
412  std::cout << "[convert_riegl_project] Error: Error while writing image distortion \
413  params to " << (images_dir / out_file_buf) << std::endl;
414  }
415 
416  image_nr++;
417  }
418 
419  scan_nr++;
420  }
421 
422  return true;
423 }
424 
425 int main(int argc, char **argv) {
426  // @TODO move other informations into structure (calibrations)
427  Options opt;
428  if (!opt.parse(argc, argv)) {
429  return 0;
430  }
431 
432  std::cout << "Arguments:\n InputDir: " << opt.getInputDir() << "; OutputDir: " << opt.getOutputDir() << "; Forced: " << opt.force_overwrite() << "; Reduction: " << opt.getReductionFactor() << "; Start: " << opt.getStartscan() << "; End: " << opt.getEndscan() << std::endl;
433 
434 #if 1
436 
437 
438 
439  if (!tmp.parse_project(opt.getStartscan(), opt.getEndscan())) {
440  std::cout << "[main] Error: The directory \'" << opt.getInputDir() << "\' is NOT a Riegl Scanproject directory." << std::endl;
441 
442  return 0;
443  }
444 
445  if (!convert_riegl_project(tmp, fs::path(opt.getOutputDir()), opt.force_overwrite(), opt.getReductionFactor())) {
446  std::cout << "[main] Error: It occured an error while converting the Riegl Scan Project." << std::endl;
447 
448  return 0;
449  }
450 #else
451  fs::path scan_dir(opt.getInputDir());
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");
455 
456  std::vector<fs::path> cloud_files;
457  std::vector<fs::path> transform_files;
458 
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++) {
462 
463  fs::path current_file = *it;
464 
465  if (std::regex_match(current_file.filename().string(), files_3d_reg)) {
466  cloud_files.push_back(current_file);
467  }
468 
469  if (std::regex_match(current_file.filename().string(), files_4x4_reg)) {
470  transform_files.push_back(current_file);
471  }
472  }
473 
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; });
476 
477  std::vector<lvr2::ModelPtr> clouds;
478  std::vector<lvr2::Matrix4<Vec>> transforms;
479 
480  for (int i = 0; i < cloud_files.size(); i++) {
481  clouds.push_back(lvr2::ModelFactory::readModel(cloud_files[i].string()));
482  lvr2::Matrix4<Vec> trans;
483  trans.loadFromFile(transform_files[i].string());
484  trans.transpose();
485  transforms.push_back(trans);
486  }
487 
488  for (int i = 0; i < clouds.size(); i++) {
489  if (!clouds[i]->m_pointCloud)
490  continue;
491  size_t n;
492  lvr2::floatArr cloud = clouds[i]->m_pointCloud->getPointArray(n);
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;
502 
503  }
504 
505  lvr2::ModelFactory::saveModel(clouds[i], scan_dir.string() + "trans" + std::to_string(i) + ".3d");
506  }
507 
508 #endif
509  return 0;
510 }
lvr2::floatArr
boost::shared_array< float > floatArr
Definition: DataStruct.hpp:133
lvr2::ModelFactory::saveModel
static void saveModel(ModelPtr m, std::string file)
Definition: ModelFactory.cpp:225
BaseVector.hpp
Options
Definition: src/tools/lvr2_riegl_project_converter/Options.hpp:34
char_to_int
int char_to_int(char in)
Definition: main.cpp:101
RieglProject.hpp
lvr2::Matrix4
A 4x4 matrix class implementation for use with the provided vertex types.
Definition: Matrix4.hpp:64
lvr2::BaseVector< float >
transform
Definition: src/tools/lvr2_transform/Options.cpp:44
convert_riegl_project
bool convert_riegl_project(lvr2::RieglProject &ri_proj, const fs::path &out_scan_dir, bool force_overwrite, unsigned int reduction, std::string output_coords="slam6d")
Definition: main.cpp:284
write_params_to_file
bool write_params_to_file(fs::path dest, bool force_overwrite, ValueType *values, int count)
Definition: main.cpp:77
lvr2::Util::rad_to_deg
static ValueType rad_to_deg(ValueType rad)
Converts an angle from radian to degree.
Definition: Util.hpp:215
lvr2::RieglProject
Definition: RieglProject.hpp:48
transformModel
void transformModel(lvr2::ModelPtr model, const lvr2::Transformd &transform)
Definition: main.cpp:60
lvr2::Transformd
Transform< double > Transformd
4x4 double precision transformation matrix
Definition: MatrixTypes.hpp:71
RxpIO.hpp
main
int main(int argc, char **argv)
Definition: main.cpp:425
write_mat4_to_pose_file
bool write_mat4_to_pose_file(const fs::path file, const lvr2::Transformd &transform, bool force_overwrite)
Definition: main.cpp:149
lvr2::Matrix4::transpose
void transpose()
Transposes the current matrix.
Definition: Matrix4.hpp:406
Options::force_overwrite
bool force_overwrite()
Definition: src/tools/lvr2_riegl_project_converter/Options.hpp:62
Options::parse
bool parse(int argc, char **argv)
Definition: src/tools/lvr2_riegl_project_converter/Options.cpp:50
lvr2::RxpIO::read
ModelPtr read(std::string filename)
Reads .rxp files.
Definition: RxpIO.cpp:44
lvr2::RieglProject::parse_project
bool parse_project(unsigned int start, unsigned int end)
Definition: RieglProject.cpp:342
lvr2::ScanPosition
Definition: ScanTypes.hpp:276
lvr2::lvrToSlam6d
static Vector3< T > lvrToSlam6d(const Vector3< T > &in)
Lvr to Slam6D coordinate change: Point.
Definition: TransformUtils.hpp:444
lvr2::RxpIO
Reads .rxp files.
Definition: RxpIO.hpp:52
lvr2::RieglProject::m_input_cloud_format
std::string m_input_cloud_format
Definition: RieglProject.hpp:67
Options::getOutputDir
std::string getOutputDir()
Definition: src/tools/lvr2_riegl_project_converter/Options.hpp:46
scripts.create_png.values
values
Definition: create_png.py:26
MatrixTypes.hpp
Vec
BaseVector< float > Vec
Definition: src/tools/lvr2_cuda_normals/Main.cpp:57
Options::getInputFormat
std::string getInputFormat()
Definition: src/tools/lvr2_riegl_project_converter/Options.hpp:58
lvr2::RieglProject::m_scan_positions
std::vector< ScanPosition > m_scan_positions
Definition: RieglProject.hpp:61
Options::getReductionFactor
unsigned int getReductionFactor()
Definition: src/tools/lvr2_riegl_project_converter/Options.hpp:74
argc
int argc
Definition: tests_high_five_parallel.cpp:27
lvr2::eigenToEuler
void eigenToEuler(Transform< T > &mat, T *pose)
Definition: TransformUtils.hpp:630
file
FILE * file
Definition: arithmeticencoder.cpp:77
Util.hpp
convert_rxp_to_3d_per_thread
void convert_rxp_to_3d_per_thread(std::vector< lvr2::ScanPosition > *work, int *read_file_count, int *current_file_idx, const fs::path *scans_dir, std::mutex *mtx, int id, bool force_overwrite, unsigned int reduction, std::string inputformat="rxp", std::string outputcoords="slam6d")
Definition: main.cpp:190
Options::getEndscan
unsigned int getEndscan()
Definition: src/tools/lvr2_riegl_project_converter/Options.hpp:66
lvr2::ModelPtr
std::shared_ptr< Model > ModelPtr
Definition: Model.hpp:80
Options::getInputDir
std::string getInputDir()
Definition: src/tools/lvr2_riegl_project_converter/Options.hpp:54
Options.hpp
lvr2::ModelFactory::readModel
static ModelPtr readModel(std::string filename)
Definition: ModelFactory.cpp:65
ModelFactory.hpp
Options::getStartscan
unsigned int getStartscan()
Definition: src/tools/lvr2_riegl_project_converter/Options.hpp:70
lvr2::Matrix4::loadFromFile
void loadFromFile(string filename)
Loads matrix values from a given file.
Definition: Matrix4.hpp:470
copy_file
bool copy_file(const fs::path from, const fs::path to, bool force_overwrite)
Definition: main.cpp:130
argv
char ** argv
Definition: tests_high_five_parallel.cpp:28
write_mat4_to_file
bool write_mat4_to_file(lvr2::Transformd mat, fs::path dest, bool force_overwrite)
Definition: main.cpp:106
TransformUtils.hpp


lvr2
Author(s): Thomas Wiemann , Sebastian Pütz , Alexander Mock , Lars Kiesow , Lukas Kalbertodt , Tristan Igelbrink , Johan M. von Behren , Dominik Feldschnieders , Alexander Löhr
autogenerated on Wed Mar 2 2022 00:37:24