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