mirror of
https://github.com/introlab/rtabmap.git
synced 2026-10-07 02:27:47 +08:00
Created C++ RGB-D Mapping (markdown)
matlabbe
+358
@@ -0,0 +1,358 @@
|
||||
## 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](https://raw.githubusercontent.com/wiki/introlab/rtabmap/doc/Tutorials/CplusplusExampleRGBD/CMakeLists.txt)
|
||||
* [main.cpp](https://github.com/introlab/rtabmap/blob/master/examples/RGBDMapping/main.cpp)
|
||||
* [MapBuilder.h](https://github.com/introlab/rtabmap/blob/master/examples/RGBDMapping/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_->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_ */
|
||||
```
|
||||
|
||||
## 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.
|
||||
Reference in New Issue
Block a user