matcherMex.cpp
Go to the documentation of this file.
00001 /*
00002 Copyright 2012. All rights reserved.
00003 Institute of Measurement and Control Systems
00004 Karlsruhe Institute of Technology, Germany
00005 
00006 This file is part of libviso2.
00007 Authors: Andreas Geiger
00008 
00009 libviso2 is free software; you can redistribute it and/or modify it under the
00010 terms of the GNU General Public License as published by the Free Software
00011 Foundation; either version 2 of the License, or any later version.
00012 
00013 libviso2 is distributed in the hope that it will be useful, but WITHOUT ANY
00014 WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A
00015 PARTICULAR PURPOSE. See the GNU General Public License for more details.
00016 
00017 You should have received a copy of the GNU General Public License along with
00018 libviso2; if not, write to the Free Software Foundation, Inc., 51 Franklin
00019 Street, Fifth Floor, Boston, MA 02110-1301, USA 
00020 */
00021 
00022 #include "mex.h"
00023 #include <iostream>
00024 #include <string.h>
00025 #include "matcher.h"
00026 
00027 using namespace std;
00028 
00029 static Matcher *M;
00030 
00031 template<class T> T* transpose(T* I,const int32_t* dims) {
00032   T* I_ = (T*)malloc(dims[0]*dims[1]*sizeof(T));
00033   for (int32_t u=0; u<dims[1]; u++) {
00034     for (int32_t v=0; v<dims[0]; v++) {
00035       I_[v*dims[1]+u] = *I;
00036       I++;
00037     }
00038   }
00039   return I_;
00040 }
00041 
00042 void mexFunction (int nlhs,mxArray *plhs[],int nrhs,const mxArray *prhs[]) {
00043 
00044   // read command
00045   char command[128];
00046   mxGetString(prhs[0],command,128);
00047 
00048   // init
00049   if (!strcmp(command,"init")) {
00050     
00051     // check arguments
00052     if (nrhs!=1+1)
00053       mexErrMsgTxt("1 input required (param).");
00054     
00055     // check if we have a parameter structure
00056     if (!mxIsStruct(prhs[1]))
00057       mexErrMsgTxt("Input param must be a structure.");    
00058     
00059     // parameters
00060     Matcher::parameters param;
00061 
00062     // for all fields of parameter structure overwrite parameters
00063     for (int32_t i=0; i<mxGetNumberOfFields(prhs[1]); i++) {
00064       const char *field_name = mxGetFieldNameByNumber(prhs[1],i);
00065       mxArray    *field_val  = mxGetFieldByNumber(prhs[1],0,i);
00066 
00067       // value
00068       if (mxIsDouble(field_val)) {
00069         double val = *((double*)mxGetPr(field_val));
00070         if (!strcmp(field_name,"nms_n"))                  param.nms_n = val;
00071         if (!strcmp(field_name,"nms_tau"))                param.nms_tau = val;
00072         if (!strcmp(field_name,"match_binsize"))          param.match_binsize = val;
00073         if (!strcmp(field_name,"match_radius"))           param.match_radius = val;
00074         if (!strcmp(field_name,"match_disp_tolerance"))   param.match_disp_tolerance = val;
00075         if (!strcmp(field_name,"outlier_disp_tolerance")) param.outlier_disp_tolerance = val;
00076         if (!strcmp(field_name,"outlier_flow_tolerance")) param.outlier_flow_tolerance = val;
00077         if (!strcmp(field_name,"multi_stage"))            param.multi_stage = val;
00078         if (!strcmp(field_name,"half_resolution"))        param.half_resolution = val;
00079         if (!strcmp(field_name,"refinement"))             param.refinement = val;
00080       }
00081     }
00082     
00083     // create matcher instance
00084     M = new Matcher(param);
00085 
00086   // close
00087   } else if (!strcmp(command,"close")) {
00088     delete M;
00089     
00090   // push back stereo image pair
00091   } else if (!strcmp(command,"push") || !strcmp(command,"replace")) {
00092     
00093     // check arguments
00094     if (nrhs!=1+1 && nrhs!=1+2)
00095       mexErrMsgTxt("1 or 2 inputs required (I1=left image,I2=right image).");
00096     if (!mxIsUint8(prhs[1]) || mxGetNumberOfDimensions(prhs[1])!=2)
00097       mexErrMsgTxt("Input I1 (left image) must be a uint8_t matrix.");
00098     
00099     // get pointer to left image
00100     uint8_t* I1          = (uint8_t*)mxGetPr(prhs[1]);
00101     const int32_t *dims1 = mxGetDimensions(prhs[1]);
00102     
00103     // transpose
00104     uint8_t* I1_         = transpose<uint8_t>(I1,dims1);
00105     int32_t  dims1_[]    = {dims1[1],dims1[0],dims1[1]};
00106     
00107     // push back single image
00108     if (nrhs==1+1) {
00109       
00110       // compute features and put them to ring buffer
00111       M->pushBack(I1_,dims1_,!strcmp(command,"replace"));
00112     
00113     // push back stereo image pair
00114     } else {
00115       
00116       if (!mxIsUint8(prhs[2]) || mxGetNumberOfDimensions(prhs[2])!=2)
00117         mexErrMsgTxt("Input I2 (right image) must be a uint8_t matrix.");
00118       
00119       // get pointer to right image
00120       uint8_t* I2          = (uint8_t*)mxGetPr(prhs[2]);
00121       const int32_t *dims2 = mxGetDimensions(prhs[2]);
00122       
00123       // transpose
00124       uint8_t* I2_         = transpose<uint8_t>(I2,dims2);
00125       int32_t  dims2_[]    = {dims2[1],dims2[0],dims2[1]};
00126 
00127       // check image size
00128       if (dims1_[0]!=dims2_[0] || dims1_[1]!=dims2_[1])
00129         mexErrMsgTxt("Input I1 and I2 must be images of same size.");
00130 
00131       // compute features and put them to ring buffer
00132       M->pushBack(I1_,I2_,dims1_,!strcmp(command,"replace"));
00133       
00134       // free temporary memory
00135       free(I2_);
00136     }
00137     
00138     // free temporary memory
00139     free(I1_);
00140 
00141   // match features
00142   } else if (!strcmp(command,"match")) {
00143     
00144     // check arguments
00145     if (nrhs!=1+1 && nrhs!=1)
00146       mexErrMsgTxt("1 input required (method).");
00147     if (nlhs!=0)
00148       mexErrMsgTxt("No output required.");
00149     
00150     if (!mxIsDouble(prhs[1]) || mxGetM(prhs[1])*mxGetN(prhs[1])!=1)
00151         mexErrMsgTxt("Input method must be a double scalar.");
00152     
00153     // matching method: 0 = flow, 1 = stereo, 2 = quad matching
00154     int32_t method  =  (int32_t)*((double*)mxGetPr(prhs[1]));
00155     
00156     // do matching
00157     M->matchFeatures(method);
00158     
00159   // bucketing
00160   } else if (!strcmp(command,"bucketing")) {
00161     
00162     // check arguments
00163     if (nrhs!=1+3)
00164       mexErrMsgTxt("3 inputs required (max_features,bucket_width,bucket_height).");
00165     if (nlhs!=0)
00166       mexErrMsgTxt("No output required.");
00167     
00168     // remove closeby features via bucketing
00169     if (!mxIsDouble(prhs[1]) || mxGetM(prhs[1])*mxGetN(prhs[1])!=1)
00170       mexErrMsgTxt("Input max_features must be a double scalar.");
00171     if (!mxIsDouble(prhs[2]) || mxGetM(prhs[2])*mxGetN(prhs[2])!=1)
00172       mexErrMsgTxt("Input bucket_width must be a double scalar.");
00173     if (!mxIsDouble(prhs[3]) || mxGetM(prhs[3])*mxGetN(prhs[3])!=1)
00174       mexErrMsgTxt("Input bucket_height must be a double scalar.");
00175 
00176     // get pointers to input data
00177     int32_t max_features  =  (int32_t)*((double*)mxGetPr(prhs[1]));
00178     float   bucket_width  =           *((double*)mxGetPr(prhs[2]));
00179     float   bucket_height =           *((double*)mxGetPr(prhs[3]));
00180 
00181     // bucketing
00182     M->bucketFeatures(max_features,bucket_width,bucket_height);
00183     
00184   // get matches
00185   } else if (!strcmp(command,"get_matches")) {
00186     
00187     // check arguments
00188     if (nrhs!=1+1)
00189       mexErrMsgTxt("1 input required (method).");
00190     if (nlhs!=1)
00191       mexErrMsgTxt("One output required (p_matched).");
00192     
00193     if (!mxIsDouble(prhs[1]) || mxGetM(prhs[1])*mxGetN(prhs[1])!=1)
00194         mexErrMsgTxt("Input method must be a double scalar.");
00195     
00196     // matching method: 0 = flow, 1 = stereo, 2 = quad matching
00197     int32_t method  =  (int32_t)*((double*)mxGetPr(prhs[1]));
00198     
00199     // grab matches
00200     vector<Matcher::p_match> matches = M->getMatches();
00201     
00202     // method: flow
00203     if (method==0) {
00204       
00205       // create output matrix with matches
00206       const int32_t p_matched_dims[] = {4,matches.size()};
00207       plhs[0] = mxCreateNumericArray(2,p_matched_dims,mxDOUBLE_CLASS,mxREAL);
00208       double* p_matched_mex = (double*)mxGetPr(plhs[0]);
00209 
00210       // copy matches to mex array (convert indices: C++ => MATLAB)
00211       int32_t k=0;
00212       for (vector<Matcher::p_match>::iterator it=matches.begin(); it!=matches.end(); it++) {
00213         *(p_matched_mex+k++) = it->u1p+1;
00214         *(p_matched_mex+k++) = it->v1p+1;
00215         *(p_matched_mex+k++) = it->u1c+1;
00216         *(p_matched_mex+k++) = it->v1c+1;
00217       }
00218 
00219     // method: stereo
00220     } else if (method==1) {
00221       
00222       // create output matrix with matches
00223       const int32_t p_matched_dims[] = {4,matches.size()};
00224       plhs[0] = mxCreateNumericArray(2,p_matched_dims,mxDOUBLE_CLASS,mxREAL);
00225       double* p_matched_mex = (double*)mxGetPr(plhs[0]);
00226 
00227       // copy matches to mex array (convert indices: C++ => MATLAB)
00228       int32_t k=0;
00229       for (vector<Matcher::p_match>::iterator it=matches.begin(); it!=matches.end(); it++) {
00230         *(p_matched_mex+k++) = it->u1c+1;
00231         *(p_matched_mex+k++) = it->v1c+1;
00232         *(p_matched_mex+k++) = it->u2c+1;
00233         *(p_matched_mex+k++) = it->v2c+1;
00234       }
00235 
00236     // method: quad matching
00237     } else {
00238       
00239       // create output matrix with matches
00240       const int32_t p_matched_dims[] = {8,matches.size()};
00241       plhs[0] = mxCreateNumericArray(2,p_matched_dims,mxDOUBLE_CLASS,mxREAL);
00242       double* p_matched_mex = (double*)mxGetPr(plhs[0]);
00243 
00244       // copy matches to mex array (convert indices: C++ => MATLAB)
00245       int32_t k=0;
00246       for (vector<Matcher::p_match>::iterator it=matches.begin(); it!=matches.end(); it++) {
00247         *(p_matched_mex+k++) = it->u1p+1;
00248         *(p_matched_mex+k++) = it->v1p+1;
00249         *(p_matched_mex+k++) = it->u2p+1;
00250         *(p_matched_mex+k++) = it->v2p+1;
00251         *(p_matched_mex+k++) = it->u1c+1;
00252         *(p_matched_mex+k++) = it->v1c+1;
00253         *(p_matched_mex+k++) = it->u2c+1;
00254         *(p_matched_mex+k++) = it->v2c+1;
00255       }
00256     }
00257     
00258   // get matching indices
00259   } else if (!strcmp(command,"get_indices")) {
00260     
00261     // check arguments
00262     if (nrhs!=1+1)
00263       mexErrMsgTxt("1 input required (method).");
00264     if (nlhs!=1)
00265       mexErrMsgTxt("One output required (i_matched).");
00266     if (!mxIsDouble(prhs[1]) || mxGetM(prhs[1])*mxGetN(prhs[1])!=1)
00267         mexErrMsgTxt("Input method must be a double scalar.");
00268     
00269     // matching method: 0 = flow, 1 = stereo, 2 = quad matching
00270     int32_t method  =  (int32_t)*((double*)mxGetPr(prhs[1]));
00271     
00272     // grab matches
00273     vector<Matcher::p_match> matches = M->getMatches();
00274     
00275     // method: flow
00276     if (method==0) {
00277       
00278       // create output matrix with matches
00279       const int32_t i_matched_dims[] = {2,matches.size()};
00280       plhs[0] = mxCreateNumericArray(2,i_matched_dims,mxDOUBLE_CLASS,mxREAL);
00281       double* i_matched_mex = (double*)mxGetPr(plhs[0]);
00282 
00283       // copy matches to mex array
00284       int32_t k=0;
00285       for (vector<Matcher::p_match>::iterator it=matches.begin(); it!=matches.end(); it++) {
00286         *(i_matched_mex+k++) = it->i1p+1;
00287         *(i_matched_mex+k++) = it->i1c+1;
00288       }
00289 
00290     // method: stereo
00291     } else if (method==1) {
00292       
00293       // create output matrix with matches
00294       const int32_t i_matched_dims[] = {2,matches.size()};
00295       plhs[0] = mxCreateNumericArray(2,i_matched_dims,mxDOUBLE_CLASS,mxREAL);
00296       double* i_matched_mex = (double*)mxGetPr(plhs[0]);
00297 
00298       // copy matches to mex array
00299       int32_t k=0;
00300       for (vector<Matcher::p_match>::iterator it=matches.begin(); it!=matches.end(); it++) {
00301         *(i_matched_mex+k++) = it->i1c+1;
00302         *(i_matched_mex+k++) = it->i2c+1;
00303       }
00304 
00305     // method: quad matching
00306     } else {
00307       
00308       // create output matrix with matches
00309       const int32_t i_matched_dims[] = {4,matches.size()};
00310       plhs[0] = mxCreateNumericArray(2,i_matched_dims,mxDOUBLE_CLASS,mxREAL);
00311       double* i_matched_mex = (double*)mxGetPr(plhs[0]);
00312 
00313       // copy matches to mex array
00314       int32_t k=0;
00315       for (vector<Matcher::p_match>::iterator it=matches.begin(); it!=matches.end(); it++) {
00316         *(i_matched_mex+k++) = it->i1p+1;
00317         *(i_matched_mex+k++) = it->i2p+1;
00318         *(i_matched_mex+k++) = it->i1c+1;
00319         *(i_matched_mex+k++) = it->i2c+1;
00320       }
00321     }
00322 
00323   // unknown command
00324   } else {
00325     mexPrintf("Unknown command: %s\n",command);
00326   }
00327 }


libviso2
Author(s): Andreas Geiger, Stephan Wirth
autogenerated on Mon Oct 6 2014 08:40:54