-
Notifications
You must be signed in to change notification settings - Fork 908
Cplusplus RGBD Mapping
matlabbe edited this page Jan 22, 2015
·
25 revisions
This tutorial will show an example of usage of the RTAB-Map library in C++ for RGB-D mapping.
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})
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_->updateCameraPosition(data.pose());
}
}
cloudViewer_->render();
_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)
{
// Add the new cloud
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud = util3d::cloudFromDepthRGB(
stats.getSignature().getImageRaw(),
stats.getSignature().getDepthRaw(),
stats.getSignature().getDepthCx(),
stats.getSignature().getDepthCy(),
stats.getSignature().getDepthFx(),
stats.getSignature().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_->render();
_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_ */In your directory:
$ cmake .
$ make
$ ./rgbd_exampleYou should see a window with the 3D map constructing. If the background turns red, that means that odometry cannot be computed.