root / rgbdslam / gicp / optimize.h @ master
History | View | Annotate | Download (3.73 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 |
#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 |
|
115 |
|