reconstructionMex.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 "reconstruction.h"
00026 
00027 using namespace std;
00028 
00029 static Reconstruction *R;
00030 
00031 void mexFunction (int nlhs,mxArray *plhs[],int nrhs,const mxArray *prhs[]) {
00032 
00033   // read command
00034   char command[128];
00035   mxGetString(prhs[0],command,128);
00036 
00037   // init
00038   if (!strcmp(command,"init")) {
00039     
00040     // check arguments
00041     if(nrhs!=1+3)
00042       mexErrMsgTxt("3 parameters required (f,cu,cv).");
00043     if(!mxIsDouble(prhs[1]) || mxGetM(prhs[1])*mxGetN(prhs[1])!=1)
00044       mexErrMsgTxt("Input f must be a double scalar.");
00045     if(!mxIsDouble(prhs[2]) || mxGetM(prhs[2])*mxGetN(prhs[2])!=1)
00046       mexErrMsgTxt("Input cu must be a double scalar.");
00047     if(!mxIsDouble(prhs[3]) || mxGetM(prhs[3])*mxGetN(prhs[3])!=1)
00048       mexErrMsgTxt("Input cv must be a double scalar.");
00049     
00050     double f    = *((double*)mxGetPr(prhs[1]));
00051     double cu   = *((double*)mxGetPr(prhs[2]));
00052     double cv   = *((double*)mxGetPr(prhs[3]));
00053 
00054     R = new Reconstruction();
00055     R->setCalibration(f,cu,cv);
00056 
00057   // close
00058   } else if (!strcmp(command,"close")) {
00059     delete R;
00060 
00061   // update via observations
00062   } else if (!strcmp(command,"update")) {
00063     
00064     // check arguments (for parameter description see reconstruction.h)
00065     if(nrhs!=1+7)
00066       mexErrMsgTxt("7 inputs required (p_matched,i_matched,Tr,point_type,min_track_length,max_dist,min_angle).");
00067     if(!mxIsDouble(prhs[1]) || mxGetM(prhs[1])!=4)
00068       mexErrMsgTxt("Input p_matched must be a 4xN double matrix.");
00069     if(!mxIsDouble(prhs[2]) || mxGetM(prhs[2])!=2)
00070       mexErrMsgTxt("Input i_matched must be a 2xN double matrix.");
00071     if(!mxIsDouble(prhs[3]) || mxGetM(prhs[3])!=4 || mxGetN(prhs[3])!=4)
00072       mexErrMsgTxt("Input Tr must be a 4x4 double matrix.");
00073     if(!mxIsDouble(prhs[4]) || mxGetM(prhs[4])*mxGetN(prhs[4])!=1)
00074       mexErrMsgTxt("Input point_type must be a double scalar.");
00075     if(!mxIsDouble(prhs[5]) || mxGetM(prhs[5])*mxGetN(prhs[5])!=1)
00076       mexErrMsgTxt("Input min_track_length must be a double scalar.");
00077     if(!mxIsDouble(prhs[6]) || mxGetM(prhs[6])*mxGetN(prhs[6])!=1)
00078       mexErrMsgTxt("Input max_dist must be a double scalar.");
00079     if(!mxIsDouble(prhs[7]) || mxGetM(prhs[7])*mxGetN(prhs[7])!=1)
00080       mexErrMsgTxt("Input min_angle must be a double scalar.");
00081     if(nlhs!=0)
00082       mexErrMsgTxt("No outputs required.");
00083     
00084     // get pointers
00085     double* p_matched_data   =            (double*)mxGetPr(prhs[1]);
00086     double* i_matched_data   =            (double*)mxGetPr(prhs[2]);
00087     double* Tr_data          =            (double*)mxGetPr(prhs[3]);
00088     int32_t point_type       = (int32_t)*((double*)mxGetPr(prhs[4]));
00089     int32_t min_track_length = (int32_t)*((double*)mxGetPr(prhs[5]));
00090     double  max_dist         =          *((double*)mxGetPr(prhs[6]));
00091     double  min_angle        =          *((double*)mxGetPr(prhs[7]));
00092     int     N                =                      mxGetN(prhs[1]);
00093     
00094     // create match vector (convert indices: MATLAB => C++)
00095     vector<Matcher::p_match> p_matched;    
00096     Matcher::p_match match;
00097     int32_t k1=0,k2=0;
00098     for (int i=0; i<N; i++) {
00099       match.u1p =          p_matched_data[k1++]-1;
00100       match.v1p =          p_matched_data[k1++]-1;
00101       match.i1p = (int32_t)i_matched_data[k2++]-1;
00102       match.u1c =          p_matched_data[k1++]-1;
00103       match.v1c =          p_matched_data[k1++]-1;
00104       match.i1c = (int32_t)i_matched_data[k2++]-1;
00105       p_matched.push_back(match);
00106     }
00107     
00108     // create matrix
00109     Matrix Tr(4,4,Tr_data);
00110     Tr = ~Tr;
00111     
00112     // update
00113     R->update(p_matched,Tr,point_type,min_track_length,max_dist,min_angle);
00114     
00115   // get points
00116   } else if (!strcmp(command,"getpoints")) {
00117     
00118     // check arguments
00119     if(nlhs!=1)
00120       mexErrMsgTxt("1 output required (points).");
00121     
00122     // get inliers
00123     vector<Reconstruction::point3d> points = R->getPoints();
00124     const int dims[] = {3,points.size()};
00125     plhs[0] = mxCreateNumericArray(2,dims,mxDOUBLE_CLASS,mxREAL);
00126 
00127     double *points_mex = (double*)mxGetPr(plhs[0]);
00128     int32_t k=0;
00129     for (int i=0; i<points.size(); i++) {
00130       points_mex[k++] = points[i].x;
00131       points_mex[k++] = points[i].y;
00132       points_mex[k++] = points[i].z;
00133     }
00134     
00135   // unknown command
00136   } else {
00137     mexPrintf("Unknown command: %s\n",command);
00138   }
00139   
00140 }


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