src/tools/lvr2_slam6d_merger/Main.cpp
Go to the documentation of this file.
1 
35 #include <iostream>
36 #include <algorithm>
37 #include <string>
38 #include <stdio.h>
39 #include <cstdio>
40 #include <fstream>
41 #include <utility>
42 #include <iterator>
43 using namespace std;
44 
45 #include <boost/filesystem.hpp>
46 
47 
48 #include <Eigen/Dense>
49 
50 #include "Options.hpp"
51 
52 #include "lvr2/io/Timestamp.hpp"
53 #include "lvr2/io/ModelFactory.hpp"
54 #include "lvr2/io/IOUtils.hpp"
58 
59 #ifdef LVR2_USE_PCL
61 #endif
62 
63 #define BUF_SIZE 1024
64 
65 namespace slam6dmerger
66 {
67 
68 using namespace lvr2;
69 
70 boost::filesystem::path getCorrespondingPath(const boost::filesystem::path& scan, const string& extension)
71 {
72  std::stringstream ss;
73  ss << scan.stem().string() << extension;
74  return boost::filesystem::path(ss.str());
75 }
76 
77 
78 
79 } // namespace slam6dmerger
80 
81 
82 int main(int argc, char** argv)
83 {
84  using namespace slam6dmerger;
85  using boost::filesystem::path;
86  using boost::filesystem::directory_iterator;
87 
89 
90  std::cout << options << std::endl;
91 
92  // CHECK PARAMETERS ---------------------------------------------------------------------------------
93 
94  path transformPath(options.getTransformFile());
95  if(!exists(transformPath) || !is_regular_file(transformPath))
96  {
97  std::cout << timestamp << "Could not open transformation file " << options.getTransformFile() << std::endl;
98  exit(-1);
99  }
100 
101  Transformf transform = getTransformationFromFrames<float>(transformPath);
102  //transform = inverseTransform(transform);
103  BaseVector<float> transform_position;
104  BaseVector<float> transform_angles;
105  getPoseFromMatrix(transform_position, transform_angles, transform);
106 
107  std::cout << timestamp << "Transforming: " << std::endl << std::endl;
108  std::cout << transform << std::endl << std::endl;
109 
110  path inputDir(options.getInputDir());
111  if(!is_directory(inputDir))
112  {
113  std::cout << timestamp << "Input directory is not valid: " << options.getInputDir() << std::endl;
114  exit(-1);
115  }
116 
117  path outputDir(options.getOutputDir());
118  if(!is_directory(inputDir))
119  {
120  std::cout << timestamp << "Output directory is not valid: " << options.getOutputDir() << std::endl;
121  exit(-1);
122  }
123 
124  if(inputDir == outputDir)
125  {
126  std::cout << timestamp << "Input directory and output directory should not be equal." << std::endl;
127  exit(-1);
128  }
129 
130  path mergeDir(options.getMergeDir());
131  if(!is_directory(mergeDir))
132  {
133  std::cout << timestamp << "Merge directory is not valid: " << options.getMergeDir() << std::endl;
134  exit(-1);
135  }
136 
137  if(mergeDir == outputDir)
138  {
139  std::cout << timestamp << "Merge directory and output directory should not be equal." << std::endl;
140  exit(-1);
141  }
142 
143 
145 
146  vector<path> input_scans;
147  vector<path> merge_scans;
148 
149  directory_iterator end;
150  for(directory_iterator it(inputDir); it != end; ++it)
151  {
152  string extension = it->path().extension().string();
153  if(extension == ".3d")
154  {
155  input_scans.push_back(it->path());
156  }
157  }
158 
159  for(directory_iterator it(mergeDir); it != end; ++it)
160  {
161  string extension = it->path().extension().string();
162  if(extension == ".3d")
163  {
164  merge_scans.push_back(it->path());
165  }
166  }
167 
168  std::sort(input_scans.begin(), input_scans.end());
169  std::sort(merge_scans.begin(), merge_scans.end());
170 
171  // Copy files from input directory and merge directory
172  // and assure consistent numbering
173  int scan_counter = 0;
174  char name_buffer[256];
175  for(auto current_path : input_scans)
176  {
177  // -------->>>> SCAN FILE
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);
182 
183  // -------->>>> OCT FILE
184 
185  path oct_in = inputDir / getCorrespondingPath(current_path, ".oct");
186  if(exists(oct_in))
187  {
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);
192  }
193 
194  // -------->>>> FRAMES
195 
196  // Try to find frames file for current scan
197  path frames_in = inputDir / getCorrespondingPath(current_path, ".frames");
198 
199  // Generate target path for frames file
200  sprintf(name_buffer, "scan%03d.frames", scan_counter);
201  path frames_out = outputDir / path(name_buffer);
202 
203  // Check for exisiting frames file
204  if(!exists(frames_in))
205  {
206  std::cout << timestamp << "Warning: Could not find " << frames_in.string() << std::endl;
207  }
208  else
209  {
210  std::cout << timestamp << "Copying " << frames_in.string() << " to " << frames_out.string() << "." << std::endl;
211  boost::filesystem::copy(frames_in, frames_out);
212  }
213 
214  // ------->>>> POSE
215 
216  // Try to find pose file for current scan
217  path pose_in = inputDir / getCorrespondingPath(current_path, ".pose");
218 
219  // Generate target path for frames file
220  sprintf(name_buffer, "scan%03d.pose", scan_counter);
221  path pose_out = outputDir / path(name_buffer);
222 
223  // Check for exisiting frames file
224  if(!exists(pose_in))
225  {
226  std::cout << timestamp << "Warning: Could not find " << pose_in.string() << std::endl;
227  }
228  else
229  {
230  std::cout << timestamp << "Copying " << pose_in.string() << " to " << pose_out.string() << "." << std::endl;
231  boost::filesystem::copy(pose_in, pose_out);
232  }
233 
234 
235  scan_counter++;
236  }
237 
238  for(auto current_path : merge_scans)
239  {
240  // -------->>>> SCAN
241  // Copy scan file
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);
246 
247  // -------->>>> OCT FILE
248 
249  path oct_in = inputDir / getCorrespondingPath(current_path, ".oct");
250  if(exists(oct_in))
251  {
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);
256  }
257 
258  // -------->>>> FRAMES
259 
260  // Try to find frames file for current scan
261  path frames_in = mergeDir / getCorrespondingPath(current_path, ".frames");
262 
263  // Generate target path for frames file
264  sprintf(name_buffer, "scan%03d.frames", scan_counter);
265  path frames_out = outputDir / path(name_buffer);
266 
267  // Check for exisiting frames file
268  if(!exists(frames_in))
269  {
270  std::cout << timestamp << "Warning: Could not find " << frames_in.string() << std::endl;
271  }
272  else
273  {
274  // Get transformation from file and transform
275  std::cout << timestamp << "Transforming " << frames_in.string() << std::endl;
276  Transformf registration = getTransformationFromFrames<float>(frames_in);
277  //registration *= transform;
278  Transformf t_reg = transformRegistration<float>(transform, registration);
279 
280  std::cout << timestamp << "Writing transformed registration to " << frames_out.string() << std::endl;
281  writeFrame(t_reg, frames_out);
282 
283  }
284 
285  // ------->>>> POSE
286 
287  path pose_in = mergeDir / getCorrespondingPath(current_path, ".pose");
288 
289  // Generate target path for frames file
290  sprintf(name_buffer, "scan%03d.pose", scan_counter);
291  path pose_out = outputDir / path(name_buffer);
292 
293  // Check for exisiting frames file
294  if(!exists(frames_in))
295  {
296  std::cout << timestamp << "Warning: Could not find " << frames_in.string() << std::endl;
297  }
298  else
299  {
300  // Get transformation from file and transform
301  std::cout << timestamp << "Transforming " << pose_in.string() << std::endl;
302  BaseVector<float> pos;
303  BaseVector<float> ang;
304  getPoseFromFile(pos, ang, pose_in);
305 
306  pos += transform_position;
307  ang += transform_angles;
308 
309  std::cout << timestamp << "Writing transformed pose estimat to " << pose_out.string() << std::endl;
310  writePose(pos, ang, pose_out);
311 
312  }
313 
314 
315  scan_counter++;
316  }
317 
318  return 0;
319 }
BaseVector.hpp
lvr2::BaseVector< float >
transform
Definition: src/tools/lvr2_transform/Options.cpp:44
main
int main(int argc, char **argv)
Definition: src/tools/lvr2_slam6d_merger/Main.cpp:82
kaboom::Options::getInputDir
string getInputDir() const
Definition: src/tools/lvr2_kaboom/Options.cpp:98
lvr2::getPoseFromFile
void getPoseFromFile(BaseVector< float > &position, BaseVector< float > &angles, const boost::filesystem::path file)
Loads an Euler representation of from a pose file.
Definition: IOUtils.cpp:285
lvr2::writeFrame
void writeFrame(const Transform< T > &transform, const boost::filesystem::path &framesOut)
Writes a Eigen transformation into a .frames file.
IOUtils.hpp
lvr2::getPoseFromMatrix
void getPoseFromMatrix(BaseVector< T > &position, BaseVector< T > &angles, const Transform< T > &mat)
Computes a Euler representation from the given transformation matrix.
options
const kaboom::Options * options
Definition: src/tools/lvr2_kaboom/Main.cpp:45
slam6dmerger::Options
A class to parse the program options for the reconstruction executable.
Definition: src/tools/lvr2_slam6d_merger/Options.hpp:59
Options.hpp
PCLFiltering.hpp
lvr2::timestamp
static Timestamp timestamp
A global time stamp object for program runtime measurement.
Definition: Timestamp.hpp:116
argc
int argc
Definition: tests_high_five_parallel.cpp:27
slam6dmerger
Definition: src/tools/lvr2_slam6d_merger/Main.cpp:65
lvr2::Transformf
Transform< float > Transformf
4x4 single precision transformation matrix
Definition: MatrixTypes.hpp:68
std
Definition: HalfEdge.hpp:124
Matrix4.hpp
lvr2
Definition: BaseBufferManipulators.hpp:39
slam6dmerger::getCorrespondingPath
boost::filesystem::path getCorrespondingPath(const boost::filesystem::path &scan, const string &extension)
Definition: src/tools/lvr2_slam6d_merger/Main.cpp:70
Timestamp.hpp
ModelFactory.hpp
lvr2::writePose
void writePose(const BaseVector< float > &position, const BaseVector< float > &angles, const boost::filesystem::path &out)
Writes pose information in Euler representation to the given file.
Definition: IOUtils.cpp:154
kaboom::Options::getOutputDir
string getOutputDir() const
Definition: src/tools/lvr2_kaboom/Options.cpp:103
argv
char ** argv
Definition: tests_high_five_parallel.cpp:28
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