1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
|
// 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;
}
|
