RGBDMapping/MapBuilder.h
Go to the documentation of this file.
1 /*
2 Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
3 All rights reserved.
4 
5 Redistribution and use in source and binary forms, with or without
6 modification, are permitted provided that the following conditions are met:
7  * Redistributions of source code must retain the above copyright
8  notice, this list of conditions and the following disclaimer.
9  * Redistributions in binary form must reproduce the above copyright
10  notice, this list of conditions and the following disclaimer in the
11  documentation and/or other materials provided with the distribution.
12  * Neither the name of the Universite de Sherbrooke nor the
13  names of its contributors may be used to endorse or promote products
14  derived from this software without specific prior written permission.
15 
16 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
17 ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
18 WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
19 DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
20 DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
21 (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
22 LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
23 ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
24 (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
25 SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
26 */
27 
28 #ifndef MAPBUILDER_H_
29 #define MAPBUILDER_H_
30 
31 #include <QVBoxLayout>
32 #include <QtCore/QMetaType>
33 #include <QAction>
34 
35 #ifndef Q_MOC_RUN // Mac OS X issue
37 #include "rtabmap/core/util3d.h"
42 #endif
43 #include "rtabmap/utilite/UStl.h"
49 
50 using namespace rtabmap;
51 
52 // This class receives RtabmapEvent and construct/update a 3D Map
53 class MapBuilder : public QWidget, public UEventsHandler
54 {
55  Q_OBJECT
56 public:
57  //Camera ownership is not transferred!
58  MapBuilder(CameraThread * camera = 0) :
59  camera_(camera),
60  odometryCorrection_(Transform::getIdentity()),
61  processingStatistics_(false),
62  lastOdometryProcessed_(true)
63  {
64  this->setWindowFlags(Qt::Dialog);
65  this->setWindowTitle(tr("3D Map"));
66  this->setMinimumWidth(800);
67  this->setMinimumHeight(600);
68 
69  cloudViewer_ = new CloudViewer(this);
70 
71  QVBoxLayout *layout = new QVBoxLayout();
72  layout->addWidget(cloudViewer_);
73  this->setLayout(layout);
74 
75  qRegisterMetaType<rtabmap::OdometryEvent>("rtabmap::OdometryEvent");
76  qRegisterMetaType<rtabmap::Statistics>("rtabmap::Statistics");
77 
78  QAction * pause = new QAction(this);
79  this->addAction(pause);
80  pause->setShortcut(Qt::Key_Space);
81  connect(pause, SIGNAL(triggered()), this, SLOT(pauseDetection()));
82  }
83 
84  virtual ~MapBuilder()
85  {
86  this->unregisterFromEventsManager();
87  }
88 
89 protected Q_SLOTS:
90  virtual void pauseDetection()
91  {
92  UWARN("");
93  if(camera_)
94  {
95  if(camera_->isCapturing())
96  {
97  camera_->join(true);
98  }
99  else
100  {
101  camera_->start();
102  }
103  }
104  }
105 
106  virtual void processOdometry(const rtabmap::OdometryEvent & odom)
107  {
108  if(!this->isVisible())
109  {
110  return;
111  }
112 
113  Transform pose = odom.pose();
114  if(pose.isNull())
115  {
116  //Odometry lost
117  cloudViewer_->setBackgroundColor(Qt::darkRed);
118 
119  pose = lastOdomPose_;
120  }
121  else
122  {
123  cloudViewer_->setBackgroundColor(cloudViewer_->getDefaultBackgroundColor());
124  }
125  if(!pose.isNull())
126  {
127  lastOdomPose_ = pose;
128 
129  // 3d cloud
130  if(odom.data().depthOrRightRaw().cols == odom.data().imageRaw().cols &&
131  odom.data().depthOrRightRaw().rows == odom.data().imageRaw().rows &&
132  !odom.data().depthOrRightRaw().empty() &&
133  (odom.data().stereoCameraModel().isValidForProjection() || odom.data().cameraModels().size()))
134  {
135  pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud = util3d::cloudRGBFromSensorData(
136  odom.data(),
137  2, // decimation
138  4.0f); // max depth
139  if(cloud->size())
140  {
141  if(!cloudViewer_->addCloud("cloudOdom", cloud, odometryCorrection_*pose))
142  {
143  UERROR("Adding cloudOdom to viewer failed!");
144  }
145  }
146  else
147  {
148  cloudViewer_->setCloudVisibility("cloudOdom", false);
149  UWARN("Empty cloudOdom!");
150  }
151  }
152 
153  if(!odom.pose().isNull())
154  {
155  // update camera position
156  cloudViewer_->updateCameraTargetPosition(odometryCorrection_*odom.pose());
157  }
158  }
159  cloudViewer_->update();
160 
161  lastOdometryProcessed_ = true;
162  }
163 
164 
165  virtual void processStatistics(const rtabmap::Statistics & stats)
166  {
167  processingStatistics_ = true;
168 
169  //============================
170  // Add RGB-D clouds
171  //============================
172  const std::map<int, Transform> & poses = stats.poses();
173  QMap<std::string, Transform> clouds = cloudViewer_->getAddedClouds();
174  for(std::map<int, Transform>::const_iterator iter = poses.lower_bound(1); iter!=poses.end(); ++iter)
175  {
176  if(!iter->second.isNull())
177  {
178  std::string cloudName = uFormat("cloud%d", iter->first);
179 
180  // 3d point cloud
181  if(clouds.contains(cloudName))
182  {
183  // Update only if the pose has changed
184  Transform tCloud;
185  cloudViewer_->getPose(cloudName, tCloud);
186  if(tCloud.isNull() || iter->second != tCloud)
187  {
188  if(!cloudViewer_->updateCloudPose(cloudName, iter->second))
189  {
190  UERROR("Updating pose cloud %d failed!", iter->first);
191  }
192  }
193  cloudViewer_->setCloudVisibility(cloudName, true);
194  }
195  else if(iter->first == stats.getLastSignatureData().id())
196  {
197  Signature s = stats.getLastSignatureData();
198  s.sensorData().uncompressData(); // make sure data is uncompressed
199  // Add the new cloud
200  pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud = util3d::cloudRGBFromSensorData(
201  s.sensorData(),
202  4, // decimation
203  4.0f); // max depth
204  if(cloud->size())
205  {
206  if(!cloudViewer_->addCloud(cloudName, cloud, iter->second))
207  {
208  UERROR("Adding cloud %d to viewer failed!", iter->first);
209  }
210  }
211  else
212  {
213  UWARN("Empty cloud %d!", iter->first);
214  }
215  }
216  }
217  else
218  {
219  UWARN("Null pose for %d ?!?", iter->first);
220  }
221  }
222 
223  //============================
224  // Add 3D graph (show all poses)
225  //============================
226  cloudViewer_->removeAllGraphs();
227  cloudViewer_->removeCloud("graph_nodes");
228  if(poses.size())
229  {
230  // Set graph
231  pcl::PointCloud<pcl::PointXYZ>::Ptr graph(new pcl::PointCloud<pcl::PointXYZ>);
232  pcl::PointCloud<pcl::PointXYZ>::Ptr graphNodes(new pcl::PointCloud<pcl::PointXYZ>);
233  for(std::map<int, Transform>::const_iterator iter=poses.lower_bound(1); iter!=poses.end(); ++iter)
234  {
235  graph->push_back(pcl::PointXYZ(iter->second.x(), iter->second.y(), iter->second.z()));
236  }
237  *graphNodes = *graph;
238 
239 
240  // add graph
241  cloudViewer_->addOrUpdateGraph("graph", graph, Qt::gray);
242  cloudViewer_->addCloud("graph_nodes", graphNodes, Transform::getIdentity(), Qt::green);
243  cloudViewer_->setCloudPointSize("graph_nodes", 5);
244  }
245 
246  //============================
247  // Update/add occupancy grid (when RGBD/CreateOccupancyGrid is true)
248  //============================
249  if(grid_.addedNodes().find(stats.getLastSignatureData().id()) == grid_.addedNodes().end())
250  {
251  if(stats.getLastSignatureData().sensorData().gridCellSize() > 0.0f)
252  {
253  cv::Mat groundCells, obstacleCells, emptyCells;
254  stats.getLastSignatureData().sensorData().uncompressDataConst(0, 0, 0, 0, &groundCells, &obstacleCells, &emptyCells);
255  grid_.addToCache(stats.getLastSignatureData().id(), groundCells, obstacleCells, emptyCells);
256  }
257  }
258 
259  if(grid_.addedNodes().size() || grid_.cacheSize())
260  {
261  grid_.update(stats.poses());
262  }
263  if(grid_.addedNodes().size())
264  {
265  float xMin, yMin;
266  cv::Mat map8S = grid_.getMap(xMin, yMin);
267  if(!map8S.empty())
268  {
269  //convert to gray scaled map
270  cv::Mat map8U = util3d::convertMap2Image8U(map8S);
271  cloudViewer_->addOccupancyGridMap(map8U, grid_.getCellSize(), xMin, yMin, 0.75);
272  }
273  }
274 
275  odometryCorrection_ = stats.mapCorrection();
276 
277  cloudViewer_->update();
278 
279  processingStatistics_ = false;
280  }
281 
282  virtual bool handleEvent(UEvent * event)
283  {
284  if(event->getClassName().compare("RtabmapEvent") == 0)
285  {
286  RtabmapEvent * rtabmapEvent = (RtabmapEvent *)event;
287  const Statistics & stats = rtabmapEvent->getStats();
288  // Statistics must be processed in the Qt thread
289  if(this->isVisible())
290  {
291  QMetaObject::invokeMethod(this, "processStatistics", Q_ARG(rtabmap::Statistics, stats));
292  }
293  }
294  else if(event->getClassName().compare("OdometryEvent") == 0)
295  {
296  OdometryEvent * odomEvent = (OdometryEvent *)event;
297  // Odometry must be processed in the Qt thread
298  if(this->isVisible() &&
299  lastOdometryProcessed_ &&
300  !processingStatistics_)
301  {
302  lastOdometryProcessed_ = false; // if we receive too many odometry events!
303  QMetaObject::invokeMethod(this, "processOdometry", Q_ARG(rtabmap::OdometryEvent, *odomEvent));
304  }
305  }
306  return false;
307  }
308 
309 protected:
310  CloudViewer * cloudViewer_;
312  Transform lastOdomPose_;
313  Transform odometryCorrection_;
317 };
318 
319 
320 #endif /* MAPBUILDER_H_ */
SensorData & data()
Definition: OdometryEvent.h:69
void uncompressDataConst(cv::Mat *imageRaw, cv::Mat *depthOrRightRaw, LaserScan *laserScanRaw=0, cv::Mat *userDataRaw=0, cv::Mat *groundCellsRaw=0, cv::Mat *obstacleCellsRaw=0, cv::Mat *emptyCellsRaw=0) const
Definition: SensorData.cpp:579
virtual void pauseDetection()
Definition: UEvent.h:57
const Statistics & getStats() const
Definition: RtabmapEvent.h:50
const Signature & getLastSignatureData() const
Definition: Statistics.h:265
static Transform getIdentity()
Definition: Transform.cpp:380
CameraThread * camera_
float gridCellSize() const
Definition: SensorData.h:245
Some conversion functions.
MapBuilder(CameraThread *camera=0)
const cv::Mat & imageRaw() const
Definition: SensorData.h:164
Wrappers of STL for convenient functions.
const Transform & pose() const
Definition: OdometryEvent.h:71
#define true
Definition: ConvertUTF.c:57
bool isNull() const
Definition: Transform.cpp:107
virtual std::string getClassName() const =0
pcl::PointCloud< pcl::PointXYZRGB >::Ptr RTABMAP_EXP cloudRGBFromSensorData(const SensorData &sensorData, int decimation=1, float maxDepth=0.0f, float minDepth=0.0f, std::vector< int > *validIndices=0, const ParametersMap &stereoParameters=ParametersMap(), const std::vector< float > &roiRatios=std::vector< float >())
Definition: util3d.cpp:1031
virtual void processOdometry(const rtabmap::OdometryEvent &odom)
OccupancyGrid grid_
int id() const
Definition: Signature.h:70
const cv::Mat & depthOrRightRaw() const
Definition: SensorData.h:165
#define false
Definition: ConvertUTF.c:56
const std::map< int, Transform > & poses() const
Definition: Statistics.h:267
cv::Mat RTABMAP_EXP convertMap2Image8U(const cv::Mat &map8S, bool pgmFormat=false)
const std::vector< CameraModel > & cameraModels() const
Definition: SensorData.h:215
SensorData & sensorData()
Definition: Signature.h:137
#define UERROR(...)
ULogger class and convenient macros.
const StereoCameraModel & stereoCameraModel() const
Definition: SensorData.h:216
#define UWARN(...)
virtual void processStatistics(const rtabmap::Statistics &stats)
virtual bool handleEvent(UEvent *event)
const Transform & mapCorrection() const
Definition: Statistics.h:269
std::string UTILITE_EXP uFormat(const char *fmt,...)


rtabmap
Author(s): Mathieu Labbe
autogenerated on Mon Dec 14 2020 03:34:59