root / rgbdslam / gicp / scan.cpp @ 9240aaa3
History | View | Annotate | Download (2.77 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 "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 | } |