librpp.cpp
Go to the documentation of this file.
1 /* ========================================================================
2  * PROJECT: ARToolKitPlus
3  * ========================================================================
4  *
5  * The robust pose estimator algorithm has been provided by G. Schweighofer
6  * and A. Pinz (Inst.of El.Measurement and Measurement Signal Processing,
7  * Graz University of Technology). Details about the algorithm are given in
8  * a Technical Report: TR-EMT-2005-01, available at:
9  * http://www.emt.tu-graz.ac.at/publications/index.htm
10  *
11  * Ported from MATLAB to C by T.Pintaric (Vienna University of Technology).
12  *
13  * Copyright of the derived and new portions of this work
14  * (C) 2006 Graz University of Technology
15  *
16  * This framework is free software; you can redistribute it and/or modify
17  * it under the terms of the GNU General Public License as published by
18  * the Free Software Foundation; either version 2 of the License, or
19  * (at your option) any later version.
20  *
21  * This framework is distributed in the hope that it will be useful,
22  * but WITHOUT ANY WARRANTY; without even the implied warranty of
23  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
24  * GNU General Public License for more details.
25  *
26  * You should have received a copy of the GNU General Public License
27  * along with this framework; if not, write to the Free Software
28  * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
29  *
30  * For further information please contact
31  * Dieter Schmalstieg
32  * <schmalstieg@icg.tu-graz.ac.at>
33  * Graz University of Technology,
34  * Institut for Computer Graphics and Vision,
35  * Inffeldgasse 16a, 8010 Graz, Austria.
36  * ========================================================================
37  ** @author Thomas Pintaric
38  *
39  * $Id: librpp.cpp 162 2006-04-19 21:28:10Z grabner $
40  * @file
41  * ======================================================================== */
42 
43 
44 #include "librpp.h"
45 #include "rpp.h"
46 #include "rpp_vecmat.h"
47 using namespace rpp;
48 
49 #ifdef LIBRPP_DLL
50 BOOL APIENTRY DllMain( HANDLE hModule,
51  DWORD ul_reason_for_call,
52  LPVOID lpReserved
53  )
54 {
55  switch (ul_reason_for_call)
56  {
57  case DLL_PROCESS_ATTACH:
58  case DLL_THREAD_ATTACH:
59  case DLL_THREAD_DETACH:
60  case DLL_PROCESS_DETACH:
61  break;
62  }
63  return TRUE;
64 }
65 #endif //LIBRPP_DLL
66 
67 
68 #ifndef _NO_LIBRPP_
69 
70 
72  rpp_mat &R,
73  rpp_vec &t,
74  const rpp_float cc[2],
75  const rpp_float fc[2],
76  const rpp_vec *model,
77  const rpp_vec *iprts,
78  const unsigned int model_iprts_size,
79  const rpp_mat R_init,
80  const bool estimate_R_init,
81  const rpp_float epsilon,
82  const rpp_float tolerance,
83  const unsigned int max_iterations)
84 {
85  vec3_array _model;
86  vec3_array _iprts;
87  _model.resize(model_iprts_size);
88  _iprts.resize(model_iprts_size);
89 
90  mat33_t K, K_inv;
91  mat33_eye(K);
92  K.m[0][0] = (real_t)fc[0];
93  K.m[1][1] = (real_t)fc[1];
94  K.m[0][2] = (real_t)cc[0];
95  K.m[1][2] = (real_t)cc[1];
96 
97  mat33_inv(K_inv, K);
98 
99  for(unsigned int i=0; i<model_iprts_size; i++)
100  {
101  vec3_t _v,_v2;
102  vec3_assign(_v,(real_t)model[i][0],(real_t)model[i][1],(real_t)model[i][2]);
103  _model[i] = _v;
104  vec3_assign(_v,(real_t)iprts[i][0],(real_t)iprts[i][1],(real_t)iprts[i][2]);
105  vec3_mult(_v2,K_inv,_v);
106  _iprts[i] = _v2;
107  }
108 
110  options.max_iter = max_iterations;
111  options.epsilon = (real_t)(epsilon == 0 ? DEFAULT_EPSILON : epsilon);
112  options.tol = (real_t)(tolerance == 0 ? DEFAULT_TOL : tolerance);
113  if(estimate_R_init)
114  mat33_set_all_zeros(options.initR);
115  else
116  {
117  mat33_assign(options.initR,
118  (real_t)R_init[0][0], (real_t)R_init[0][1], (real_t)R_init[0][2],
119  (real_t)R_init[1][0], (real_t)R_init[1][1], (real_t)R_init[1][2],
120  (real_t)R_init[2][0], (real_t)R_init[2][1], (real_t)R_init[2][2]);
121  }
122 
123  real_t _err;
124  mat33_t _R;
125  vec3_t _t;
126 
127  robust_pose(_err,_R,_t,_model,_iprts,options);
128 
129  for(int j=0; j<3; j++)
130  {
131  R[j][0] = (rpp_float)_R.m[j][0];
132  R[j][1] = (rpp_float)_R.m[j][1];
133  R[j][2] = (rpp_float)_R.m[j][2];
134  t[j] = (rpp_float)_t.v[j];
135  }
136  err = (rpp_float)_err;
137 }
138 
139 
141 {
142  return true;
143 }
144 
145 
146 #else //_NO_LIBRPP_
147 
148 
150  rpp_mat &R,
151  rpp_vec &t,
152  const rpp_float cc[2],
153  const rpp_float fc[2],
154  const rpp_vec *model,
155  const rpp_vec *iprts,
156  const unsigned int model_iprts_size,
157  const rpp_mat R_init,
158  const bool estimate_R_init,
159  const rpp_float epsilon,
160  const rpp_float tolerance,
161  const unsigned int max_iterations)
162 {
163 }
164 
165 
166 bool rppSupportAvailabe()
167 {
168  return false;
169 }
170 
171 #endif //_NO_LIBRPP_
real_t epsilon
Definition: rpp_types.h:107
double epsilon
void mat33_eye(mat33_t &m)
Definition: rpp_vecmat.cpp:560
double rpp_mat[3][3]
Definition: rpp.cpp:55
LIBRPP_API void robustPlanarPose(rpp_float &err, rpp_mat &R, rpp_vec &t, const rpp_float cc[2], const rpp_float fc[2], const rpp_vec *model, const rpp_vec *iprts, const unsigned int model_iprts_size, const rpp_mat R_init, const bool estimate_R_init, const rpp_float epsilon, const rpp_float tolerance, const unsigned int max_iterations)
Definition: librpp.cpp:71
std::vector< vec3_t > vec3_array
Definition: rpp_types.h:71
options
void mat33_assign(mat33_t &m, const real_t m00, const real_t m01, const real_t m02, const real_t m10, const real_t m11, const real_t m12, const real_t m20, const real_t m21, const real_t m22)
Definition: rpp_vecmat.cpp:184
#define LIBRPP_API
Definition: librpp.h:59
#define DEFAULT_TOL
Definition: rpp_const.h:63
double rpp_vec[3]
#define DEFAULT_EPSILON
Definition: rpp_const.h:64
bool rppSupportAvailabe()
Definition: librpp.cpp:140
void mat33_set_all_zeros(mat33_t &m)
Definition: rpp_vecmat.cpp:595
double real_t
Definition: rpp_types.h:64
void mat33_inv(mat33_t &mi, const mat33_t &ma)
Definition: rpp_vecmat.cpp:666
unsigned int max_iter
Definition: rpp_types.h:108
mat33_t initR
Definition: rpp_types.h:105
void robust_pose(real_t &err, mat33_t &R, vec3_t &t, const vec3_array &_model, const vec3_array &_iprts, const options_t _options)
Definition: rpp.cpp:741
void vec3_assign(vec3_t &v, const real_t x, const real_t y, const real_t z)
Definition: rpp_vecmat.cpp:297
void vec3_mult(vec3_t &va, const real_t n)
Definition: rpp_vecmat.cpp:368


tuw_artoolkitplus
Author(s): Markus Bader
autogenerated on Sun Sep 4 2016 03:24:33