/* mexFunction - Entry point for Matlab */ void mexFunction( int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[] ) { double *x_obst, *x_drone, *R_G2B, *parameter, *dist_out; size_t N; /* Check for proper number of arguments. */ if(nrhs!=4) { mexErrMsgIdAndTxt( "MATLAB:rotation_matrix:invalidNumInputs", "4 input required! (x_obst, x_drone, R_G2B, parameter)"); } else if(nlhs>1) { mexErrMsgIdAndTxt( "MATLAB:rotation_matrix:invalidNumOutputs", "Too many output arguments."); } /* Assign memory for result */ plhs[0] = mxCreateDoubleMatrix((mwSize)6, (mwSize)1, mxREAL); /* Assign pointers to each input and output. */ x_obst = mxGetPr(prhs[0]); x_drone = mxGetPr(prhs[1]); R_G2B = mxGetPr(prhs[2]); parameter = mxGetPr(prhs[3]); dist_out = mxGetPr(plhs[0]); N = mxGetN(prhs[0]); #ifdef __DEBUG_2 mexPrintf("Size of x_obst: %d\n",N); #endif /* Call the subroutine. */ rangeFinder(dist_out, x_obst, x_drone, R_G2B, parameter, N); }
int main() { /* the basic rig config */ RigConfig rc; rc.loadFromFile("rigconfig_core_pullup.xml"); /* camera parameter data */ CameraParameters::Ptr cameraParameters(new CameraParameters(rc)); cameraParameters->print(); /* pose tracking for the camera*/ PoseTrackerKinfu::Ptr poseTracker(new PoseTrackerKinfu(rc)); poseTracker->start(); /* range finder with kinfu cloud provider */ CloudProvider<pcl::PointXYZ>::Ptr rangeimageProvider = poseTracker->getKinfu(); RangeFinder<pcl::PointXYZ>::Ptr rangeFinder(new RangeFinder<pcl::PointXYZ>(rc)); rangeFinder->setCloudSource(rangeimageProvider); /* viewfinder to extract pois from the scene */ ViewFinderRangeImage<pcl::PointXYZ> viewFinder(0.5); viewFinder.setRangeFinder(rangeFinder); viewFinder.setCameraParameters(cameraParameters); /* the focus motor */ FocusMotor::Ptr focusMotor(new FocusMotorTwoPolys(rc)); /* create a focus controller */ FocusControllerSinglePoint::Ptr fCtrlSingle(new FocusControllerSinglePoint( cameraParameters, poseTracker)); fCtrlSingle->setIdentifier("SingeTracker0"); fCtrlSingle->setPriority(5); fCtrlSingle->start(); /* focus control */ CentralFocusControl fControl; fControl.setFocusMotor(focusMotor); fControl.addFocusController(fCtrlSingle); fControl.start(); /* simulate a main loop */ for (int i=0; i<50; i++) { poseTracker->getPose(); fControl.focusDistance(); fControl.getFocusedPoint(); fControl.activlyControlled(); sleep(0.55); } /* save cloud from kinfu */ pcl::PCDWriter writer; CloudProvider<pcl::PointXYZ>::CloudPtr outCloud( new CloudProvider<pcl::PointXYZ>::Cloud( *rangeimageProvider->getLastCloud())); writer.writeASCII("kinfu_cloud_pullup.pcd", *outCloud ); viewFinder.compute(); pcl::RangeImagePlanar::Ptr rangeImage = viewFinder.getRangeImage(); RangeImageWriter riWriter(rangeImage); riWriter.save("rangeimage_pullup.png"); viewFinder.getMiddlePoint(); }