MapBuilder.h
Go to the documentation of this file.
00001 /*
00002 Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
00003 All rights reserved.
00004 
00005 Redistribution and use in source and binary forms, with or without
00006 modification, are permitted provided that the following conditions are met:
00007     * Redistributions of source code must retain the above copyright
00008       notice, this list of conditions and the following disclaimer.
00009     * Redistributions in binary form must reproduce the above copyright
00010       notice, this list of conditions and the following disclaimer in the
00011       documentation and/or other materials provided with the distribution.
00012     * Neither the name of the Universite de Sherbrooke nor the
00013       names of its contributors may be used to endorse or promote products
00014       derived from this software without specific prior written permission.
00015 
00016 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
00017 ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
00018 WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
00019 DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
00020 DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
00021 (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00022 LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
00023 ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
00024 (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
00025 SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
00026 */
00027 
00028 #ifndef MAPBUILDER_H_
00029 #define MAPBUILDER_H_
00030 
00031 #include <QVBoxLayout>
00032 #include <QtCore/QMetaType>
00033 #include <QAction>
00034 
00035 #ifndef Q_MOC_RUN // Mac OS X issue
00036 #include "rtabmap/gui/CloudViewer.h"
00037 #include "rtabmap/core/util3d.h"
00038 #include "rtabmap/core/util3d_filtering.h"
00039 #include "rtabmap/core/util3d_transforms.h"
00040 #include "rtabmap/core/RtabmapEvent.h"
00041 #endif
00042 #include "rtabmap/utilite/UStl.h"
00043 #include "rtabmap/utilite/UConversion.h"
00044 #include "rtabmap/utilite/UEventsHandler.h"
00045 #include "rtabmap/utilite/ULogger.h"
00046 #include "rtabmap/core/OdometryEvent.h"
00047 #include "rtabmap/core/CameraThread.h"
00048 
00049 using namespace rtabmap;
00050 
00051 // This class receives RtabmapEvent and construct/update a 3D Map
00052 class MapBuilder : public QWidget, public UEventsHandler
00053 {
00054         Q_OBJECT
00055 public:
00056         //Camera ownership is not transferred!
00057         MapBuilder(CameraThread * camera = 0) :
00058                 camera_(camera),
00059                 odometryCorrection_(Transform::getIdentity()),
00060                 processingStatistics_(false),
00061                 lastOdometryProcessed_(true)
00062         {
00063                 this->setWindowFlags(Qt::Dialog);
00064                 this->setWindowTitle(tr("3D Map"));
00065                 this->setMinimumWidth(800);
00066                 this->setMinimumHeight(600);
00067 
00068                 cloudViewer_ = new CloudViewer(this);
00069 
00070                 QVBoxLayout *layout = new QVBoxLayout();
00071                 layout->addWidget(cloudViewer_);
00072                 this->setLayout(layout);
00073 
00074                 qRegisterMetaType<rtabmap::OdometryEvent>("rtabmap::OdometryEvent");
00075                 qRegisterMetaType<rtabmap::Statistics>("rtabmap::Statistics");
00076 
00077                 QAction * pause = new QAction(this);
00078                 this->addAction(pause);
00079                 pause->setShortcut(Qt::Key_Space);
00080                 connect(pause, SIGNAL(triggered()), this, SLOT(pauseDetection()));
00081         }
00082 
00083         virtual ~MapBuilder()
00084         {
00085                 this->unregisterFromEventsManager();
00086         }
00087 
00088 protected Q_SLOTS:
00089         virtual void pauseDetection()
00090         {
00091                 UWARN("");
00092                 if(camera_)
00093                 {
00094                         if(camera_->isCapturing())
00095                         {
00096                                 camera_->join(true);
00097                         }
00098                         else
00099                         {
00100                                 camera_->start();
00101                         }
00102                 }
00103         }
00104 
00105         virtual void processOdometry(const rtabmap::OdometryEvent & odom)
00106         {
00107                 if(!this->isVisible())
00108                 {
00109                         return;
00110                 }
00111 
00112                 Transform pose = odom.pose();
00113                 if(pose.isNull())
00114                 {
00115                         //Odometry lost
00116                         cloudViewer_->setBackgroundColor(Qt::darkRed);
00117 
00118                         pose = lastOdomPose_;
00119                 }
00120                 else
00121                 {
00122                         cloudViewer_->setBackgroundColor(cloudViewer_->getDefaultBackgroundColor());
00123                 }
00124                 if(!pose.isNull())
00125                 {
00126                         lastOdomPose_ = pose;
00127 
00128                         // 3d cloud
00129                         if(odom.data().depthOrRightRaw().cols == odom.data().imageRaw().cols &&
00130                            odom.data().depthOrRightRaw().rows == odom.data().imageRaw().rows &&
00131                            !odom.data().depthOrRightRaw().empty() &&
00132                            (odom.data().stereoCameraModel().isValidForProjection() || odom.data().cameraModels().size()))
00133                         {
00134                                 pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud = util3d::cloudRGBFromSensorData(
00135                                         odom.data(),
00136                                         2,     // decimation
00137                                         4.0f); // max depth
00138                                 if(cloud->size())
00139                                 {
00140                                         if(!cloudViewer_->addCloud("cloudOdom", cloud, odometryCorrection_*pose))
00141                                         {
00142                                                 UERROR("Adding cloudOdom to viewer failed!");
00143                                         }
00144                                 }
00145                                 else
00146                                 {
00147                                         cloudViewer_->setCloudVisibility("cloudOdom", false);
00148                                         UWARN("Empty cloudOdom!");
00149                                 }
00150                         }
00151 
00152                         if(!odom.pose().isNull())
00153                         {
00154                                 // update camera position
00155                                 cloudViewer_->updateCameraTargetPosition(odometryCorrection_*odom.pose());
00156                         }
00157                 }
00158                 cloudViewer_->update();
00159 
00160                 lastOdometryProcessed_ = true;
00161         }
00162 
00163 
00164         virtual void processStatistics(const rtabmap::Statistics & stats)
00165         {
00166                 processingStatistics_ = true;
00167 
00168                 //============================
00169                 // Add RGB-D clouds
00170                 //============================
00171                 const std::map<int, Transform> & poses = stats.poses();
00172                 QMap<std::string, Transform> clouds = cloudViewer_->getAddedClouds();
00173                 for(std::map<int, Transform>::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)
00174                 {
00175                         if(!iter->second.isNull())
00176                         {
00177                                 std::string cloudName = uFormat("cloud%d", iter->first);
00178 
00179                                 // 3d point cloud
00180                                 if(clouds.contains(cloudName))
00181                                 {
00182                                         // Update only if the pose has changed
00183                                         Transform tCloud;
00184                                         cloudViewer_->getPose(cloudName, tCloud);
00185                                         if(tCloud.isNull() || iter->second != tCloud)
00186                                         {
00187                                                 if(!cloudViewer_->updateCloudPose(cloudName, iter->second))
00188                                                 {
00189                                                         UERROR("Updating pose cloud %d failed!", iter->first);
00190                                                 }
00191                                         }
00192                                         cloudViewer_->setCloudVisibility(cloudName, true);
00193                                 }
00194                                 else if(uContains(stats.getSignatures(), iter->first))
00195                                 {
00196                                         Signature s = stats.getSignatures().at(iter->first);
00197                                         s.sensorData().uncompressData(); // make sure data is uncompressed
00198                                         // Add the new cloud
00199                                         pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud = util3d::cloudRGBFromSensorData(
00200                                                         s.sensorData(),
00201                                                     4,     // decimation
00202                                                     4.0f); // max depth
00203                                         if(cloud->size())
00204                                         {
00205                                                 if(!cloudViewer_->addCloud(cloudName, cloud, iter->second))
00206                                                 {
00207                                                         UERROR("Adding cloud %d to viewer failed!", iter->first);
00208                                                 }
00209                                         }
00210                                         else
00211                                         {
00212                                                 UWARN("Empty cloud %d!", iter->first);
00213                                         }
00214                                 }
00215                         }
00216                         else
00217                         {
00218                                 UWARN("Null pose for %d ?!?", iter->first);
00219                         }
00220                 }
00221 
00222                 //============================
00223                 // Add 3D graph (show all poses)
00224                 //============================
00225                 cloudViewer_->removeAllGraphs();
00226                 cloudViewer_->removeCloud("graph_nodes");
00227                 if(poses.size())
00228                 {
00229                         // Set graph
00230                         pcl::PointCloud<pcl::PointXYZ>::Ptr graph(new pcl::PointCloud<pcl::PointXYZ>);
00231                         pcl::PointCloud<pcl::PointXYZ>::Ptr graphNodes(new pcl::PointCloud<pcl::PointXYZ>);
00232                         for(std::map<int, Transform>::const_iterator iter=poses.begin(); iter!=poses.end(); ++iter)
00233                         {
00234                                 graph->push_back(pcl::PointXYZ(iter->second.x(), iter->second.y(), iter->second.z()));
00235                         }
00236                         *graphNodes = *graph;
00237 
00238 
00239                         // add graph
00240                         cloudViewer_->addOrUpdateGraph("graph", graph, Qt::gray);
00241                         cloudViewer_->addCloud("graph_nodes", graphNodes, Transform::getIdentity(), Qt::green);
00242                         cloudViewer_->setCloudPointSize("graph_nodes", 5);
00243                 }
00244 
00245                 odometryCorrection_ = stats.mapCorrection();
00246 
00247                 cloudViewer_->update();
00248 
00249                 processingStatistics_ = false;
00250         }
00251 
00252         virtual bool handleEvent(UEvent * event)
00253         {
00254                 if(event->getClassName().compare("RtabmapEvent") == 0)
00255                 {
00256                         RtabmapEvent * rtabmapEvent = (RtabmapEvent *)event;
00257                         const Statistics & stats = rtabmapEvent->getStats();
00258                         // Statistics must be processed in the Qt thread
00259                         if(this->isVisible())
00260                         {
00261                                 QMetaObject::invokeMethod(this, "processStatistics", Q_ARG(rtabmap::Statistics, stats));
00262                         }
00263                 }
00264                 else if(event->getClassName().compare("OdometryEvent") == 0)
00265                 {
00266                         OdometryEvent * odomEvent = (OdometryEvent *)event;
00267                         // Odometry must be processed in the Qt thread
00268                         if(this->isVisible() &&
00269                            lastOdometryProcessed_ &&
00270                            !processingStatistics_)
00271                         {
00272                                 lastOdometryProcessed_ = false; // if we receive too many odometry events!
00273                                 QMetaObject::invokeMethod(this, "processOdometry", Q_ARG(rtabmap::OdometryEvent, *odomEvent));
00274                         }
00275                 }
00276                 return false;
00277         }
00278 
00279 protected:
00280         CloudViewer * cloudViewer_;
00281         CameraThread * camera_;
00282         Transform lastOdomPose_;
00283         Transform odometryCorrection_;
00284         bool processingStatistics_;
00285         bool lastOdometryProcessed_;
00286 };
00287 
00288 
00289 #endif /* MAPBUILDER_H_ */


rtabmap
Author(s): Mathieu Labbe
autogenerated on Thu Jun 6 2019 21:59:20