Skip to content

Cplusplus RGBD Mapping

Jump to bottom
matlabbe edited this page Jan 30, 2015 · 25 revisions

Introduction

This tutorial will show an example of usage of the RTAB-Map library in C++ for RGB-D mapping.

Details

Project configuration (example with CMake)

Here our project directory:

MyProject
|
-->CMakeLists.txt
-->main.cpp
-->MapBuilder.h

Files:

In your 'CMakeLists.txt':

cmake_minimum_required(VERSION 2.8)
PROJECT( MyProject )

FIND_PACKAGE(RTABMap REQUIRED)
FIND_PACKAGE(OpenCV REQUIRED)
FIND_PACKAGE(PCL 1.7 REQUIRED)
FIND_PACKAGE(Qt4 REQUIRED COMPONENTS QtCore QtGui QtSvg) 

SET(INCLUDE_DIRS
   ${RTABMap_INCLUDE_DIRS}
   ${OpenCV_INCLUDE_DIRS}
   ${PCL_INCLUDE_DIRS}
)

INCLUDE(${QT_USE_FILE})

SET(LIBRARIES
   ${RTABMap_LIBRARIES}
   ${OpenCV_LIBRARIES}
   ${QT_LIBRARIES} 
   ${PCL_LIBRARIES}
)

INCLUDE_DIRECTORIES(${INCLUDE_DIRS})

QT4_WRAP_CPP(moc_srcs MapBuilder.h)

ADD_EXECUTABLE(rgbd_example main.cpp ${moc_srcs})
 
TARGET_LINK_LIBRARIES(rgbd_example ${LIBRARIES})

Code

The 'main.cpp'

#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/utilite/UEventsManager.h"
#include <QtGui/QApplication>
#include <stdio.h>

#include "MapBuilder.h"

using namespace rtabmap;
int main(int argc, char * argv[])
{
   ULogger::setType(ULogger::kTypeConsole);
   ULogger::setLevel(ULogger::kWarning);

   // GUI stuff, there the handler will receive RtabmapEvent and construct the map
   QApplication app(argc, argv);
   MapBuilder mapBuilder;

   // 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 forward
   CameraThread cameraThread(new CameraOpenni("", 30, rtabmap::Transform(0,0,1,0, -1,0,0,0, 0,-1,0,0)));
   if(!cameraThread.init())
   {
   	UERROR("Camera init failed!");
   	exit(1);
   }	

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


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

   // Setup handlers
   odomThread.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 threads
   rtabmapThread.start();
   odomThread.start();
   cameraThread.start();

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

   // remove handlers
   mapBuilder.unregisterFromEventsManager();
   rtabmapThread.unregisterFromEventsManager();
   odomThread.unregisterFromEventsManager();

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

   return 0;
}

The MapBuilder class (for visualization):

#ifndef MAPBUILDER_H_
#define MAPBUILDER_H_

#include <QtGui/QVBoxLayout>
#include <QtCore/QMetaType>
#include "rtabmap/gui/CloudViewer.h"
#include "rtabmap/utilite/UStl.h"
#include "rtabmap/utilite/UConversion.h"
#include "rtabmap/utilite/UEventsHandler.h"
#include "rtabmap/utilite/ULogger.h"
#include "rtabmap/core/util3d.h"
#include "rtabmap/core/RtabmapEvent.h"
#include "rtabmap/core/OdometryEvent.h"

using namespace rtabmap;

// This class receives RtabmapEvent and construct/update a 3D Map
class MapBuilder : public QWidget, public UEventsHandler
{
   Q_OBJECT
public:
   MapBuilder() :
   	_processingStatistics(false),
   	_lastOdometryProcessed(true)
   {
   	this->setWindowFlags(Qt::Dialog);
   	this->setWindowTitle(tr("3D Map"));
   	this->setMinimumWidth(800);
   	this->setMinimumHeight(600);

   	cloudViewer_ = new CloudViewer(this);

   	QVBoxLayout *layout = new QVBoxLayout();
   	layout->addWidget(cloudViewer_);
   	this->setLayout(layout);

   	qRegisterMetaType<rtabmap::Statistics>("rtabmap::Statistics");
   	qRegisterMetaType<rtabmap::SensorData>("rtabmap::SensorData");
   }

