WifiMapping/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"
41 #endif
42 #include "rtabmap/utilite/UStl.h"
48 
49 using namespace rtabmap;
50 
51 // This class receives RtabmapEvent and construct/update a 3D Map
52 class MapBuilder : public QWidget, public UEventsHandler
53 {
54  Q_OBJECT
55 public:
56  //Camera ownership is not transferred!
57  MapBuilder(CameraThread * camera = 0) :
58  camera_(camera),
59  odometryCorrection_(Transform::getIdentity()),
60  processingStatistics_(false),
61  lastOdometryProcessed_(true)
62  {
63  this->setWindowFlags(Qt::Dialog);
64  this->setWindowTitle(tr("3D Map"));
65  this->setMinimumWidth(800);
66  this->setMinimumHeight(600);
67 
68  cloudViewer_ = new CloudViewer(this);
69 
70  QVBoxLayout *layout = new QVBoxLayout();
71  layout->addWidget(cloudViewer_);
72  this->setLayout(layout);
73 
74  qRegisterMetaType<rtabmap::OdometryEvent>("rtabmap::OdometryEvent");
75  qRegisterMetaType<rtabmap::Statistics>("rtabmap::Statistics");
76 
77  QAction * pause = new QAction(this);
78  this->addAction(pause);
79  pause->setShortcut(Qt::Key_Space);
80  connect(pause, SIGNAL(triggered()), this, SLOT(pauseDetection()));
81  }
82 
83  virtual ~MapBuilder()
84  {
85  this->unregisterFromEventsManager();
86  }
87 
88 protected Q_SLOTS:
89  virtual void pauseDetection()
90  {
91  UWARN("");
92  if(camera_)
93  {
94  if(camera_->isCapturing())
95  {
96  camera_->join(true);
97  }
98  else
99  {
100  camera_->start();
101  }
102  }
103  }
104 
105  virtual void processOdometry(const rtabmap::OdometryEvent & odom)
106  {
107  if(!this->isVisible())
108  {
109  return;
110  }
111 
112  Transform pose = odom.pose();
113  if(pose.isNull())
114  {
115  //Odometry lost
116  cloudViewer_->setBackgroundColor(Qt::darkRed);
117 
118  pose = lastOdomPose_;
119  }
120  else
121  {
122  cloudViewer_->setBackgroundColor(cloudViewer_->getDefaultBackgroundColor());
123  }
124  if(!pose.isNull())
125  {
126  lastOdomPose_ = pose;
127 
128  // 3d cloud
129  if(odom.data().depthOrRightRaw().cols == odom.data().imageRaw().cols &&
130  odom.data().depthOrRightRaw().rows == odom.data().imageRaw().rows &&
131  !odom.data().depthOrRightRaw().empty() &&
132  (odom.data().stereoCameraModel().isValidForProjection() || odom.data().cameraModels().size()))
133  {
134  pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud = util3d::cloudRGBFromSensorData(
135  odom.data(),
136  2, // decimation
137  4.0f); // max depth
138  if(cloud->size())
139  {
140  if(!cloudViewer_->addCloud("cloudOdom", cloud, odometryCorrection_*pose))
141  {
142  UERROR("Adding cloudOdom to viewer failed!");
143  }
144  }
145  else
146  {
147  cloudViewer_->setCloudVisibility("cloudOdom", false);
148  UWARN("Empty cloudOdom!");
149  }
150  }
151 
152  if(!odom.pose().isNull())
153  {
154  // update camera position
155  cloudViewer_->updateCameraTargetPosition(odometryCorrection_*odom.pose());
156  }
157  }
158  cloudViewer_->update();
159 
160  lastOdometryProcessed_ = true;
161  }
162 
163 
164  virtual void processStatistics(const rtabmap::Statistics & stats)
165  {
166  processingStatistics_ = true;
167 
168  //============================
169  // Add RGB-D clouds
170  //============================
171  const std::map<int, Transform> & poses = stats.poses();
172  QMap<std::string, Transform> clouds = cloudViewer_->getAddedClouds();
173  for(std::map<int, Transform>::const_iterator iter = poses.lower_bound(1); iter!=poses.end(); ++iter)
174  {
175  if(!iter->second.isNull())
176  {
177  std::string cloudName = uFormat("cloud%d", iter->first);
178 
179  // 3d point cloud
180  if(clouds.contains(cloudName))
181  {
182  // Update only if the pose has changed
183  Transform tCloud;
184  cloudViewer_->getPose(cloudName, tCloud);
185  if(tCloud.isNull() || iter->second != tCloud)
186  {
187  if(!cloudViewer_->updateCloudPose(cloudName, iter->second))
188  {
189  UERROR("Updating pose cloud %d failed!", iter->first);
190  }
191  }
192  cloudViewer_->setCloudVisibility(cloudName, true);
193  }
194  else if(iter->first == stats.getLastSignatureData().id())
195  {
196  Signature s = stats.getLastSignatureData();
197  s.sensorData().uncompressData(); // make sure data is uncompressed
198  // Add the new cloud
199  pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud = util3d::cloudRGBFromSensorData(
200  s.sensorData(),
201  4, // decimation
202  4.0f); // max depth
203  if(cloud->size())
204  {
205  if(!cloudViewer_->addCloud(cloudName, cloud, iter->second))
206  {
207  UERROR("Adding cloud %d to viewer failed!", iter->first);
208  }
209  }
210  else
211  {
212  UWARN("Empty cloud %d!", iter->first);
213  }
214  }
215  }
216  else
217  {
218  UWARN("Null pose for %d ?!?", iter->first);
219  }
220  }
221 
222  //============================
223  // Add 3D graph (show all poses)
224  //============================
225  cloudViewer_->removeAllGraphs();
226  cloudViewer_->removeCloud("graph_nodes");
227  if(poses.size())
228  {
229  // Set graph
230  pcl::PointCloud<pcl::PointXYZ>::Ptr graph(new pcl::PointCloud<pcl::PointXYZ>);
231  pcl::PointCloud<pcl::PointXYZ>::Ptr graphNodes(new pcl::PointCloud<pcl::PointXYZ>);
232  for(std::map<int, Transform>::const_iterator iter=poses.lower_bound(1); iter!=poses.end(); ++iter)
233  {
234  graph->push_back(pcl::PointXYZ(iter->second.x(), iter->second.y(), iter->second.z()));
235  }
236  *graphNodes = *graph;
237 
238 
239  // add graph
240  cloudViewer_->addOrUpdateGraph("graph", graph, Qt::gray);
241  cloudViewer_->addCloud("graph_nodes", graphNodes, Transform::getIdentity(), Qt::green);
242  cloudViewer_->setCloudPointSize("graph_nodes", 5);
243  }
244 
245  odometryCorrection_ = stats.mapCorrection();
246 
247  cloudViewer_->update();
248 
249  processingStatistics_ = false;
250  }
251 
252  virtual bool handleEvent(UEvent * event)
253  {
254  if(event->getClassName().compare("RtabmapEvent") == 0)
255  {
256  RtabmapEvent * rtabmapEvent = (RtabmapEvent *)event;
257  const Statistics & stats = rtabmapEvent->getStats();
258  // Statistics must be processed in the Qt thread
259  if(this->isVisible())
260  {
261  QMetaObject::invokeMethod(this, "processStatistics", Q_ARG(rtabmap::Statistics, stats));
262  }
263  }
264  else if(event->getClassName().compare("OdometryEvent") == 0)
265  {
266  OdometryEvent * odomEvent = (OdometryEvent *)event;
267  // Odometry must be processed in the Qt thread
268  if(this->isVisible() &&
269  lastOdometryProcessed_ &&
270  !processingStatistics_)
271  {
272  lastOdometryProcessed_ = false; // if we receive too many odometry events!
273  QMetaObject::invokeMethod(this, "processOdometry", Q_ARG(rtabmap::OdometryEvent, *odomEvent));
274  }
275  }
276  return false;
277  }
278 
279 protected:
280  CloudViewer * cloudViewer_;
281  CameraThread * camera_;
282  Transform lastOdomPose_;
283  Transform odometryCorrection_;
284  bool processingStatistics_;
285  bool lastOdometryProcessed_;
286 };
287 
288 
289 #endif /* MAPBUILDER_H_ */
SensorData & data()
Definition: OdometryEvent.h:69
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
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)
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
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