librpp.cpp

Go to the documentation of this file.

/* ========================================================================
 * PROJECT: ARToolKitPlus
 * ========================================================================
 *
 * The robust pose estimator algorithm has been provided by G. Schweighofer
 * and A. Pinz (Inst.of El.Measurement and Measurement Signal Processing,
 * Graz University of Technology). Details about the algorithm are given in
 * a Technical Report: TR-EMT-2005-01, available at:
 * http://www.emt.tu-graz.ac.at/publications/index.htm
 *
 * Ported from MATLAB to C by T.Pintaric (Vienna University of Technology).
 *
 * Copyright of the derived and new portions of this work
 *     (C) 2006 Graz University of Technology
 *
 * This framework is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 2 of the License, or
 * (at your option) any later version.
 *
 * This framework is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this framework; if not, write to the Free Software
 * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
 *
 * For further information please contact 
 *   Dieter Schmalstieg
 *   <schmalstieg@icg.tu-graz.ac.at>
 *   Graz University of Technology, 
 *   Institut for Computer Graphics and Vision,
 *   Inffeldgasse 16a, 8010 Graz, Austria.
 * ========================================================================
 ** @author   Thomas Pintaric
 *
 * $Id: librpp.cpp 162 2006-04-19 21:28:10Z grabner $
 * @file
 * ======================================================================== */


#include "librpp.h"
#include "rpp.h"
#include "rpp_vecmat.h"
using namespace rpp;

#ifdef LIBRPP_DLL
BOOL APIENTRY DllMain( HANDLE hModule, 
                                        DWORD  ul_reason_for_call, 
                                        LPVOID lpReserved
                                        )
{
        switch (ul_reason_for_call)
        {
        case DLL_PROCESS_ATTACH:
        case DLL_THREAD_ATTACH:
        case DLL_THREAD_DETACH:
        case DLL_PROCESS_DETACH:
                break;
        }
        return TRUE;
}
#endif //LIBRPP_DLL


#ifndef _NO_LIBRPP_


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)
{
        vec3_array _model;
        vec3_array _iprts;
        _model.resize(model_iprts_size);
        _iprts.resize(model_iprts_size);

        mat33_t K, K_inv;
        mat33_eye(K);
        K.m[0][0] = (real_t)fc[0];
        K.m[1][1] = (real_t)fc[1];
        K.m[0][2] = (real_t)cc[0];
        K.m[1][2] = (real_t)cc[1];

        mat33_inv(K_inv, K);

        for(unsigned int i=0; i<model_iprts_size; i++)
        {
                vec3_t _v,_v2;
                vec3_assign(_v,(real_t)model[i][0],(real_t)model[i][1],(real_t)model[i][2]);
                _model[i] = _v;
                vec3_assign(_v,(real_t)iprts[i][0],(real_t)iprts[i][1],(real_t)iprts[i][2]);
                vec3_mult(_v2,K_inv,_v);
                _iprts[i] = _v2;
        }

        options_t options;
        options.max_iter = max_iterations;
        options.epsilon = (real_t)(epsilon == 0 ? DEFAULT_EPSILON : epsilon);
        options.tol =     (real_t)(tolerance == 0 ? DEFAULT_TOL : tolerance);
        if(estimate_R_init)
                mat33_set_all_zeros(options.initR);
        else
        {
                mat33_assign(options.initR,
                                        (real_t)R_init[0][0], (real_t)R_init[0][1], (real_t)R_init[0][2],
                                        (real_t)R_init[1][0], (real_t)R_init[1][1], (real_t)R_init[1][2],
                                        (real_t)R_init[2][0], (real_t)R_init[2][1], (real_t)R_init[2][2]);
        }

        real_t _err;
        mat33_t _R;
        vec3_t _t;

        robust_pose(_err,_R,_t,_model,_iprts,options);

        for(int j=0; j<3; j++)
        {
                R[j][0] = (rpp_float)_R.m[j][0];
                R[j][1] = (rpp_float)_R.m[j][1];
                R[j][2] = (rpp_float)_R.m[j][2];
                t[j] = (rpp_float)_t.v[j];
        }
        err = (rpp_float)_err;
}


bool rppSupportAvailabe()
{
        return true;
}


#else //_NO_LIBRPP_


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)
{
}


bool rppSupportAvailabe()
{
        return false;
}

#endif //_NO_LIBRPP_