Statistics
| Branch: | Revision:

root / rgbdslam / gicp / scan2ascii.cpp @ 9240aaa3

History | View | Annotate | Download (4.53 KB)

1
/*************************************************************
2
  Generalized-ICP Copyright (c) 2009 Aleksandr Segal.
3
  All rights reserved.
4

5
  Redistribution and use in source and binary forms, with 
6
  or without modification, are permitted provided that the 
7
  following conditions are met:
8

9
* Redistributions of source code must retain the above 
10
  copyright notice, this list of conditions and the 
11
  following disclaimer.
12
* Redistributions in binary form must reproduce the above
13
  copyright notice, this list of conditions and the 
14
  following disclaimer in the documentation and/or other
15
  materials provided with the distribution.
16
* The names of the contributors may not be used to endorse
17
  or promote products derived from this software
18
  without specific prior written permission.
19

20
  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND
21
  CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED
22
  WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
23
  WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
24
  PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25
  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, 
26
  INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 
27
  CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, 
28
  PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
29
  DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
30
  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 
31
  CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE 
32
  OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
33
  SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH
34
  DAMAGE.
35
*************************************************************/
36

    
37

    
38

    
39
#include <iostream>
40
#include <fstream>
41
#include <string>
42
#include <sstream>
43

    
44
// program options
45
#include <boost/program_options.hpp>
46
#include <boost/filesystem/path.hpp>
47
#include <boost/filesystem/fstream.hpp>
48
#include <boost/tokenizer.hpp> 
49

    
50
#include "scan.h"
51
#include "transform.h"
52

    
53
using namespace std;
54
namespace po = boost::program_options;
55

    
56
static string filename_in;
57
static string filename_out;
58
static string filename_tfm_out;
59

    
60
void print_usage(const char* program_name, po::options_description const& desc) {
61
  cout << program_name << " scan_in [scan_out]" << endl;
62
  cout << desc << endl;  
63
}
64

    
65
bool parse_options(int argc, char** argv) {
66
  bool error = false;
67
  po::options_description desc("Allowed options");
68
  desc.add_options()
69
    ("help", "print help message")
70
    ("scan_in", po::value<string>(), "input scan filename")
71
    ("scan_out", po::value<string>(), "output scan filename");
72

    
73
  po::positional_options_description p;
74
  p.add("scan_in", 1);
75
  p.add("scan_out", 1);
76

    
77
  po::variables_map vm;
78
  try {    
79
    po::store(po::command_line_parser(argc, argv).options(desc).positional(p).run(), vm);
80
    po::notify(vm);
81
  }
82
  catch(const std::exception &e) {
83
    cout << "Error: " << e.what() << endl;
84
    print_usage(argv[0], desc);
85
    error = true;
86
    return error;
87
  }
88

    
89
  if(vm.count("help")) {
90
    print_usage(argv[0], desc);
91
    error = true;
92
    return error;
93
  }
94

    
95

    
96
  if(!vm.count("scan_in")) {
97
    cout << "Error: scan_in filename not specified!" << endl;
98
    error = true;
99
  }
100
  else {
101
    filename_in = vm["scan_in"].as<string>();
102
  }
103

    
104
  if(!vm.count("scan_out")) {
105
    filename_out = filename_in;
106
    
107
    int dot_pos = filename_out.find_last_of(".");
108
    if(dot_pos != -1) {
109
      filename_out = filename_out.substr(0, dot_pos);
110
    }
111
    filename_out += ".ascii";
112
  }
113
  else {
114
    filename_out = vm["scan_out"].as<string>();
115
  }
116

    
117
  filename_tfm_out = filename_out;
118

    
119
  int dot_pos = filename_tfm_out.find_last_of(".");
120
  if(dot_pos != -1) {
121
    filename_tfm_out = filename_tfm_out.substr(0, dot_pos);   
122
  }
123
  filename_tfm_out += ".tfm";
124
  
125
  return error;
126
}
127

    
128

    
129
int main(int argc, char** argv) {
130
  bool error;
131

    
132
  error = parse_options(argc, argv);
133
  
134
  if(error) {
135
    return 1;
136
  }
137
  dgc_scan_t scan;
138
  
139
  
140
  scan.load(filename_in.c_str());
141

    
142
  ofstream fout(filename_out.c_str());
143

    
144
  if(!fout) {
145
    cout << "Error: could not open output file " << filename_out << endl;
146
    return 1;
147
  }
148
  
149
  for(int i = 0; i < scan.points.size(); i++) {
150
    fout << scan.points[i].vec[0] << "\t";
151
    fout << scan.points[i].vec[1] << "\t";
152
    fout << scan.points[i].vec[2] << endl;;
153
  }
154

    
155
  dgc_transform_t pose_tfm;
156
  dgc_transform_identity(pose_tfm);
157
  dgc_transform_rotate_x(pose_tfm, scan.pose.roll);
158
  dgc_transform_rotate_y(pose_tfm, scan.pose.pitch);
159
  dgc_transform_rotate_z(pose_tfm, scan.pose.yaw);
160
  dgc_transform_translate(pose_tfm, scan.pose.x, scan.pose.y, scan.pose.z);
161

    
162
  dgc_transform_write(pose_tfm, filename_tfm_out.c_str());
163

    
164
  fout.close();
165
  return 0;
166
}
167