constraint_app
/home/jbrooksh/drc/software/perception/constraint_app/src/matlab_mex/cam_setCurrentStateEstimate.cpp
Go to the documentation of this file.
00001 #include "mex.h"
00002 #include "ConstraintApp.h"
00003 
00016 void mexFunction( int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[] )
00017 {
00018   if ( nrhs != 2 ) {
00019     mexErrMsgTxt("requires exactly 2 parameters"); 
00020   }
00021 
00022   if ( nlhs != 0 ) {
00023     mexErrMsgTxt("returns exactly 0 parameters"); 
00024   }
00025 
00026   ConstraintApp* app(*(ConstraintApp**)mxGetData(prhs[0]));
00027 
00028   int stateSize = app->GetStateSize();
00029   if ( mxGetN(prhs[1]) * mxGetM(prhs[1]) != stateSize ) {
00030     mexErrMsgTxt("state must be Sx1, where S is the state size");
00031   }
00032 
00033   double* s((double*)mxGetData(prhs[1]));
00034 
00035   std::vector<double> state(stateSize);
00036   std::copy(s, s+stateSize, state.begin());
00037   app->SetCurrentStateEstimate(state);
00038 }
 All Classes Files Functions Variables Typedefs