diff --git a/C---RGB-D-Mapping.md b/C---RGB-D-Mapping.md new file mode 100644 index 0000000..b13a58b --- /dev/null +++ b/C---RGB-D-Mapping.md @@ -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 +#include + +#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 +#include +#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"); + qRegisterMetaType("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::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(cloud, "z", 0, 4.0f); + if(cloud->size()) + { + cloud = util3d::transformPointCloud(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 & poses = stats.poses(); + QMap clouds = cloudViewer_->getAddedClouds(); + for(std::map::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::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(cloud, "z", 0, 4.0f); + if(cloud->size()) + { + cloud = util3d::transformPointCloud(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. \ No newline at end of file