C:\Users\KHOAND_VN\Desktop\1.cpp Wednesday, October 21, 2015 3:24 PM

#include "rtabmap/core/Rtabmap.h"#include "rtabmap/core/RtabmapThread.h"#include "rtabmap/core/CameraRGBD.h"#include "rtabmap/core/CameraThread.h"#include "rtabmap/core/Odometry.h"#include "rtabmap/core/OdometryThread.h"#include "rtabmap/utilite/UEventsManager.h"#include #include

#include "MapBuilder.h"

void showUsage(){

printf("\nUsage:\n""rtabmap-rgbd_mapping driver\n"" driver Driver number to use: 0=OpenNI-PCL, 1=OpenNI2, 2=Freenect, 3=OpenNI-CV, 4=OpenNI-CV-ASUS\n\n");

exit(1);}

using namespace rtabmap;int main(int argc, char * argv[]){

ULogger::setType(ULogger::kTypeConsole);ULogger::setLevel(ULogger::kWarning);

int driver = 0;if(argc < 2){

showUsage();}else{

driver = atoi(argv[argc-1]);if(driver < 0 || driver > 4){

UERROR("driver should be between 0 and 4.");showUsage();

}}

// Here is the pipeline that we will use:// CameraOpenni -> "CameraEvent" -> OdometryThread -> "OdometryEvent" -> RtabmapThread -> "RtabmapEvent"

// Create the OpenNI camera, it will send a CameraEvent at the rate specified.// Set transform to camera so z is up, y is left and x going forwardCamera * camera = 0;Transform opticalRotation(0,0,1,0, -1,0,0,0, 0,-1,0,0);if(driver == 1){

if(!CameraOpenNI2::available()){

-1-

C:\Users\KHOAND_VN\Desktop\1.cpp Wednesday, October 21, 2015 3:24 PM

UERROR("Not built with OpenNI2 support...");exit(-1);

}camera = new CameraOpenNI2("", 0, opticalRotation);

}else if(driver == 2){

if(!CameraFreenect::available()){

UERROR("Not built with Freenect support...");exit(-1);

}camera = new CameraFreenect(0, 0, opticalRotation);

}else if(driver == 3){

if(!CameraOpenNICV::available()){

UERROR("Not built with OpenNI from OpenCV support...");exit(-1);

}camera = new CameraOpenNICV(false, 0, opticalRotation);

}else if(driver == 4){

if(!CameraOpenNICV::available()){

UERROR("Not built with OpenNI from OpenCV support...");exit(-1);

}camera = new CameraOpenNICV(true, 0, opticalRotation);

}else{

camera = new rtabmap::CameraOpenni("", 0, opticalRotation);}

if(!camera->init()){

UERROR("Camera init failed!");}

CameraThread cameraThread(camera);

// GUI stuff, there the handler will receive RtabmapEvent and construct the map// We give it the camera so the GUI can pause/resume the cameraQApplication app(argc, argv);MapBuilder mapBuilder(&cameraThread);

// Create an odometry thread to process camera events, it will send OdometryEvent.OdometryThread odomThread(new OdometryBOW());

-2-

C:\Users\KHOAND_VN\Desktop\1.cpp Wednesday, October 21, 2015 3:24 PM

// Create RTAB-Map to process OdometryEventRtabmap * rtabmap = new Rtabmap();rtabmap->init();RtabmapThread rtabmapThread(rtabmap); // ownership is transfered

// Setup handlersodomThread.registerToEventsManager();rtabmapThread.registerToEventsManager();mapBuilder.registerToEventsManager();

// The RTAB-Map is subscribed by default to CameraEvent, but we want// RTAB-Map to process OdometryEvent instead, ignoring the CameraEvent.// We can do that by creating a "pipe" between the camera and odometry, then// only the odometry will receive CameraEvent from that camera. RTAB-Map is// also subscribed to OdometryEvent by default, so no need to create a pipe between// odometry and RTAB-Map.UEventsManager::createPipe(&cameraThread, &odomThread, "CameraEvent");

// Let's start the threadsrtabmapThread.start();odomThread.start();cameraThread.start();

mapBuilder.show();app.exec(); // main loop

// remove handlersmapBuilder.unregisterFromEventsManager();rtabmapThread.unregisterFromEventsManager();odomThread.unregisterFromEventsManager();

// Kill all threadscameraThread.kill();odomThread.join(true);rtabmapThread.join(true);

return 0;}

-3-