Go to the documentation of this file.00001
00002
00003 #include <vector>
00004 #include "../rpp.h"
00005 #include "../rpp_types.h"
00006 #include "../rpp_vecmat.h"
00007 #include "mex.h"
00008
00009 using namespace rpp;
00010
00011 void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
00012 {
00013 const mxArray *model_p, *iprts_p;
00014 double *model_v, *iprts_v, *R_v, *t_v, *e_v;
00015
00016 model_p = prhs[0];
00017 iprts_p = prhs[1];
00018 model_v = mxGetPr(model_p);
00019 iprts_v = mxGetPr(iprts_p);
00020
00021 if(mxGetN(model_p) != mxGetN(iprts_p))
00022 {
00023 mexErrMsgTxt("size(model) ~= size(iprts)");
00024 return;
00025 }
00026 if((mxGetM(model_p) != 3) || (mxGetM(iprts_p) != 3))
00027 {
00028 mexErrMsgTxt("[model] and [iprts] must be 3-by-N matrices");
00029 return;
00030 }
00031
00032
00033 unsigned int n = mxGetN(model_p);
00034 vec3_array model;
00035 vec3_array iprts;
00036
00037 model.resize(n);
00038 iprts.resize(n);
00039
00040 unsigned int j=0;
00041 for(unsigned int i=0; i<n; i++)
00042 {
00043 vec3_t _vm,_vi;
00044 vec3_assign(_vm,(real_t)model_v[j],(real_t)model_v[j+1],(real_t)model_v[j+2]);
00045 vec3_assign(_vi,(real_t)iprts_v[j],(real_t)iprts_v[j+1],(real_t)iprts_v[j+2]);
00046 j += 3;
00047 model[i] = _vm;
00048 iprts[i] = _vi;
00049 }
00050
00051 options_t options;
00052 options.epsilon = (float)DEFAULT_EPSILON;
00053 options.tol = (float)DEFAULT_TOL;
00054 mat33_set_all_zeros(options.initR);
00055
00056 real_t err;
00057 vec3_t t;
00058 mat33_t R;
00059
00060 robust_pose(err,R,t,model,iprts,options);
00061
00062 plhs[0] = mxCreateDoubleMatrix(3, 3, mxREAL);
00063 R_v = mxGetPr(plhs[0]);
00064
00065 R_v[0] = R.m[0][0]; R_v[1] = R.m[1][0]; R_v[2] = R.m[2][0];
00066 R_v[3] = R.m[0][1]; R_v[4] = R.m[1][1]; R_v[5] = R.m[2][1];
00067 R_v[6] = R.m[0][2]; R_v[7] = R.m[1][2]; R_v[8] = R.m[2][2];
00068
00069 plhs[1] = mxCreateDoubleMatrix(3, 1, mxREAL);
00070 t_v = mxGetPr(plhs[1]);
00071
00072 t_v[0] = t.v[0];
00073 t_v[1] = t.v[1];
00074 t_v[2] = t.v[2];
00075
00076 plhs[2] = mxCreateDoubleMatrix(1, 1, mxREAL);
00077 e_v = mxGetPr(plhs[2]);
00078 e_v[0] = (double)err;
00079
00080 return;
00081 }