root / rgbdslam / gicp / gicp.h @ 9240aaa3
History | View | Annotate | Download (3.46 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 | #ifndef GICP_H_
|
||
40 | #define GICP_H_
|
||
41 | |||
42 | #include <ANN/ANN.h> |
||
43 | #include <vector> |
||
44 | #include <iostream> |
||
45 | //#include <gsl/gsl.h>
|
||
46 | #include <semaphore.h> |
||
47 | #include "transform.h" |
||
48 | |||
49 | namespace dgc { |
||
50 | namespace gicp { |
||
51 | typedef double gicp_mat_t[3][3]; |
||
52 | |||
53 | struct GICPPoint {
|
||
54 | double x, y, z;
|
||
55 | float range;
|
||
56 | gicp_mat_t C; // covariance matrix
|
||
57 | }; |
||
58 | |||
59 | class GICPPointSet { |
||
60 | public:
|
||
61 | GICPPointSet(); |
||
62 | ~GICPPointSet(); |
||
63 | void BuildKDTree();
|
||
64 | void ComputeMatrices();
|
||
65 | |||
66 | void SavePoints(const char *filename); |
||
67 | void SaveMatrices(const char *filename); |
||
68 | |||
69 | int NumPoints() { return (int)point_.size(); } |
||
70 | void Clear(void); |
||
71 | int Size() { return point_.size(); } |
||
72 | inline void AppendPoint(GICPPoint const & pt) { point_.push_back(pt); } |
||
73 | void SetMaxIteration(int iter) { max_iteration_ = iter; } |
||
74 | void SetMaxIterationInner(int iter) { max_iteration_inner_ = iter; } |
||
75 | void SetEpsilon(double eps) { epsilon_ = eps; } |
||
76 | void SetSolveRotation(bool s) { solve_rotation_ = s; } |
||
77 | void SetGICPEpsilon(double eps) { gicp_epsilon_ = eps; } |
||
78 | void SetDebug(bool d) { debug_ = d; } |
||
79 | |||
80 | |||
81 | GICPPoint & operator[](int i) { return point_[i]; } |
||
82 | GICPPoint const& operator[](int i) const { return point_[i]; } |
||
83 | |||
84 | // returns number of iterations it took to converge
|
||
85 | int AlignScan(GICPPointSet *scan, dgc_transform_t base_t, dgc_transform_t t, double max_match_dist, bool save_error_plot = 0); |
||
86 | |||
87 | private:
|
||
88 | std::vector <GICPPoint> point_; |
||
89 | ANNpointArray kdtree_points_; |
||
90 | ANNkd_tree *kdtree_; |
||
91 | int max_iteration_;
|
||
92 | int max_iteration_inner_;
|
||
93 | double epsilon_;
|
||
94 | double epsilon_rot_;
|
||
95 | double gicp_epsilon_;
|
||
96 | bool debug_;
|
||
97 | bool solve_rotation_;
|
||
98 | bool matrices_done_;
|
||
99 | bool kdtree_done_;
|
||
100 | pthread_mutex_t mutex_; |
||
101 | }; |
||
102 | |||
103 | } |
||
104 | |||
105 | } |
||
106 | |||
107 | #endif |