root / rgbdslam / gicp / gicp.h @ 9240aaa3
History | View | Annotate | Download (3.46 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 |
#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
|