root / rgbdslam / gicp / scan2ascii.cpp @ 9240aaa3
History | View | Annotate | Download (4.53 KB)
1 | 9240aaa3 | Alex | /*************************************************************
|
---|---|---|---|
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 | } |