librpp.cpp
Go to the documentation of this file.
00001 /* ========================================================================
00002  * PROJECT: ARToolKitPlus
00003  * ========================================================================
00004  *
00005  * The robust pose estimator algorithm has been provided by G. Schweighofer
00006  * and A. Pinz (Inst.of El.Measurement and Measurement Signal Processing,
00007  * Graz University of Technology). Details about the algorithm are given in
00008  * a Technical Report: TR-EMT-2005-01, available at:
00009  * http://www.emt.tu-graz.ac.at/publications/index.htm
00010  *
00011  * Ported from MATLAB to C by T.Pintaric (Vienna University of Technology).
00012  *
00013  * Copyright of the derived and new portions of this work
00014  *     (C) 2006 Graz University of Technology
00015  *
00016  * This framework is free software; you can redistribute it and/or modify
00017  * it under the terms of the GNU General Public License as published by
00018  * the Free Software Foundation; either version 2 of the License, or
00019  * (at your option) any later version.
00020  *
00021  * This framework is distributed in the hope that it will be useful,
00022  * but WITHOUT ANY WARRANTY; without even the implied warranty of
00023  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00024  * GNU General Public License for more details.
00025  *
00026  * You should have received a copy of the GNU General Public License
00027  * along with this framework; if not, write to the Free Software
00028  * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
00029  *
00030  * For further information please contact 
00031  *   Dieter Schmalstieg
00032  *   <schmalstieg@icg.tu-graz.ac.at>
00033  *   Graz University of Technology, 
00034  *   Institut for Computer Graphics and Vision,
00035  *   Inffeldgasse 16a, 8010 Graz, Austria.
00036  * ========================================================================
00037  ** @author   Thomas Pintaric
00038  *
00039  * $Id: librpp.cpp 162 2006-04-19 21:28:10Z grabner $
00040  * @file
00041  * ======================================================================== */
00042 
00043 
00044 #include "rpp/librpp.h"
00045 #include "rpp/rpp.h"
00046 #include "rpp/rpp_vecmat.h"
00047 using namespace rpp;
00048 
00049 #ifdef LIBRPP_DLL
00050 BOOL APIENTRY DllMain( HANDLE hModule, 
00051                                         DWORD  ul_reason_for_call, 
00052                                         LPVOID lpReserved
00053                                         )
00054 {
00055         switch (ul_reason_for_call)
00056         {
00057         case DLL_PROCESS_ATTACH:
00058         case DLL_THREAD_ATTACH:
00059         case DLL_THREAD_DETACH:
00060         case DLL_PROCESS_DETACH:
00061                 break;
00062         }
00063         return TRUE;
00064 }
00065 #endif //LIBRPP_DLL
00066 
00067 
00068 #ifndef _NO_LIBRPP_
00069 
00070 
00071 LIBRPP_API void robustPlanarPose(rpp_float &err,
00072                                                                  rpp_mat &R,
00073                                                                  rpp_vec &t,
00074                                                                  const rpp_float cc[2],
00075                                                                  const rpp_float fc[2],
00076                                                                  const rpp_vec *model,
00077                                                                  const rpp_vec *iprts,
00078                                                                  const unsigned int model_iprts_size,
00079                                                                  const rpp_mat R_init,
00080                                                                  const bool estimate_R_init,
00081                                                                  const rpp_float epsilon,
00082                                                                  const rpp_float tolerance,
00083                                                                  const unsigned int max_iterations)
00084 {
00085         vec3_array _model;
00086         vec3_array _iprts;
00087         _model.resize(model_iprts_size);
00088         _iprts.resize(model_iprts_size);
00089 
00090         mat33_t K, K_inv;
00091         mat33_eye(K);
00092         K.m[0][0] = (real_t)fc[0];
00093         K.m[1][1] = (real_t)fc[1];
00094         K.m[0][2] = (real_t)cc[0];
00095         K.m[1][2] = (real_t)cc[1];
00096 
00097         mat33_inv(K_inv, K);
00098 
00099         for(unsigned int i=0; i<model_iprts_size; i++)
00100         {
00101                 vec3_t _v,_v2;
00102                 vec3_assign(_v,(real_t)model[i][0],(real_t)model[i][1],(real_t)model[i][2]);
00103                 _model[i] = _v;
00104                 vec3_assign(_v,(real_t)iprts[i][0],(real_t)iprts[i][1],(real_t)iprts[i][2]);
00105                 vec3_mult(_v2,K_inv,_v);
00106                 _iprts[i] = _v2;
00107         }
00108 
00109         options_t options;
00110         options.max_iter = max_iterations;
00111         options.epsilon = (real_t)(epsilon == 0 ? DEFAULT_EPSILON : epsilon);
00112         options.tol =     (real_t)(tolerance == 0 ? DEFAULT_TOL : tolerance);
00113         if(estimate_R_init)
00114                 mat33_set_all_zeros(options.initR);
00115         else
00116         {
00117                 mat33_assign(options.initR,
00118                                         (real_t)R_init[0][0], (real_t)R_init[0][1], (real_t)R_init[0][2],
00119                                         (real_t)R_init[1][0], (real_t)R_init[1][1], (real_t)R_init[1][2],
00120                                         (real_t)R_init[2][0], (real_t)R_init[2][1], (real_t)R_init[2][2]);
00121         }
00122 
00123         real_t _err;
00124         mat33_t _R;
00125         vec3_t _t;
00126 
00127         robust_pose(_err,_R,_t,_model,_iprts,options);
00128 
00129         for(int j=0; j<3; j++)
00130         {
00131                 R[j][0] = (rpp_float)_R.m[j][0];
00132                 R[j][1] = (rpp_float)_R.m[j][1];
00133                 R[j][2] = (rpp_float)_R.m[j][2];
00134                 t[j] = (rpp_float)_t.v[j];
00135         }
00136         err = (rpp_float)_err;
00137 }
00138 
00139 
00140 bool rppSupportAvailabe()
00141 {
00142         return true;
00143 }
00144 
00145 
00146 #else //_NO_LIBRPP_
00147 
00148 
00149 LIBRPP_API void robustPlanarPose(rpp_float &err,
00150                                                                  rpp_mat &R,
00151                                                                  rpp_vec &t,
00152                                                                  const rpp_float cc[2],
00153                                                                  const rpp_float fc[2],
00154                                                                  const rpp_vec *model,
00155                                                                  const rpp_vec *iprts,
00156                                                                  const unsigned int model_iprts_size,
00157                                                                  const rpp_mat R_init,
00158                                                                  const bool estimate_R_init,
00159                                                                  const rpp_float epsilon,
00160                                                                  const rpp_float tolerance,
00161                                                                  const unsigned int max_iterations)
00162 {
00163 }
00164 
00165 
00166 bool rppSupportAvailabe()
00167 {
00168         return false;
00169 }
00170 
00171 #endif //_NO_LIBRPP_
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Properties Friends Defines


aruco_pose
Author(s): Julian Brunner
autogenerated on Thu May 23 2013 12:15:46