robust_pose.cpp

Go to the documentation of this file.

// Author: Thomas Pintaric <pintaric@ims.tuwien.ac.at>

#include <vector>
#include "../rpp.h"
#include "../rpp_types.h"
#include "../rpp_vecmat.h"
#include "mex.h"

using namespace rpp;

void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
{
    const mxArray *model_p, *iprts_p;
    double *model_v, *iprts_v, *R_v, *t_v, *e_v;
    
    model_p = prhs[0]; 
    iprts_p = prhs[1]; 
    model_v = mxGetPr(model_p);
    iprts_v = mxGetPr(iprts_p);

        if(mxGetN(model_p) != mxGetN(iprts_p))
        {
        mexErrMsgTxt("size(model) ~= size(iprts)");        
        return;
        }
        if((mxGetM(model_p) != 3) || (mxGetM(iprts_p) != 3))
        {
        mexErrMsgTxt("[model] and [iprts] must be 3-by-N matrices");        
        return;
        }
    

        unsigned int n = mxGetN(model_p);
        vec3_array model;
        vec3_array iprts;

        model.resize(n);
        iprts.resize(n);

        unsigned int j=0;
        for(unsigned int i=0; i<n; i++)
        {
                vec3_t _vm,_vi;
                vec3_assign(_vm,(real_t)model_v[j],(real_t)model_v[j+1],(real_t)model_v[j+2]);
                vec3_assign(_vi,(real_t)iprts_v[j],(real_t)iprts_v[j+1],(real_t)iprts_v[j+2]);
                j += 3;
                model[i] = _vm;
                iprts[i] = _vi;
        }

        options_t options;
    options.epsilon = (float)DEFAULT_EPSILON;
    options.tol =     (float)DEFAULT_TOL;
        mat33_set_all_zeros(options.initR);

        real_t  err;
        vec3_t  t;
        mat33_t R;    
    
        robust_pose(err,R,t,model,iprts,options);
   
    plhs[0] = mxCreateDoubleMatrix(3, 3, mxREAL);
    R_v = mxGetPr(plhs[0]);

        R_v[0] = R.m[0][0];     R_v[1] = R.m[1][0];     R_v[2] = R.m[2][0];
        R_v[3] = R.m[0][1];     R_v[4] = R.m[1][1];     R_v[5] = R.m[2][1];
        R_v[6] = R.m[0][2];     R_v[7] = R.m[1][2];     R_v[8] = R.m[2][2];

    plhs[1] = mxCreateDoubleMatrix(3, 1, mxREAL);
    t_v = mxGetPr(plhs[1]);

        t_v[0] = t.v[0];
        t_v[1] = t.v[1];
        t_v[2] = t.v[2];

    plhs[2] = mxCreateDoubleMatrix(1, 1, mxREAL);
    e_v = mxGetPr(plhs[2]);
        e_v[0] = (double)err;

    return;
}