   virtual ~MapBuilder()
   {
   	this->unregisterFromEventsManager();
   }

private slots:
   void processOdometry(const rtabmap::SensorData & data)
   {
   	if(!this->isVisible())
   	{
   		return;
   	}

   	Transform pose = data.pose();
   	if(pose.isNull())
   	{
   		//Odometry lost
   		cloudViewer_->setBackgroundColor(Qt::darkRed);

   		pose = lastOdomPose_;
   	}
   	else
   	{
   		cloudViewer_->setBackgroundColor(Qt::black);
   	}
   	if(!pose.isNull())
   	{
   		lastOdomPose_ = pose;

   		// 3d cloud
   		if(data.depth().cols == data.image().cols &&
   		   data.depth().rows == data.image().rows &&
   		   !data.depth().empty() &&
   		   data.fx() > 0.0f &&
   		   data.fy() > 0.0f)
   		{
   			pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud = util3d::cloudFromDepthRGB(
   				data.image(),
   				data.depth(),
   				data.cx(),
   				data.cy(),
   				data.fx(),
   				data.fy(),
   				2); // decimation // high definition
   			if(cloud->size())
   			{
   				cloud = util3d::passThrough<pcl::PointXYZRGB>(cloud, "z", 0, 4.0f);
   				if(cloud->size())
   				{
   					cloud = util3d::transformPointCloud<pcl::PointXYZRGB>(cloud, data.localTransform());
   				}
   			}
   			if(!cloudViewer_->addOrUpdateCloud("cloudOdom", cloud, pose))
   			{
   				UERROR("Adding cloudOdom to viewer failed!");
   			}
   		}

   		if(!data.pose().isNull())
   		{
   			// update camera position
   			cloudViewer_->updateCameraTargetPosition(data.pose());
   		}
   	}
   	cloudViewer_->update();

   	_lastOdometryProcessed = true;
   }


   void processStatistics(const rtabmap::Statistics & stats)
   {
   	_processingStatistics = true;

   	const std::map<int, Transform> & poses = stats.poses();
   	QMap<std::string, Transform> clouds = cloudViewer_->getAddedClouds();
   	for(std::map<int, Transform>::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)
   	{
   		if(!iter->second.isNull())
   		{
   			std::string cloudName = uFormat("cloud%d", iter->first);

   			// 3d point cloud
   			if(clouds.contains(cloudName))
   			{
   				// Update only if the pose has changed
   				Transform tCloud;
   				cloudViewer_->getPose(cloudName, tCloud);
   				if(tCloud.isNull() || iter->second != tCloud)
   				{
   					if(!cloudViewer_->updateCloudPose(cloudName, iter->second))
   					{
   						UERROR("Updating pose cloud %d failed!", iter->first);
   					}
   				}
   				cloudViewer_->setCloudVisibility(cloudName, true);
   			}
   			else if(iter->first == stats.refImageId() &&
   					stats.getSignature().id() == iter->first)
   			{
   				Signature s = stats.getSignature();
   				s.uncompressData(); // make sure data is uncompressed
   				// Add the new cloud
   				pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud = util3d::cloudFromDepthRGB(
   						s.getImageRaw(),
   						s.getDepthRaw(),
   						s.getDepthCx(),
   						s.getDepthCy(),
   						s.getDepthFx(),
   						s.getDepthFy(),
   					   8); // decimation

   				if(cloud->size())
   				{
   					cloud = util3d::passThrough<pcl::PointXYZRGB>(cloud, "z", 0, 4.0f);
   					if(cloud->size())
   					{
   						cloud = util3d::transformPointCloud<pcl::PointXYZRGB>(cloud, stats.getSignature().getLocalTransform());
   					}
   				}
   				if(!cloudViewer_->addOrUpdateCloud(cloudName, cloud, iter->second))
   				{
   					UERROR("Adding cloud %d to viewer failed!", iter->first);
   				}
   			}
   		}
   	}

   	cloudViewer_->update();

   	_processingStatistics = false;
   }

protected:
   virtual void handleEvent(UEvent * event)
   {
   	if(event->getClassName().compare("RtabmapEvent") == 0)
   	{
   		RtabmapEvent * rtabmapEvent = (RtabmapEvent *)event;
   		const Statistics & stats = rtabmapEvent->getStats();
   		// Statistics must be processed in the Qt thread
   		if(this->isVisible())
   		{
   			QMetaObject::invokeMethod(this, "processStatistics", Q_ARG(rtabmap::Statistics, stats));
   		}
   	}
   	else if(event->getClassName().compare("OdometryEvent") == 0)
   	{
   		OdometryEvent * odomEvent = (OdometryEvent *)event;
   		// Odometry must be processed in the Qt thread
   		if(this->isVisible() &&
   		   _lastOdometryProcessed &&
   		   !_processingStatistics)
   		{
   			_lastOdometryProcessed = false; // if we receive too many odometry events!
   			QMetaObject::invokeMethod(this, "processOdometry", Q_ARG(rtabmap::SensorData, odomEvent->data()));
   		}
   	}
   }

private:
   CloudViewer * cloudViewer_;
   Transform lastOdomPose_;
   bool _processingStatistics;
   bool _lastOdometryProcessed;
};


#endif /* MAPBUILDER_H_ */

Build and run

In your directory:

$ cmake .
$ make
$ ./rgbd_example

You should see a window with the 3D map constructing. If the background turns red, that means that odometry cannot be computed: visit the Lost Odometry section on this page for more information.

Clone this wiki locally