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
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
|
static const char *copyright =
" Copyright (c) 2013 Laurent Kneip, ANU. All rights reserved.";
/******************************************************************************
* Author: Laurent Kneip *
* Contact: kneip.laurent@gmail.com *
* License: Copyright (c) 2013 Laurent Kneip, ANU. All rights reserved. *
* *
* Redistribution and use in source and binary forms, with or without *
* modification, are permitted provided that the following conditions *
* are met: *
* * Redistributions of source code must retain the above copyright *
* notice, this list of conditions and the following disclaimer. *
* * Redistributions in binary form must reproduce the above copyright *
* notice, this list of conditions and the following disclaimer in the *
* documentation and/or other materials provided with the distribution. *
* * Neither the name of ANU nor the names of its contributors may be *
* used to endorse or promote products derived from this software without *
* specific prior written permission. *
* *
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"*
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE *
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE *
* ARE DISCLAIMED. IN NO EVENT SHALL ANU OR THE CONTRIBUTORS BE LIABLE *
* FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL *
* DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR *
* SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER *
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT *
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY *
* OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF *
* SUCH DAMAGE. *
******************************************************************************/
// Matlab usage:
//
// X = opengv_experimental2( data1, data2, algorithm )
//
// where
// data1, data2 are matched points of dimension 6xn
// algorithm is 0 for sixpt, 1 for ge, and 2 for seventeenpt
// X is a 3x5 matrix returning the found transformation, plus the number of
// Ransac-iterations and inliers
//
//matlab header
//standard headers
#include <stdlib.h>
#include <stdio.h>
#include <vector>
#include "mex.h"
//include generic headers for opengv stuff
#include <opengv/types.hpp>
//include the matlab-adapters
#include <opengv/relative_pose/MANoncentralRelative.hpp>
//expose all ransac-facilities to matlab
#include <opengv/sac/Ransac.hpp>
#include <opengv/sac_problems/relative_pose/NoncentralRelativePoseSacProblem.hpp>
typedef opengv::sac_problems::relative_pose::NoncentralRelativePoseSacProblem nrelRansac;
typedef std::shared_ptr<nrelRansac> nrelRansacPtr;
// The main mex-function
void mexFunction( int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[] )
{
// Characterize the type of the call
const mxArray *data1 = prhs[0];
const mxArray *data2 = prhs[1];
const mxArray *temp1 = prhs[2];
double *temp2 = (double*) mxGetData(temp1);
int algorithm = floor(temp2[0]+0.01);
const mwSize *data1dim = mxGetDimensions(data1);
const mwSize *data2dim = mxGetDimensions(data2);
//create three pointers to absolute, relative, and point_cloud adapters here
opengv::relative_pose::RelativeAdapterBase* relativeAdapter =
new opengv::relative_pose::MANoncentralRelative(
(double*) mxGetData(data1),
(double*) mxGetData(data2),
data1dim[1],
data2dim[1] );
nrelRansacPtr problem;
switch(algorithm)
{
case 0:
problem = nrelRansacPtr( new nrelRansac( *relativeAdapter, nrelRansac::SIXPT ) );
break;
case 1:
problem = nrelRansacPtr( new nrelRansac( *relativeAdapter, nrelRansac::GE ) );
break;
case 2:
problem = nrelRansacPtr( new nrelRansac( *relativeAdapter, nrelRansac::SEVENTEENPT ) );
break;
}
opengv::sac::Ransac<nrelRansac> ransac;
ransac.sac_model_ = problem;
ransac.threshold_ = 2.0*(1.0 - cos(atan(sqrt(2.0)*0.5/800.0)));
ransac.max_iterations_ = 10000000;
ransac.computeModel();
Eigen::Matrix<double,3,5> result;
result.block<3,4>(0,0) = ransac.model_coefficients_;
result(0,4) = ransac.iterations_;
result(1,4) = ransac.inliers_.size();
int dims[2];
dims[0] = 3;
dims[1] = 5;
plhs[0] = mxCreateNumericArray(2, dims, mxDOUBLE_CLASS, mxREAL);
memcpy(mxGetData(plhs[0]), result.data(), 15*sizeof(double));
}
|
