CameraViewer.cpp
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 #include <rtabmap/core/SensorEvent.h>
29 #include "rtabmap/gui/CameraViewer.h"
30 
31 #include <rtabmap/core/util3d.h>
32 #include <rtabmap/core/util2d.h>
33 #include <rtabmap/core/util3d_filtering.h>
34 #include <rtabmap/core/MarkerDetector.h>
35 #include <rtabmap/gui/ImageView.h>
36 #include <rtabmap/gui/CloudViewer.h>
37 #include <rtabmap/utilite/UCv2Qt.h>
38 #include <rtabmap/utilite/ULogger.h>
39 #include <QtCore/QMetaType>
40 #include <QHBoxLayout>
41 #include <QVBoxLayout>
42 #include <QLabel>
43 #include <QSpinBox>
44 #include <QDialogButtonBox>
45 #include <QCheckBox>
46 #include <QPushButton>
47 
48 namespace rtabmap {
49 
50 
51 CameraViewer::CameraViewer(QWidget * parent, const ParametersMap & parameters) :
52  QDialog(parent),
53  imageView_(new ImageView(this)),
54  cloudView_(new CloudViewer(this)),
55  processingImages_(false),
56  parameters_(parameters),
57  markerDetector_(0)
58 {
59  qRegisterMetaType<rtabmap::SensorData>("rtabmap::SensorData");
60 
61  imageView_->setImageDepthShown(true);
62  imageView_->setMinimumSize(320, 240);
63  imageView_->setVisible(false);
64  QHBoxLayout * layout = new QHBoxLayout();
65  layout->setContentsMargins(0,0,0,0);
66  layout->addWidget(imageView_,1);
67  layout->addWidget(cloudView_,1);
68 
69  QLabel * decimationLabel = new QLabel("Decimation", this);
70  decimationSpin_ = new QSpinBox(this);
71  decimationSpin_->setMinimum(-16);
72  decimationSpin_->setMaximum(16);
73  decimationSpin_->setValue(2);
74 
75  pause_ = new QPushButton("Pause", this);
76  pause_->setCheckable(true);
77  showCloudCheckbox_ = new QCheckBox("Show RGB-D cloud", this);
78  showCloudCheckbox_->setEnabled(false);
79  showCloudCheckbox_->setChecked(true);
80  showScanCheckbox_ = new QCheckBox("Show scan", this);
81  showScanCheckbox_->setEnabled(false);
82  showScanCheckbox_->setChecked(true);
83 
84  markerCheckbox_ = new QCheckBox("Detect markers", this);
85 #ifdef HAVE_OPENCV_ARUCO
86  markerCheckbox_->setEnabled(true);
87  markerDetector_ = new MarkerDetector(parameters);
88 #else
89  markerCheckbox_->setEnabled(false);
90  markerCheckbox_->setToolTip("Disabled: RTAB-Map is not built with OpenCV's aruco module.");
91 #endif
92  markerCheckbox_->setChecked(false);
93 
94  imageSizeLabel_ = new QLabel(this);
95 
96  QDialogButtonBox * buttonBox = new QDialogButtonBox(this);
97  buttonBox->setStandardButtons(QDialogButtonBox::Close);
98  connect(buttonBox, SIGNAL(rejected()), this, SLOT(reject()));
99 
100  QHBoxLayout * layout2 = new QHBoxLayout();
101  layout2->addWidget(pause_);
102  layout2->addWidget(decimationLabel);
103  layout2->addWidget(decimationSpin_);
104  layout2->addWidget(showCloudCheckbox_);
105  layout2->addWidget(showScanCheckbox_);
106  layout2->addWidget(markerCheckbox_);
107  layout2->addWidget(imageSizeLabel_);
108  layout2->addStretch(1);
109  layout2->addWidget(buttonBox);
110 
111  QVBoxLayout * vlayout = new QVBoxLayout(this);
112  vlayout->setContentsMargins(0,0,0,0);
113  vlayout->setSpacing(0);
114  vlayout->addLayout(layout, 1);
115  vlayout->addLayout(layout2);
116 
117  this->setLayout(vlayout);
118 }
119 
120 CameraViewer::~CameraViewer()
121 {
122  this->unregisterFromEventsManager();
123  delete markerDetector_;
124 }
125 
126 void CameraViewer::setDecimation(int value)
127 {
128  decimationSpin_->setValue(value);
129 }
130 
131 void CameraViewer::showImage(const rtabmap::SensorData & data)
132 {
133  processingImages_ = true;
134  QString sizes;
135  imageView_->setVisible(!data.imageRaw().empty() || !data.imageRaw().empty());
136  std::map<int, MarkerInfo> detections;
137  if(!data.imageRaw().empty())
138  {
139  std::vector<CameraModel> models;
140  if(markerCheckbox_->isEnabled() && markerCheckbox_->isChecked())
141  {
142  models = data.cameraModels();
143  if(models.empty())
144  {
145  for(size_t i=0; i<data.stereoCameraModels().size(); ++i)
146  {
147  models.push_back(data.stereoCameraModels()[i].left());
148  }
149  }
150  }
151 
152  if(!models.empty() && models[0].isValidForProjection())
153  {
154  cv::Mat imageWithDetections;
155  detections = markerDetector_->detect(data.imageRaw(), models, data.depthRaw(), std::map<int, float>(), &imageWithDetections);
156  imageView_->setImage(uCvMat2QImage(imageWithDetections));
157  }
158  else
159  {
160  imageView_->setImage(uCvMat2QImage(data.imageRaw()));
161  }
162  sizes.append(QString("Color=%1x%2").arg(data.imageRaw().cols).arg(data.imageRaw().rows));
163  }
164  if(!data.depthOrRightRaw().empty())
165  {
166  imageView_->setImageDepth(data.depthOrRightRaw());
167  sizes.append(QString(" Depth=%1x%2").arg(data.depthOrRightRaw().cols).arg(data.depthOrRightRaw().rows));
168  }
169  imageSizeLabel_->setText(sizes);
170 
171  if(!data.depthOrRightRaw().empty() &&
172  ((data.stereoCameraModels().size() && data.stereoCameraModels()[0].isValidForProjection()) || (data.cameraModels().size() && data.cameraModels().at(0).isValidForProjection())))
173  {
174  if(showCloudCheckbox_->isChecked())
175  {
176  if(!data.imageRaw().empty() && !data.depthOrRightRaw().empty())
177  {
178  showCloudCheckbox_->setEnabled(true);
179  cloudView_->addCloud("cloud", util3d::cloudRGBFromSensorData(data, decimationSpin_->value()!=0?decimationSpin_->value():1, 0, 0, 0, parameters_));
180  }
181  else if(!data.depthOrRightRaw().empty())
182  {
183  showCloudCheckbox_->setEnabled(true);
184  cloudView_->addCloud("cloud", util3d::cloudFromSensorData(data, decimationSpin_->value()!=0?fabs(decimationSpin_->value()):1, 0, 0, 0, parameters_));
185  }
186 
187  // Add landmarks to 3D Map view
188 #if PCL_VERSION_COMPARE(>=, 1, 7, 2)
189  cloudView_->removeAllCoordinates("landmark_");
190 #endif
191  cloudView_->removeAllTexts();
192  if(!detections.empty())
193  {
194  for(std::map<int, MarkerInfo>::const_iterator iter=detections.begin(); iter!=detections.end(); ++iter)
195  {
196 #if PCL_VERSION_COMPARE(>=, 1, 7, 2)
197  cloudView_->addOrUpdateCoordinate(uFormat("landmark_%d", iter->first), iter->second.pose(), iter->second.length(), false);
198 #endif
199  std::string num = uNumber2Str(iter->first);
200  cloudView_->addOrUpdateText(
201  std::string("landmark_str_") + num,
202  num,
203  iter->second.pose(),
204  0.05,
205  Qt::yellow);
206  }
207  }
208  }
209  }
210 
211  if(!data.laserScanRaw().isEmpty())
212  {
213  showScanCheckbox_->setEnabled(true);
214  if(showScanCheckbox_->isChecked())
215  {
216  if(data.laserScanRaw().hasNormals())
217  {
218  if(data.laserScanRaw().hasIntensity())
219  {
220  cloudView_->addCloud("scan", util3d::downsample(util3d::laserScanToPointCloudINormal(data.laserScanRaw()), decimationSpin_->value()!=0?fabs(decimationSpin_->value()):1), data.laserScanRaw().localTransform(), Qt::yellow);
221  }
222  else if(data.laserScanRaw().hasRGB())
223  {
224  cloudView_->addCloud("scan", util3d::downsample(util3d::laserScanToPointCloudRGBNormal(data.laserScanRaw()), decimationSpin_->value()!=0?fabs(decimationSpin_->value()):1), data.laserScanRaw().localTransform(), Qt::yellow);
225  }
226  else
227  {
228  cloudView_->addCloud("scan", util3d::downsample(util3d::laserScanToPointCloudNormal(data.laserScanRaw()), decimationSpin_->value()!=0?fabs(decimationSpin_->value()):1), data.laserScanRaw().localTransform(), Qt::yellow);
229  }
230  }
231  else if(data.laserScanRaw().hasIntensity())
232  {
233  cloudView_->addCloud("scan", util3d::downsample(util3d::laserScanToPointCloudI(data.laserScanRaw()), decimationSpin_->value()!=0?fabs(decimationSpin_->value()):1), data.laserScanRaw().localTransform(), Qt::yellow);
234  }
235  else if(data.laserScanRaw().hasRGB())
236  {
237  cloudView_->addCloud("scan", util3d::downsample(util3d::laserScanToPointCloudRGB(data.laserScanRaw()), decimationSpin_->value()!=0?fabs(decimationSpin_->value()):1), data.laserScanRaw().localTransform(), Qt::yellow);
238  }
239  else
240  {
241  cloudView_->addCloud("scan", util3d::downsample(util3d::laserScanToPointCloud(data.laserScanRaw()), decimationSpin_->value()!=0?fabs(decimationSpin_->value()):1), data.laserScanRaw().localTransform(), Qt::yellow);
242  }
243  }
244  }
245 
246  cloudView_->setVisible((showCloudCheckbox_->isEnabled() && showCloudCheckbox_->isChecked()) ||
247  (showScanCheckbox_->isEnabled() && showScanCheckbox_->isChecked()));
248  if(cloudView_->isVisible())
249  {
250  cloudView_->refreshView();
251  }
252  if(cloudView_->getAddedClouds().contains("cloud"))
253  {
254  cloudView_->setCloudVisibility("cloud", showCloudCheckbox_->isChecked());
255  }
256  if(cloudView_->getAddedClouds().contains("scan"))
257  {
258  cloudView_->setCloudVisibility("scan", showScanCheckbox_->isChecked());
259  }
260 
261  processingImages_ = false;
262 }
263 
264 bool CameraViewer::handleEvent(UEvent * event)
265 {
266  if(!pause_->isChecked())
267  {
268  if(event->getClassName().compare("SensorEvent") == 0)
269  {
270  SensorEvent * camEvent = (SensorEvent*)event;
271  if(camEvent->getCode() == SensorEvent::kCodeData)
272  {
273  if(camEvent->data().isValid())
274  {
275  if(!processingImages_ && this->isVisible() && camEvent->data().isValid())
276  {
277  processingImages_ = true;
278  QMetaObject::invokeMethod(this, "showImage",
279  Q_ARG(rtabmap::SensorData, camEvent->data()));
280  }
281  }
282  }
283  }
284  }
285  return false;
286 }
287 
288 } /* namespace rtabmap */
rtabmap::SensorData
Definition: SensorData.h:51
rtabmap::util3d::laserScanToPointCloudINormal
pcl::PointCloud< pcl::PointXYZINormal >::Ptr RTABMAP_CORE_EXPORT laserScanToPointCloudINormal(const LaserScan &laserScan, const Transform &transform=Transform(), float intensity=0.0f)
Definition: util3d.cpp:2402
rtabmap::CloudViewer::removeAllCoordinates
void removeAllCoordinates(const std::string &prefix="")
Definition: CloudViewer.cpp:1797
rtabmap::CameraViewer::imageView_
ImageView * imageView_
Definition: CameraViewer.h:65
ImageView.h
rtabmap::MarkerDetector::detect
RTABMAP_DEPRECATED MapIdPose detect(const cv::Mat &image, const CameraModel &model, const cv::Mat &depth=cv::Mat(), float *estimatedMarkerLength=0, cv::Mat *imageWithDetections=0)
Definition: MarkerDetector.cpp:126
rtabmap::CameraViewer::processingImages_
bool processingImages_
Definition: CameraViewer.h:67
rtabmap::ImageView::setImageDepthShown
void setImageDepthShown(bool shown)
Definition: ImageView.cpp:466
arg::arg
constexpr arg(const char *name=nullptr)
rtabmap::CloudViewer::addCloud
bool addCloud(const std::string &id, const pcl::PCLPointCloud2Ptr &binaryCloud, const Transform &pose, bool rgb, bool hasNormals, bool hasIntensity, const QColor &color=QColor(), int viewport=1)
Definition: CloudViewer.cpp:745
size
Index size
this
this
rtabmap::CameraViewer::showScanCheckbox_
QCheckBox * showScanCheckbox_
Definition: CameraViewer.h:73
rtabmap::SensorData::isValid
bool isValid() const
Definition: SensorData.h:156
rtabmap::CloudViewer::removeAllTexts
void removeAllTexts()
Definition: CloudViewer.cpp:2405
CameraViewer.h
sizes
std::vector< Array2i > sizes
MarkerDetector.h
UEvent::getCode
int getCode() const
Definition: UEvent.h:74
util3d.h
rtabmap::ParametersMap
std::map< std::string, std::string > ParametersMap
Definition: Parameters.h:43
fabs
Real fabs(const Real &a)
UEvent
Definition: UEvent.h:57
rtabmap::CameraViewer::cloudView_
CloudViewer * cloudView_
Definition: CameraViewer.h:66
rtabmap::CloudViewer
Definition: CloudViewer.h:79
rtabmap::CameraViewer::pause_
QPushButton * pause_
Definition: CameraViewer.h:70
rtabmap::CameraViewer::markerDetector_
MarkerDetector * markerDetector_
Definition: CameraViewer.h:75
rtabmap::util3d::cloudRGBFromSensorData
pcl::PointCloud< pcl::PointXYZRGB >::Ptr RTABMAP_CORE_EXPORT 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:1268
rtabmap::ImageView::setImage
void setImage(const QImage &image, const std::vector< CameraModel > &models=std::vector< CameraModel >(), const Transform &pose=Transform())
Definition: ImageView.cpp:1235
data
int data[]
UEvent::getClassName
virtual std::string getClassName() const =0
uNumber2Str
std::string UTILITE_EXPORT uNumber2Str(unsigned int number)
Definition: UConversion.cpp:91
rtabmap::CameraViewer::decimationSpin_
QSpinBox * decimationSpin_
Definition: CameraViewer.h:68
CloudViewer.h
util3d_filtering.h
rtabmap::util3d::laserScanToPointCloudRGB
pcl::PointCloud< pcl::PointXYZRGB >::Ptr RTABMAP_CORE_EXPORT laserScanToPointCloudRGB(const LaserScan &laserScan, const Transform &transform=Transform(), unsigned char r=100, unsigned char g=100, unsigned char b=100)
Definition: util3d.cpp:2322
rtabmap::CameraViewer::~CameraViewer
virtual ~CameraViewer()
Definition: CameraViewer.cpp:120
arg
rtabmap::CloudViewer::addOrUpdateCoordinate
void addOrUpdateCoordinate(const std::string &id, const Transform &transform, double scale, bool foreground=false)
Definition: CloudViewer.cpp:1735
UCv2Qt.h
rtabmap::CameraViewer::CameraViewer
CameraViewer(QWidget *parent=0, const ParametersMap &parameters=ParametersMap())
Definition: CameraViewer.cpp:51
rtabmap::SensorEvent::kCodeData
@ kCodeData
Definition: SensorEvent.h:42
rtabmap::CameraViewer::showCloudCheckbox_
QCheckBox * showCloudCheckbox_
Definition: CameraViewer.h:72
SensorEvent.h
rtabmap::util3d::downsample
LaserScan RTABMAP_CORE_EXPORT downsample(const LaserScan &cloud, int step)
Definition: util3d_filtering.cpp:398
UEventsHandler::unregisterFromEventsManager
void unregisterFromEventsManager()
Definition: UEventsHandler.cpp:33
rtabmap::CloudViewer::setCloudVisibility
void setCloudVisibility(const std::string &id, bool isVisible)
Definition: CloudViewer.cpp:3181
rtabmap::MarkerDetector
Definition: MarkerDetector.h:59
rtabmap::util3d::laserScanToPointCloudI
pcl::PointCloud< pcl::PointXYZI >::Ptr RTABMAP_CORE_EXPORT laserScanToPointCloudI(const LaserScan &laserScan, const Transform &transform=Transform(), float intensity=0.0f)
Definition: util3d.cpp:2349
rtabmap::CameraViewer::handleEvent
virtual bool handleEvent(UEvent *event)
Definition: CameraViewer.cpp:264
rtabmap::CameraViewer::imageSizeLabel_
QLabel * imageSizeLabel_
Definition: CameraViewer.h:71
uFormat
std::string UTILITE_EXPORT uFormat(const char *fmt,...)
Definition: UConversion.cpp:365
rtabmap::CloudViewer::getAddedClouds
const QMap< std::string, Transform > & getAddedClouds() const
Definition: CloudViewer.h:338
rtabmap::ImageView::setImageDepth
void setImageDepth(const cv::Mat &imageDepth)
Definition: ImageView.cpp:1264
ULogger.h
ULogger class and convenient macros.
rtabmap::util3d::cloudFromSensorData
pcl::PointCloud< pcl::PointXYZ >::Ptr RTABMAP_CORE_EXPORT cloudFromSensorData(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:1040
util2d.h
rtabmap::SensorEvent::data
const SensorData & data() const
Definition: SensorEvent.h:79
rtabmap::CameraViewer::parameters_
ParametersMap parameters_
Definition: CameraViewer.h:69
iter
iterator iter(handle obj)
rtabmap::CameraViewer::setDecimation
void setDecimation(int value)
Definition: CameraViewer.cpp:126
rtabmap::CameraViewer::showImage
void showImage(const rtabmap::SensorData &data)
Definition: CameraViewer.cpp:131
rtabmap::ImageView
Definition: ImageView.h:49
rtabmap::util3d::laserScanToPointCloud
pcl::PointCloud< pcl::PointXYZ >::Ptr RTABMAP_CORE_EXPORT laserScanToPointCloud(const LaserScan &laserScan, const Transform &transform=Transform())
Definition: util3d.cpp:2269
false
#define false
Definition: ConvertUTF.c:56
uCvMat2QImage
QImage uCvMat2QImage(const cv::Mat &image, bool isBgr=true, uCvQtDepthColorMap colorMap=uCvQtDepthWhiteToBlack, float depthMin=0, float depthMax=0)
Definition: UCv2Qt.h:47
rtabmap::CloudViewer::refreshView
void refreshView()
Definition: CloudViewer.cpp:538
rtabmap::CloudViewer::addOrUpdateText
void addOrUpdateText(const std::string &id, const std::string &text, const Transform &position, double scale, const QColor &color, bool foreground=true)
Definition: CloudViewer.cpp:2359
rtabmap::util3d::laserScanToPointCloudRGBNormal
pcl::PointCloud< pcl::PointXYZRGBNormal >::Ptr RTABMAP_CORE_EXPORT laserScanToPointCloudRGBNormal(const LaserScan &laserScan, const Transform &transform=Transform(), unsigned char r=100, unsigned char g=100, unsigned char b=100)
Definition: util3d.cpp:2376
rtabmap::CameraViewer::markerCheckbox_
QCheckBox * markerCheckbox_
Definition: CameraViewer.h:74
rtabmap
Definition: CameraARCore.cpp:35
rtabmap::util3d::laserScanToPointCloudNormal
pcl::PointCloud< pcl::PointNormal >::Ptr RTABMAP_CORE_EXPORT laserScanToPointCloudNormal(const LaserScan &laserScan, const Transform &transform=Transform())
Definition: util3d.cpp:2296
value
value
i
int i
rtabmap::SensorEvent
Definition: SensorEvent.h:37

rtabmap
Author(s): Mathieu Labbe
autogenerated on Thu Jul 25 2024 02:50:07