mirror of
https://github.com/introlab/rtabmap.git
synced 2026-10-13 05:19:51 +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