visualOdometryStereoMex.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 "viso_stereo.h"
00024 
00025 #include <iostream>
00026 #include <string.h>
00027 
00028 using namespace std;
00029 
00030 static VisualOdometryStereo *viso;
00031 
00032 template<class T> T* transpose(T* I,const int32_t* dims) {
00033   T* I_ = (T*)malloc(dims[0]*dims[1]*sizeof(T));
00034   for (int32_t u=0; u<dims[1]; u++) {
00035     for (int32_t v=0; v<dims[0]; v++) {
00036       I_[v*dims[1]+u] = *I;
00037       I++;
00038     }
00039   }
00040   return I_;
00041 }
00042 
00043 void mexFunction (int nlhs,mxArray *plhs[],int nrhs,const mxArray *prhs[]) {
00044 
00045   // read command
00046   char command[128];
00047   mxGetString(prhs[0],command,128);
00048 
00049   // init
00050   if (!strcmp(command,"init")) {
00051     
00052     // check arguments
00053     if(nrhs!=1+1)
00054       mexErrMsgTxt("1 parameter required (param).");
00055 
00056     // check if we have a parameter structure
00057     if (!mxIsStruct(prhs[1]))
00058       mexErrMsgTxt("Input param must be a structure.");
00059     
00060     // parameters
00061     VisualOdometryStereo::parameters param;
00062 
00063     // for all fields of parameter structure overwrite parameters
00064     for (int32_t i=0; i<mxGetNumberOfFields(prhs[1]); i++) {
00065       const char *field_name = mxGetFieldNameByNumber(prhs[1],i);
00066       mxArray    *field_val  = mxGetFieldByNumber(prhs[1],0,i);
00067 
00068       // value
00069       if (mxIsDouble(field_val)) {
00070         double val = *((double*)mxGetPr(field_val));
00071         if (!strcmp(field_name,"nms_n"))                  param.match.nms_n = val;
00072         if (!strcmp(field_name,"nms_tau"))                param.match.nms_tau = val;
00073         if (!strcmp(field_name,"match_binsize"))          param.match.match_binsize = val;
00074         if (!strcmp(field_name,"match_radius"))           param.match.match_radius = val;
00075         if (!strcmp(field_name,"match_disp_tolerance"))   param.match.match_disp_tolerance = val;
00076         if (!strcmp(field_name,"outlier_disp_tolerance")) param.match.outlier_disp_tolerance = val;
00077         if (!strcmp(field_name,"outlier_flow_tolerance")) param.match.outlier_flow_tolerance = val;
00078         if (!strcmp(field_name,"multi_stage"))            param.match.multi_stage = val;
00079         if (!strcmp(field_name,"half_resolution"))        param.match.half_resolution = val;
00080         if (!strcmp(field_name,"refinement"))             param.match.refinement = val;
00081         if (!strcmp(field_name,"max_features"))           param.bucket.max_features = val;
00082         if (!strcmp(field_name,"bucket_width"))           param.bucket.bucket_width = val;
00083         if (!strcmp(field_name,"bucket_height"))          param.bucket.bucket_height = val;
00084         if (!strcmp(field_name,"f"))                      param.calib.f = val;
00085         if (!strcmp(field_name,"cu"))                     param.calib.cu = val;
00086         if (!strcmp(field_name,"cv"))                     param.calib.cv = val;
00087         if (!strcmp(field_name,"base"))                   param.base = val;
00088         if (!strcmp(field_name,"ransac_iters"))           param.ransac_iters = val;
00089         if (!strcmp(field_name,"inlier_threshold"))       param.inlier_threshold = val;
00090         if (!strcmp(field_name,"reweighting"))            param.reweighting = val;
00091       }
00092     }
00093     
00094     // create visual odometry instance
00095     viso = new VisualOdometryStereo(param);
00096     
00097   // process
00098   } else if (!strcmp(command,"process")) {
00099     
00100     // check for proper number of arguments
00101     if (nrhs!=2+1 && nrhs!=3+1)
00102       mexErrMsgTxt("2 or 3 inputs required: I1 (left image), I2 (right image), [replace].");
00103     if (nlhs!=1) 
00104       mexErrMsgTxt("1 outputs required: Tr (transformation from previous to current frame.");  
00105 
00106     // check for proper argument types and sizes
00107     if (!mxIsUint8(prhs[1]) || mxGetNumberOfDimensions(prhs[1])!=2)
00108       mexErrMsgTxt("Input I1 (left image) must be a uint8 image.");
00109     if (!mxIsUint8(prhs[2]) || mxGetNumberOfDimensions(prhs[2])!=2)
00110       mexErrMsgTxt("Input I2 (right image) must be a uint8 image.");
00111     if (mxGetM(prhs[1])!=mxGetM(prhs[2]) || mxGetN(prhs[1])!=mxGetN(prhs[2]))
00112       mexErrMsgTxt("Input I1 and I2 must be images of the same size.");
00113     if (nrhs==3+1 && !mxIsDouble(prhs[3]))
00114       mexErrMsgTxt("Input replace must be a double scalar (0/1).");
00115     
00116     // get image pointers
00117     uint8_t*       I1   = (uint8_t*)mxGetPr(prhs[1]);
00118     uint8_t*       I2   = (uint8_t*)mxGetPr(prhs[2]);
00119     const int32_t *dims =   mxGetDimensions(prhs[1]);
00120     
00121     // set replacement variable
00122     bool replace = false;
00123     if (nrhs==3+1)
00124       replace = (bool)(*((double*)mxGetPr(prhs[3])));
00125     
00126     // transpose images (align row-wise)
00127     uint8_t* I1_     = transpose<uint8_t>(I1,dims);
00128     uint8_t* I2_     = transpose<uint8_t>(I2,dims);
00129     int32_t  dims_[] = {dims[1],dims[0],dims[1]};
00130         
00131     // compute visual odometry, extrapolates linearly if matching failed
00132     viso->process(I1_,I2_,dims_,replace);
00133 
00134     // return motion estimate (mapping from previous to current frame)
00135     Matrix Tr_delta = ~(viso->getMotion());
00136     const int tr_dims[] = {4,4};
00137     plhs[0] = mxCreateNumericArray(2,tr_dims,mxDOUBLE_CLASS,mxREAL);
00138     Tr_delta.getData((double*)mxGetPr(plhs[0]));
00139     
00140     // release temporary memory
00141     free(I1_);
00142     free(I2_);
00143     
00144   // query number of matches
00145   } else if (!strcmp(command,"num_matches")) {
00146     const int dims[] = {1,1};
00147     plhs[0] = mxCreateNumericArray(2,dims,mxDOUBLE_CLASS,mxREAL);
00148     *((double*)mxGetPr(plhs[0])) = (double)viso->getNumberOfMatches();
00149     
00150   // query number of inliers
00151   } else if (!strcmp(command,"num_inliers")) {
00152     const int dims[] = {1,1};
00153     plhs[0] = mxCreateNumericArray(2,dims,mxDOUBLE_CLASS,mxREAL);
00154     *((double*)mxGetPr(plhs[0])) = (double)viso->getNumberOfInliers();
00155     
00156   // get matches
00157   } else if (!strcmp(command,"get_matches")) {
00158     
00159     // check arguments
00160     if (nrhs!=1+0)
00161       mexErrMsgTxt("No input required.");
00162     if (nlhs!=1)
00163       mexErrMsgTxt("One output required (p_matched).");
00164       
00165     // grab matches
00166     vector<Matcher::p_match> matches = viso->getMatches();
00167 
00168     // create output matrix with matches
00169     const int32_t p_matched_dims[] = {8,matches.size()};
00170     plhs[0] = mxCreateNumericArray(2,p_matched_dims,mxDOUBLE_CLASS,mxREAL);
00171     double* p_matched_mex = (double*)mxGetPr(plhs[0]);
00172 
00173     // copy matches to mex array (convert indices: C++ => MATLAB)
00174     int32_t k=0;
00175     for (vector<Matcher::p_match>::iterator it=matches.begin(); it!=matches.end(); it++) {
00176       *(p_matched_mex+k++) = it->u1p+1;
00177       *(p_matched_mex+k++) = it->v1p+1;
00178       *(p_matched_mex+k++) = it->u2p+1;
00179       *(p_matched_mex+k++) = it->v2p+1;
00180       *(p_matched_mex+k++) = it->u1c+1;
00181       *(p_matched_mex+k++) = it->v1c+1;
00182       *(p_matched_mex+k++) = it->u2c+1;
00183       *(p_matched_mex+k++) = it->v2c+1;
00184     }
00185     
00186   // get matching indices
00187   } else if (!strcmp(command,"get_indices")) {
00188     
00189     // check arguments
00190     if (nrhs!=1+0)
00191       mexErrMsgTxt("No input required.");
00192     if (nlhs!=1)
00193       mexErrMsgTxt("One output required (i_matched).");
00194     
00195     // grab matches
00196     vector<Matcher::p_match> matches = viso->getMatches();
00197       
00198     // create output matrix with matches
00199     const int32_t i_matched_dims[] = {4,matches.size()};
00200     plhs[0] = mxCreateNumericArray(2,i_matched_dims,mxDOUBLE_CLASS,mxREAL);
00201     double* i_matched_mex = (double*)mxGetPr(plhs[0]);
00202 
00203     // copy matches to mex array
00204     int32_t k=0;
00205     for (vector<Matcher::p_match>::iterator it=matches.begin(); it!=matches.end(); it++) {
00206       *(i_matched_mex+k++) = it->i1p+1;
00207       *(i_matched_mex+k++) = it->i2p+1;
00208       *(i_matched_mex+k++) = it->i1c+1;
00209       *(i_matched_mex+k++) = it->i2c+1;
00210     }
00211     
00212   // close
00213   } else if (!strcmp(command,"close")) {
00214     delete viso;
00215   
00216   // unknown command
00217   } else {
00218     mexPrintf("Unknown command: %s\n",command);
00219   }
00220 }


libviso2
Author(s): Andreas Geiger, maintained by: Stephan Wirth/stephan.wirth@uib.es
autogenerated on Tue Jan 7 2014 11:42:14