visualOdometryMonoMex.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_mono.h"
00024 
00025 #include <iostream>
00026 #include <string.h>
00027 
00028 using namespace std;
00029 
00030 static VisualOdometryMono *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     VisualOdometryMono::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,"height"))                 param.height = val;
00088         if (!strcmp(field_name,"pitch"))                  param.pitch = val;
00089         if (!strcmp(field_name,"ransac_iters"))           param.ransac_iters = val;
00090         if (!strcmp(field_name,"inlier_threshold"))       param.inlier_threshold = val;
00091         if (!strcmp(field_name,"motion_threshold"))       param.motion_threshold = val;
00092       }
00093     }
00094     
00095     // create visual odometry instance
00096     viso = new VisualOdometryMono(param);
00097 
00098   // process
00099   } else if (!strcmp(command,"process")) {
00100     
00101     // check for proper number of arguments
00102     if (nrhs!=1+1 && nrhs!=2+1)
00103       mexErrMsgTxt("1 or 2 inputs required: I (image), [replace].");
00104     if (nlhs!=1) 
00105       mexErrMsgTxt("1 outputs required: Tr (transformation from previous to current frame.");  
00106 
00107     // check for proper argument types and sizes
00108     if (!mxIsUint8(prhs[1]) || mxGetNumberOfDimensions(prhs[1])!=2)
00109       mexErrMsgTxt("Input I (image) must be a uint8 image.");
00110     if (nrhs==2+1 && !mxIsDouble(prhs[2]))
00111       mexErrMsgTxt("Input replace must be a double scalar (0/1).");
00112     
00113     // get image pointers
00114     uint8_t*       I    = (uint8_t*)mxGetPr(prhs[1]);
00115     const int32_t *dims =   mxGetDimensions(prhs[1]);
00116     
00117     // set replacement variable
00118     bool replace = false;
00119     if (nrhs==2+1)
00120       replace = (bool)(*((double*)mxGetPr(prhs[2])));
00121     
00122     // transpose images (align row-wise)
00123     uint8_t* I_      = transpose<uint8_t>(I,dims);
00124     int32_t  dims_[] = {dims[1],dims[0],dims[1]};
00125         
00126     // compute visual odometry, return motion estimate on success
00127     // (mapping from previous to current frame)
00128     if (viso->process(I_,dims_,replace)) {
00129 
00130       Matrix Tr_delta = ~(viso->getMotion());
00131       const int tr_dims[] = {4,4};
00132       plhs[0] = mxCreateNumericArray(2,tr_dims,mxDOUBLE_CLASS,mxREAL);
00133       Tr_delta.getData((double*)mxGetPr(plhs[0]));
00134       
00135     // return empty transformation on failure
00136     // (motion too small to estimate scale)
00137     } else {
00138       
00139       // return motion estimate (mapping from previous to current frame)
00140       const int tr_dims[] = {0,0};
00141       plhs[0] = mxCreateNumericArray(2,tr_dims,mxDOUBLE_CLASS,mxREAL);     
00142     }
00143     
00144     // release temporary memory
00145     free(I_);
00146     
00147   // query number of matches
00148   } else if (!strcmp(command,"num_matches")) {
00149     const int dims[] = {1,1};
00150     plhs[0] = mxCreateNumericArray(2,dims,mxDOUBLE_CLASS,mxREAL);
00151     *((double*)mxGetPr(plhs[0])) = (double)viso->getNumberOfMatches();
00152     
00153   // query number of inliers
00154   } else if (!strcmp(command,"num_inliers")) {
00155     const int dims[] = {1,1};
00156     plhs[0] = mxCreateNumericArray(2,dims,mxDOUBLE_CLASS,mxREAL);
00157     *((double*)mxGetPr(plhs[0])) = (double)viso->getNumberOfInliers();
00158     
00159   // get matches
00160   } else if (!strcmp(command,"get_matches")) {
00161     
00162     // check arguments
00163     if (nrhs!=1+0)
00164       mexErrMsgTxt("No input required.");
00165     if (nlhs!=1)
00166       mexErrMsgTxt("One output required (p_matched).");
00167    
00168     // grab matches
00169     vector<Matcher::p_match> matches = viso->getMatches();
00170           
00171     // create output matrix with matches
00172     const int32_t p_matched_dims[] = {4,matches.size()};
00173     plhs[0] = mxCreateNumericArray(2,p_matched_dims,mxDOUBLE_CLASS,mxREAL);
00174     double* p_matched_mex = (double*)mxGetPr(plhs[0]);
00175 
00176     // copy matches to mex array (convert indices: C++ => MATLAB)
00177     int32_t k=0;
00178     for (vector<Matcher::p_match>::iterator it=matches.begin(); it!=matches.end(); it++) {
00179       *(p_matched_mex+k++) = it->u1p+1;
00180       *(p_matched_mex+k++) = it->v1p+1;
00181       *(p_matched_mex+k++) = it->u1c+1;
00182       *(p_matched_mex+k++) = it->v1c+1;
00183     }
00184     
00185   // get matching indices
00186   } else if (!strcmp(command,"get_indices")) {
00187     
00188     // check arguments
00189     if (nrhs!=1+0)
00190       mexErrMsgTxt("No input required.");
00191     if (nlhs!=1)
00192       mexErrMsgTxt("One output required (i_matched).");
00193     
00194     // grab matches
00195     vector<Matcher::p_match> matches = viso->getMatches();
00196 
00197     // create output matrix with matches
00198     const int32_t i_matched_dims[] = {2,matches.size()};
00199     plhs[0] = mxCreateNumericArray(2,i_matched_dims,mxDOUBLE_CLASS,mxREAL);
00200     double* i_matched_mex = (double*)mxGetPr(plhs[0]);
00201 
00202     // copy matches to mex array
00203     int32_t k=0;
00204     for (vector<Matcher::p_match>::iterator it=matches.begin(); it!=matches.end(); it++) {
00205       *(i_matched_mex+k++) = it->i1p+1;
00206       *(i_matched_mex+k++) = it->i1c+1;
00207     }
00208     
00209   // close
00210   } else if (!strcmp(command,"close")) {
00211     delete viso;
00212   
00213   // unknown command
00214   } else {
00215     mexPrintf("Unknown command: %s\n",command);
00216   }
00217 }


libviso2
Author(s): Andreas Geiger, Stephan Wirth
autogenerated on Fri Aug 28 2015 13:37:45