Cplusplus RGBD Mapping

• Introduction
• Details
• Project configuration (example with CMake)
• Code
• Build and run
• Example without threads and events

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:

• CMakeLists.txt
• main.cpp
• MapBuilder.h

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.

Example without threads and events

The previous approach based on threads and events is preferred. However, if you want to see step-by-step how images are processed in the different modules, it would be easier to remove threading stuff. So I provide an example in which I don't use any threads and events. The code is here and the dataset used can be downloaded here in the stereo tutorial. It is also built when RTAB-Map is built from source, the name is rtabmap-noEventsExample.

Example (with unzipped stereo_20Hz.zip images):

./rtabmap-noEventsExample 20 2 10 stereo_20hz stereo_20Hz stereo_20hz/left stereo_20hz/right