/* 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();
}
