RieglProject.cpp
Go to the documentation of this file.
1 
28 #include "RieglProject.hpp"
29 
30 #include <algorithm>
31 #include <iostream>
32 #include <regex>
33 
37 namespace lvr2
38 {
39 
40 using Vec = BaseVector<float>;
41 
42 // @TODO more errorprone against invalid input?
43 // And this doesn't belong in here, it should be in Matrix4.
44 Transformd string2mat4f(const std::string data)
45 {
46  double mat[16];
47  Transformd ret;
48 
49  int count = 0;
50  std::string::size_type n = 0;
51  std::string::size_type sz = 0;
52  while (count < 16) {
53  mat[count++] = std::stof(data.substr(n), &sz);
54  n += sz;
55  n = data.find_first_not_of(' ', n);
56  }
57 
58  return buildTransformation(mat);
59 }
60 
61 std::string get_first_group_regex(std::regex regex_string, std::string data) {
62  std::regex regex(regex_string);
63  std::smatch sm;
64  std::regex_match(data, sm, regex);
65 
66  return sm.str(1);
67 }
68 
70  std::string dir,
71  std::string input_cloud_format
72 )
73 :m_project_dir(fs::path(dir))
74 ,m_input_cloud_format(input_cloud_format)
75 {
76 
77 }
78 
79 void RieglProject::parse_scanpositions(pt::ptree project_ptree, unsigned int start, unsigned int end) {
80  int scan_id = 0;
81  for (auto scanpos_info : project_ptree.get_child("project.scanpositions")) {
82  if (scanpos_info.first != "scanposition") continue;
83 
84  ScanPosition scanpos;
85 
86  std::string scan_name = scanpos_info.second.get<std::string>("name");
87 
88  if (!scan_name.length() == 10) {
89  std::cout << "[RieglProject] Warning: The scanpos " << scan_name << " is skipped"
90  << " because the name is malformed." << std::endl;
91 
92  continue;
93  }
94 
95  unsigned int scan_pos_nr = std::stoul(scan_name.substr(7));
96 
97  // skip scanpositions that aren't in range
98  if (scan_pos_nr < start || (end != 0 && scan_pos_nr > end)) {
99  continue;
100  }
101 
102  fs::path scan_dir = m_project_dir / ("SCANS/" + scan_name + "/");
103 
104  unsigned int scan_file_size = 0;
105 
106  // find the .rxp file for scanpos
107 
108  // @TODO What should happen in this case if it is possible?
109  // Currently we take the biggest file but maybe we should merge all scan files.
110  for (auto scan_data : scanpos_info.second.get_child("singlescans")) {
111  if (scan_data.first != "scan") continue;
112 
113  std::string current_scan_filename = scan_data.second.get<std::string>("file");
114 
115  // we don't want .mon files
116  if (current_scan_filename.find(".mon") != std::string::npos) {
117  continue;
118  }
119 
120  fs::path current_scan_file = scan_dir / ("SINGLESCANS/" + current_scan_filename);
121 
122  if (!fs::exists(current_scan_file) || !fs::is_regular_file(current_scan_file)) {
123  std::cout << "[RieglProject] Warning: The scan data file '" << current_scan_file
124  << "' doesn't exists or isn't a regular file and there \
125  is gonna be skipped." << std::endl;
126 
127  continue;
128  }
129 
130  unsigned int current_scan_file_size = fs::file_size(current_scan_file);
131 
132  if ( current_scan_file_size > scan_file_size) {
133  scanpos.scan_file = current_scan_file;
134  scan_file_size = current_scan_file_size;
135  }
136  }
137 
138  // @TODO Maybe it should be possible to have scanpositions without scan data but
139  // with images. Then we would have to change this behaviour.
140  if (scanpos.scan_file.empty()) {
141  std::cout << "[RieglProject] Warning: The scanposition '" << scan_dir
142  << "' has no scan data and therefore we gonna skip this scanposition."
143  << std::endl;
144 
145  continue;
146  }
147 
148  // parse scanpos transformation
149  std::string transform = scanpos_info.second.get<std::string>("sop.matrix");
150  scanpos.transform = string2mat4f(transform);
151 
152  parse_images_per_scanpos(scanpos, scanpos_info.second, project_ptree);
153 
154  // @TODO maybe we shouldn't skip scanpositions only because they have no images.
155  if (scanpos.images.empty()) {
156  std::cout << "[RieglProject] Warning: Scanposition '" << scan_dir
157  << "' has no images and will be skipped." << std::endl;
158 
159  // continue;
160  }
161 
162  if(scan_id < m_scan_positions.size())
163  {
164  m_scan_positions[scan_id] = scanpos;
165  } else {
166  m_scan_positions.push_back(scanpos);
167  }
168 
169  scan_id++;
170  }
171 
172 }
173 
175  pt::ptree scanpos_ptree,
176  pt::ptree project_ptree) {
177 
178  for (auto img_info : scanpos_ptree.get_child("scanposimages")) {
179  if (img_info.first != "scanposimage") continue;
180 
181  ImageFile img;
182 
183  std::string img_file = img_info.second.get<std::string>("file");
184  img.image_file = scanpos.scan_file.parent_path().parent_path() / ("SCANPOSIMAGES/" + img_file);
185 
186 
187  if (!fs::exists(img.image_file) && !fs::is_regular_file(img.image_file)) {
188  std::cout << "[RieglProject] Warning: Image file '" << img.image_file
189  << "' doesn't exists or is not a regular file and there is skipped."
190  << std::endl;
191 
192  continue;
193  }
194 
195 
196 
197  // get orientation of image
198  img.orientation_transform = string2mat4f(img_info.second.get<std::string>("cop.matrix"));
199 
200 
201 
202  // @TODO refactor mountcalib and camcalib reference search into one...
203 
204  //get extrinsic transformation for image
205  std::string mountcalib_ref = img_info.second.get<std::string>("mountcalib_ref.<xmlattr>.noderef");
206  mountcalib_ref = mountcalib_ref.substr(mountcalib_ref.find_last_of('/') + 1);
207 
208  bool found_mountcalib = false;
209  for (auto mountcalib_info : project_ptree.get_child("project.calibrations.mountcalibs")) {
210  if (mountcalib_info.first != "mountcalib") continue;
211  if (mountcalib_info.second.get<std::string>("<xmlattr>.name") != mountcalib_ref) continue;
212 
213  found_mountcalib = true;
214 
215  img.extrinsic_transform = string2mat4f(mountcalib_info.second.get<std::string>("matrix"));
216  }
217 
218  // skip image if no calibration data was found...
219  if (!found_mountcalib) {
220  std::cout << "[RieglProject] Warning: Extrinsic transformation for image file '"
221  << img.image_file << "' wasn't found and image is gonna be skipped."
222  << std::endl;
223 
224  continue;
225  }
226 
227 
228 
229  //get intrinsic params for image
230  std::string camcalib_ref = img_info.second.get<std::string>("camcalib_ref.<xmlattr>.noderef");
231  camcalib_ref = camcalib_ref.substr(camcalib_ref.find_last_of('/') + 1);
232 
233  bool found_camcalib = false;
234  for (auto camcalib_info : project_ptree.get_child("project.calibrations.camcalibs")) {
235  if (camcalib_info.first != "camcalib_opencv") continue;
236  if (camcalib_info.second.get<std::string>("<xmlattr>.name") != camcalib_ref) continue;
237 
238  found_camcalib = true;
239 
240  pt::ptree intrinsic_ptree = camcalib_info.second.get_child("internal_opencv");
241 
242  img.intrinsic_params[0] = intrinsic_ptree.get<float>("fx");
243  img.intrinsic_params[1] = intrinsic_ptree.get<float>("fy");
244  img.intrinsic_params[2] = intrinsic_ptree.get<float>("cx");
245  img.intrinsic_params[3] = intrinsic_ptree.get<float>("cy");
246 
247  img.distortion_params[0] = intrinsic_ptree.get<float>("k1");
248  img.distortion_params[1] = intrinsic_ptree.get<float>("k2");
249  img.distortion_params[2] = intrinsic_ptree.get<float>("k3");
250  img.distortion_params[3] = intrinsic_ptree.get<float>("k4");
251  img.distortion_params[4] = intrinsic_ptree.get<float>("p1");
252  img.distortion_params[5] = intrinsic_ptree.get<float>("p2");
253  }
254 
255  // skip image if no calibration data was found...
256  if (!found_camcalib) {
257  std::cout << "[RieglProject] Warning: Camcalibration for image file '" << img.image_file
258  << "' wasn't found and image is gonna be skipped." << std::endl;
259 
260  continue;
261  }
262 
263  scanpos.images.push_back(img);
264  }
265 }
266 
268 {
269 
270  fs::path scans_path = m_project_dir / "SCANS";
271 
272  if(!fs::exists(scans_path))
273  {
274  std::stringstream ss;
275  ss << "[RieglProject] Error: RiSCAN scans path '" << scans_path
276  << "' doesn't exist or isn't a directory";
277  throw fs::filesystem_error(
278  ss.str(),
279  scans_path,
280  boost::system::errc::make_error_code(boost::system::errc::not_a_directory)
281  );
282  }
283 
284  std::vector<fs::path> path_vec;
285 
286 
287  std::copy(
288  fs::directory_iterator{scans_path},
289  fs::directory_iterator{},
290  std::back_inserter(path_vec)
291  );
292 
293  // sort directories by name
294  std::sort(path_vec.begin(), path_vec.end());
295 
296  for(int scan_id = 0; scan_id < path_vec.size(); scan_id ++)
297  {
298  fs::path cloud_path = path_vec[scan_id] / "POINTCLOUDS";
299 
300  if(!fs::exists(cloud_path))
301  {
302  std::stringstream ss;
303  ss << "[RieglProject] Error: RiSCAN cloud path '" << cloud_path
304  << "' doesn't exist or isn't a directory";
305  throw fs::filesystem_error(
306  ss.str(),
307  cloud_path,
308  boost::system::errc::make_error_code(boost::system::errc::not_a_directory)
309  );
310  }
311 
312  std::vector<fs::path> potential_clouds;
313 
314  std::copy(
315  fs::directory_iterator{cloud_path},
316  fs::directory_iterator{},
317  std::back_inserter(potential_clouds)
318  );
319 
320  std::sort(
321  potential_clouds.begin(),
322  potential_clouds.end(),
323  [](const fs::path& a, const fs::path& b) {
324  return fs::file_size(a) > fs::file_size(b);
325  }
326  );
327 
328  fs::path biggest_cloud_path = potential_clouds[0];
329 
330  if(scan_id < m_scan_positions.size())
331  {
332  m_scan_positions[scan_id].scan_file = biggest_cloud_path;
333  } else {
334  ScanPosition sp;
335  sp.scan_file = biggest_cloud_path;
336  m_scan_positions.push_back(sp);
337  }
338 
339  }
340 }
341 
342 bool RieglProject::parse_project(unsigned int start, unsigned int end) {
343  // check if project path exists
344  if (!fs::exists(m_project_dir) || !fs::is_directory(m_project_dir)) {
345  std::cout << "[RieglProject] Error: RiSCAN project path '" << m_project_dir
346  << "' doesn't exist or isn't a directory" << std::endl;
347 
348  return false;
349  }
350 
351  // check if project.rsp exists
352  fs::path project_file = m_project_dir / "project.rsp";
353  if (!fs::exists(project_file) || !fs::is_regular_file(project_file)) {
354  std::cout << "[RieglProject] Error: The RiSCAN project file '" << project_file
355  << "' doesn't exist or isn't a file." << std::endl;
356 
357  return false;
358  }
359 
360  // read project.rsp file
361  pt::ptree project_ptree;
362  pt::read_xml(project_file.string(), project_ptree);
363 
364  parse_scanpositions(project_ptree, start, end);
365 
366  if (m_scan_positions.empty()) {
367  std::cout << "[RieglProject] Error: Unable to parse any scanposition." << std::endl;
368 
369  return false;
370  }
371 
372  // search for ascii pointcloud files if input cloud format is specified as "ascii"
373  if(m_input_cloud_format == "ascii")
374  {
376  }
377 
378  return true;
379 }
380 
381 std::ostream& operator<<(std::ostream &lhs, const RieglProject &rhs) {
382  lhs << "Scan Project dir: " << rhs.m_project_dir << "\n";
383 
384  for (const lvr2::ScanPosition &sp : rhs.m_scan_positions) {
385  lhs << "\n" << sp;
386  }
387 
388  return lhs;
389 }
390 
391 std::ostream& operator<<(std::ostream &lhs, const ScanPosition &rhs) {
392  lhs << "Scan File: " << rhs.scan_file << " "
393  << fs::file_size(rhs.scan_file) << "\n"
394  << rhs.transform
395  << "Images: ";
396 
397  for (ImageFile img : rhs.images) {
398  lhs << "\n\t" << img.image_file.filename() << " " << fs::file_size(img.image_file) << '\n'
399  << "Orientation: " << img.orientation_transform << '\n'
400  << "Extrinsic: " << img.extrinsic_transform << '\n';
401 
402  lhs << "Intrinsic: ";
403  for (int i = 0; i < 4; i++) {
404  lhs << img.intrinsic_params[i] << " ";
405  }
406 
407  lhs << '\n' << "Distortion: ";
408  for (int i = 0; i < 6; i++) {
409  lhs << img.distortion_params[i] << " ";
410  }
411  }
412 
413  lhs << std::endl;
414 
415  return lhs;
416 }
417 
418 } // namespace lvr2
lvr2::RieglProject::RieglProject
RieglProject(std::string dir, std::string input_cloud_format="rxp")
Construct a new Riegl Project object.
Definition: RieglProject.cpp:69
lvr2::RieglProject::parse_asciiclouds
void parse_asciiclouds()
Definition: RieglProject.cpp:267
BaseVector.hpp
RieglProject.hpp
transform
Definition: src/tools/lvr2_transform/Options.cpp:44
lvr2::RieglProject::parse_scanpositions
void parse_scanpositions(pt::ptree project_ptree, unsigned int start, unsigned int end)
Definition: RieglProject.cpp:79
lvr2::RieglProject
Definition: RieglProject.hpp:48
lvr2::buildTransformation
Transform< T > buildTransformation(T *alignxf)
Transforms an slam6d transformation matrix into an Eigen 4x4 matrix.
lvr2::Transformd
Transform< double > Transformd
4x4 double precision transformation matrix
Definition: MatrixTypes.hpp:71
lvr2::operator<<
std::ostream & operator<<(std::ostream &os, const BaseVector< T > &v)
Definition: BaseVector.hpp:227
lvr2::RieglProject::parse_project
bool parse_project(unsigned int start, unsigned int end)
Definition: RieglProject.cpp:342
lvr2::ScanPosition
Definition: ScanTypes.hpp:276
lvr2::RieglProject::parse_images_per_scanpos
void parse_images_per_scanpos(ScanPosition &scanpos, pt::ptree scanpos_ptree, pt::ptree project_ptree)
Definition: RieglProject.cpp:174
lvr2::RieglProject::m_project_dir
fs::path m_project_dir
Definition: RieglProject.hpp:60
lvr2::RieglProject::m_input_cloud_format
std::string m_input_cloud_format
Definition: RieglProject.hpp:67
MatrixTypes.hpp
Vec
BaseVector< float > Vec
Definition: src/tools/lvr2_cuda_normals/Main.cpp:57
lvr2::RieglProject::m_scan_positions
std::vector< ScanPosition > m_scan_positions
Definition: RieglProject.hpp:61
lvr2
Definition: BaseBufferManipulators.hpp:39
lvr2::get_first_group_regex
std::string get_first_group_regex(std::regex regex_string, std::string data)
Definition: RieglProject.cpp:61
lvr2::string2mat4f
Transformd string2mat4f(const std::string data)
Definition: RieglProject.cpp:44
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