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 |
|