root / rgbdslam / gicp / optimize.h @ 9240aaa3
History | View | Annotate | Download (3.73 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 | #ifndef OPTIMIZE_H_
|
||
39 | #define OPTIMIZE_H_
|
||
40 | |||
41 | #include <ANN/ANN.h> |
||
42 | #include "gicp.h" |
||
43 | #include <vector> |
||
44 | #include <gsl/gsl_linalg.h> |
||
45 | #include <gsl/gsl_blas.h> |
||
46 | #include <gsl/gsl_multimin.h> |
||
47 | #include <gsl/gsl_multifit_nlin.h> |
||
48 | |||
49 | namespace dgc { |
||
50 | namespace gicp { |
||
51 | inline void print_gsl_matrix(gsl_matrix *mat, const char * name) { |
||
52 | std::cout << name << "= [";
|
||
53 | for(unsigned int i = 0; i < mat->size1; i++) { |
||
54 | for(unsigned int j = 0; j < mat->size2; j++) { |
||
55 | std::cout << gsl_matrix_get(mat, i, j) << " ";
|
||
56 | } |
||
57 | std::cout << ";" << std::endl;
|
||
58 | } |
||
59 | std::cout << "]" << std::endl;
|
||
60 | } |
||
61 | |||
62 | struct GICPOptData {
|
||
63 | GICPPointSet *p1; |
||
64 | GICPPointSet *p2; |
||
65 | ANNidx *nn_indecies; // nearest point indecies
|
||
66 | gicp_mat_t *M; // mahalanobis matrices for each pair
|
||
67 | dgc_transform_t base_t; |
||
68 | int num_matches;
|
||
69 | bool solve_rotation;
|
||
70 | }; |
||
71 | |||
72 | class GICPOptimizer { |
||
73 | public:
|
||
74 | GICPOptimizer(); |
||
75 | ~GICPOptimizer(); |
||
76 | |||
77 | int Iterations() { return iter; } |
||
78 | const char* Status() { return gsl_strerror(status); } |
||
79 | |||
80 | bool Optimize(dgc_transform_t t, GICPOptData &opt_data);
|
||
81 | bool OptimizeLM(dgc_transform_t t, GICPOptData &opt_data);
|
||
82 | |||
83 | void SetDebug(bool d) { debug = d; } |
||
84 | |||
85 | void SetMaxIterations(int iter) { max_iter = iter; } |
||
86 | |||
87 | void PlotError(dgc_transform_t t, GICPOptData &opt_data, const char* filename); |
||
88 | |||
89 | private:
|
||
90 | static double f(const gsl_vector * x, void * params); |
||
91 | static void df(const gsl_vector * x, void * params, gsl_vector * g); |
||
92 | static void fdf(const gsl_vector * x, void * params, double * f, gsl_vector * g); |
||
93 | |||
94 | static void compute_dr(gsl_vector const* x, gsl_matrix const* gsl_temp_mat_r, gsl_vector *g); |
||
95 | static double mat_inner_prod(gsl_matrix const* mat1, gsl_matrix const* mat2); |
||
96 | static void apply_state(dgc_transform_t t, gsl_vector const* x); |
||
97 | |||
98 | gsl_multimin_fdfminimizer *gsl_minimizer; |
||
99 | gsl_vector *x; |
||
100 | int max_iter;
|
||
101 | int iter;
|
||
102 | int status;
|
||
103 | bool debug;
|
||
104 | const static int N = 6; |
||
105 | const gsl_multimin_fdfminimizer_type *T_min;
|
||
106 | }; |
||
107 | |||
108 | } |
||
109 | } |
||
110 | |||
111 | |||
112 | #endif
|
||
113 | |||
114 |