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/OdometryInfo.h"
00041 #include "rtabmap/core/Statistics.h"
00042 #include "rtabmap/core/Signature.h"
00043 #endif
00044 #include "rtabmap/utilite/UStl.h"
00045 #include "rtabmap/utilite/UConversion.h"
00046 #include "rtabmap/utilite/ULogger.h"
00047 
00048 using namespace rtabmap;
00049 
00050 // This class receives RtabmapEvent and construct/update a 3D Map
00051 class MapBuilder : public QWidget
00052 {
00053         Q_OBJECT
00054 public:
00055         //Camera ownership is not transferred!
00056         MapBuilder() :
00057                 odometryCorrection_(Transform::getIdentity()),
00058                 paused_(false)
00059         {
00060                 this->setWindowFlags(Qt::Dialog);
00061                 this->setWindowTitle(tr("3D Map"));
00062                 this->setMinimumWidth(800);
00063                 this->setMinimumHeight(600);
00064 
00065                 cloudViewer_ = new CloudViewer(this);
00066 
00067                 QVBoxLayout *layout = new QVBoxLayout();
00068                 layout->addWidget(cloudViewer_);
00069                 this->setLayout(layout);
00070 
00071                 QAction * pause = new QAction(this);
00072                 this->addAction(pause);
00073                 pause->setShortcut(Qt::Key_Space);
00074                 connect(pause, SIGNAL(triggered()), this, SLOT(pauseDetection()));
00075         }
00076 
00077         virtual ~MapBuilder()
00078         {
00079         }
00080 
00081         bool isPaused() const {return paused_;}
00082 
00083         void processOdometry(
00084                         const SensorData & data,
00085                         Transform pose,
00086                         const rtabmap::OdometryInfo & odom)
00087         {
00088                 if(!this->isVisible())
00089                 {
00090                         return;
00091                 }
00092 
00093                 if(pose.isNull())
00094                 {
00095                         //Odometry lost
00096                         cloudViewer_->setBackgroundColor(Qt::darkRed);
00097 
00098                         pose = lastOdomPose_;
00099                 }
00100                 else
00101                 {
00102                         cloudViewer_->setBackgroundColor(cloudViewer_->getDefaultBackgroundColor());
00103                 }
00104                 if(!pose.isNull())
00105                 {
00106                         lastOdomPose_ = pose;
00107 
00108                         // 3d cloud
00109                         if(data.depthOrRightRaw().cols == data.imageRaw().cols &&
00110                            data.depthOrRightRaw().rows == data.imageRaw().rows &&
00111                            !data.depthOrRightRaw().empty() &&
00112                            (data.stereoCameraModel().isValidForProjection() || data.cameraModels().size()))
00113                         {
00114                                 pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud = util3d::cloudRGBFromSensorData(
00115                                         data,
00116                                         4,     // decimation
00117                                         0.0f); // max depth
00118                                 if(cloud->size())
00119                                 {
00120                                         if(!cloudViewer_->addCloud("cloudOdom", cloud, odometryCorrection_*pose))
00121                                         {
00122                                                 UERROR("Adding cloudOdom to viewer failed!");
00123                                         }
00124                                 }
00125                                 else
00126                                 {
00127                                         cloudViewer_->setCloudVisibility("cloudOdom", false);
00128                                         UWARN("Empty cloudOdom!");
00129                                 }
00130                         }
00131 
00132                         if(!pose.isNull())
00133                         {
00134                                 // update camera position
00135                                 cloudViewer_->updateCameraTargetPosition(odometryCorrection_*pose);
00136                         }
00137                 }
00138                 cloudViewer_->update();
00139         }
00140 
00141 
00142         void processStatistics(const rtabmap::Statistics & stats)
00143         {
00144 
00145                 //============================
00146                 // Add RGB-D clouds
00147                 //============================
00148                 const std::map<int, Transform> & poses = stats.poses();
00149                 QMap<std::string, Transform> clouds = cloudViewer_->getAddedClouds();
00150                 for(std::map<int, Transform>::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)
00151                 {
00152                         if(!iter->second.isNull())
00153                         {
00154                                 std::string cloudName = uFormat("cloud%d", iter->first);
00155 
00156                                 // 3d point cloud
00157                                 if(clouds.contains(cloudName))
00158                                 {
00159                                         // Update only if the pose has changed
00160                                         Transform tCloud;
00161                                         cloudViewer_->getPose(cloudName, tCloud);
00162                                         if(tCloud.isNull() || iter->second != tCloud)
00163                                         {
00164                                                 if(!cloudViewer_->updateCloudPose(cloudName, iter->second))
00165                                                 {
00166                                                         UERROR("Updating pose cloud %d failed!", iter->first);
00167                                                 }
00168                                         }
00169                                         cloudViewer_->setCloudVisibility(cloudName, true);
00170                                 }
00171                                 else if(uContains(stats.getSignatures(), iter->first))
00172                                 {
00173                                         Signature s = stats.getSignatures().at(iter->first);
00174                                         s.sensorData().uncompressData(); // make sure data is uncompressed
00175                                         // Add the new cloud
00176                                         pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud = util3d::cloudRGBFromSensorData(
00177                                                         s.sensorData(),
00178                                                         4,     // decimation
00179                                                         4.0f); // max depth
00180                                         if(cloud->size())
00181                                         {
00182                                                 if(!cloudViewer_->addCloud(cloudName, cloud, iter->second))
00183                                                 {
00184                                                         UERROR("Adding cloud %d to viewer failed!", iter->first);
00185                                                 }
00186                                         }
00187                                         else
00188                                         {
00189                                                 UWARN("Empty cloud %d!", iter->first);
00190                                         }
00191                                 }
00192                         }
00193                         else
00194                         {
00195                                 UWARN("Null pose for %d ?!?", iter->first);
00196                         }
00197                 }
00198 
00199                 //============================
00200                 // Add 3D graph (show all poses)
00201                 //============================
00202                 cloudViewer_->removeAllGraphs();
00203                 cloudViewer_->removeCloud("graph_nodes");
00204                 if(poses.size())
00205                 {
00206                         // Set graph
00207                         pcl::PointCloud<pcl::PointXYZ>::Ptr graph(new pcl::PointCloud<pcl::PointXYZ>);
00208                         pcl::PointCloud<pcl::PointXYZ>::Ptr graphNodes(new pcl::PointCloud<pcl::PointXYZ>);
00209                         for(std::map<int, Transform>::const_iterator iter=poses.begin(); iter!=poses.end(); ++iter)
00210                         {
00211                                 graph->push_back(pcl::PointXYZ(iter->second.x(), iter->second.y(), iter->second.z()));
00212                         }
00213                         *graphNodes = *graph;
00214 
00215 
00216                         // add graph
00217                         cloudViewer_->addOrUpdateGraph("graph", graph, Qt::gray);
00218                         cloudViewer_->addCloud("graph_nodes", graphNodes, Transform::getIdentity(), Qt::green);
00219                         cloudViewer_->setCloudPointSize("graph_nodes", 5);
00220                 }
00221 
00222                 odometryCorrection_ = stats.mapCorrection();
00223 
00224                 cloudViewer_->update();
00225         }
00226 
00227 protected Q_SLOTS:
00228         void pauseDetection()
00229         {
00230                 paused_ = !paused_;
00231         }
00232 
00233 protected:
00234         CloudViewer * cloudViewer_;
00235         Transform lastOdomPose_;
00236         Transform odometryCorrection_;
00237         bool paused_;
00238 };
00239 
00240 
00241 #endif /* MAPBUILDER_H_ */


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