root / rgbdslam / gicp / scan.cpp @ 9240aaa3
History | View | Annotate | Download (2.77 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 "scan.h" |
40 |
#include <stdio.h> |
41 |
|
42 |
|
43 |
int dgc_scan_t::save(const char* filename) { |
44 |
int i;
|
45 |
int n = points.size();
|
46 |
int m = norms.size();
|
47 |
|
48 |
FILE *f = fopen(filename, "w");
|
49 |
if(f == NULL) { |
50 |
fprintf(stderr, "Couldn't open file.\n");
|
51 |
return 0; |
52 |
} |
53 |
fwrite(&n, sizeof(int),1, f); |
54 |
fwrite(&pose, sizeof(dgc_pose_t),1, f); |
55 |
|
56 |
for(i = 0; i < n; i++) { |
57 |
fwrite(&points[i], sizeof(dgc_vector3d_c_t), 1, f); |
58 |
} |
59 |
fwrite(&m, sizeof(int),1, f); |
60 |
for(i = 0; i < m; i++) { |
61 |
fwrite(&norms[i], sizeof(dgc_vector3d_c_t), 1, f); |
62 |
} |
63 |
|
64 |
fclose(f); |
65 |
|
66 |
return 1; |
67 |
} |
68 |
|
69 |
|
70 |
int dgc_scan_t::load(const char *filename) { |
71 |
int i, n, m;
|
72 |
points.clear(); |
73 |
norms.clear(); |
74 |
|
75 |
FILE *f = fopen(filename, "r");
|
76 |
if(f == NULL) { |
77 |
fprintf(stderr, "Couldn't open file.\n");
|
78 |
return 0; |
79 |
} |
80 |
fread(&n, sizeof(int), 1, f); |
81 |
fread(&pose, sizeof(dgc_pose_t), 1, f); |
82 |
|
83 |
dgc_vector3d_c_t pt; |
84 |
for(i = 0; i < n; i++) { |
85 |
fread(&pt, sizeof(dgc_vector3d_c_t), 1, f); |
86 |
points.push_back(pt); |
87 |
} |
88 |
|
89 |
fread(&m, sizeof(int), 1, f); |
90 |
dgc_vector3d_c_t nu; |
91 |
for(i = 0; i < m; i++) { |
92 |
fread(&nu, sizeof(dgc_vector3d_c_t), 1, f); |
93 |
norms.push_back(nu); |
94 |
} |
95 |
fclose(f); |
96 |
return 1; |
97 |
} |