2f

C:\Users\KHOAND_VN\Desktop\2.cpp Wednesday, October 21, 2015 3:24 PM

#ifndef MAPBUILDER_H_#define MAPBUILDER_H_

#include <QVBoxLayout>#include <QtCore/QMetaType>#include <QAction>

#ifndef Q_MOC_RUN // Mac OS X issue#include "rtabmap/gui/CloudViewer.h"#include "rtabmap/core/util3d.h"#include "rtabmap/core/util3d_filtering.h"#include "rtabmap/core/util3d_transforms.h"#include "rtabmap/core/RtabmapEvent.h"#endif#include "rtabmap/utilite/UStl.h"#include "rtabmap/utilite/UConversion.h"#include "rtabmap/utilite/UEventsHandler.h"#include "rtabmap/utilite/ULogger.h"#include "rtabmap/core/OdometryEvent.h"#include "rtabmap/core/CameraThread.h"

using namespace rtabmap;

// This class receives RtabmapEvent and construct/update a 3D Mapclass MapBuilder : public QWidget, public UEventsHandler{

Q_OBJECTpublic:

//Camera ownership is not transferred!MapBuilder(CameraThread * camera = 0) :

camera_(camera),odometryCorrection_(Transform::getIdentity()),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::OdometryEvent>("rtabmap::OdometryEvent");qRegisterMetaType<rtabmap::Statistics>("rtabmap::Statistics");

QAction * pause = new QAction(this);this->addAction(pause);pause->setShortcut(Qt::Key_Space);connect(pause, SIGNAL(triggered()), this, SLOT(pauseDetection()));

-1-


}

virtual ~MapBuilder(){

this->unregisterFromEventsManager();}

protected slots:virtual void pauseDetection(){

UWARN("");if(camera_){

if(camera_->isCapturing()){

camera_->join(true);}else{

camera_->start();}

}}

virtual void processOdometry(const rtabmap::OdometryEvent & odom){

if(!this->isVisible()){

return;}

Transform pose = odom.pose();if(pose.isNull()){

//Odometry lostcloudViewer_->setBackgroundColor(Qt::darkRed);

pose = lastOdomPose_;}else{

cloudViewer_->setBackgroundColor(cloudViewer_->getDefaultBackgroundColor());}if(!pose.isNull()){

lastOdomPose_ = pose;

// 3d cloudif(odom.data().depthOrRightRaw().cols == odom.data().imageRaw().cols &&

odom.data().depthOrRightRaw().rows == odom.data().imageRaw().rows &&!odom.data().depthOrRightRaw().empty() &&(odom.data().stereoCameraModel().isValid() || odom.data().cameraModels().size()))

{pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud = util3d::cloudRGBFromSensorData(

-2-


odom.data(),2, // decimation4.0f); // max depth

if(cloud->size()){

if(!cloudViewer_->addOrUpdateCloud("cloudOdom", cloud, odometryCorrection_*pose)){

UERROR("Adding cloudOdom to viewer failed!");}

}else{

cloudViewer_->setCloudVisibility("cloudOdom", false);UWARN("Empty cloudOdom!");

}}

if(!odom.pose().isNull()){

// update camera positioncloudViewer_->updateCameraTargetPosition(odometryCorrection_*odom.pose());

}}cloudViewer_->update();

lastOdometryProcessed_ = true;}

virtual void processStatistics(const rtabmap::Statistics & stats){

processingStatistics_ = true;

//============================// Add RGB-D clouds//============================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 cloudif(clouds.contains(cloudName)){

// Update only if the pose has changedTransform tCloud;cloudViewer_->getPose(cloudName, tCloud);if(tCloud.isNull() || iter->second != tCloud){

-3-


if(!cloudViewer_->updateCloudPose(cloudName, iter->second)){

UERROR("Updating pose cloud %d failed!", iter->first);}

}cloudViewer_->setCloudVisibility(cloudName, true);

}else if(uContains(stats.getSignatures(), iter->first)){

Signature s = stats.getSignatures().at(iter->first);s.sensorData().uncompressData(); // make sure data is uncompressed// Add the new cloudpcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud = util3d::cloudRGBFromSensorData(

s.sensorData(),4, // decimation4.0f); // max depth

if(cloud->size()){

if(!cloudViewer_->addOrUpdateCloud(cloudName, cloud, iter->second)){

UERROR("Adding cloud %d to viewer failed!", iter->first);}

}else{

UWARN("Empty cloud %d!", iter->first);}

}}else{

UWARN("Null pose for %d ?!?", iter->first);}

}

//============================// Add 3D graph (show all poses)//============================cloudViewer_->removeAllGraphs();cloudViewer_->removeCloud("graph_nodes");if(poses.size()){

// Set graphpcl::PointCloud<pcl::PointXYZ>::Ptr graph(new pcl::PointCloud<pcl::PointXYZ>);pcl::PointCloud<pcl::PointXYZ>::Ptr graphNodes(new pcl::PointCloud<pcl::PointXYZ>);for(std::map<int, Transform>::const_iterator iter=poses.begin(); iter!=poses.end();++iter){

graph->push_back(pcl::PointXYZ(iter->second.x(), iter->second.y(), iter->second.z()));

}*graphNodes = *graph;

-4-


// add graphcloudViewer_->addOrUpdateGraph("graph", graph, Qt::gray);cloudViewer_->addOrUpdateCloud("graph_nodes", graphNodes, Transform::getIdentity(),Qt::green);cloudViewer_->setCloudPointSize("graph_nodes", 5);

}

odometryCorrection_ = stats.mapCorrection();

cloudViewer_->update();

processingStatistics_ = false;}

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 threadif(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 threadif(this->isVisible() &&

lastOdometryProcessed_ &&!processingStatistics_)

{lastOdometryProcessed_ = false; // if we receive too many odometry events!QMetaObject::invokeMethod(this, "processOdometry", Q_ARG(rtabmap::OdometryEvent,*odomEvent));

}}

}

protected:CloudViewer * cloudViewer_;CameraThread * camera_;Transform lastOdomPose_;Transform odometryCorrection_;bool processingStatistics_;bool lastOdometryProcessed_;

};

#endif /* MAPBUILDER_H_ */

-5-

2f

Documents

Transcript of 2f