00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028 #include "rtabmap/gui/CameraViewer.h"
00029
00030 #include <rtabmap/core/CameraEvent.h>
00031 #include <rtabmap/core/util3d.h>
00032 #include <rtabmap/core/util2d.h>
00033 #include <rtabmap/core/util3d_filtering.h>
00034 #include <rtabmap/gui/ImageView.h>
00035 #include <rtabmap/gui/CloudViewer.h>
00036 #include <rtabmap/utilite/UCv2Qt.h>
00037 #include <rtabmap/utilite/ULogger.h>
00038 #include <QtCore/QMetaType>
00039 #include <QHBoxLayout>
00040 #include <QVBoxLayout>
00041 #include <QLabel>
00042 #include <QSpinBox>
00043 #include <QDialogButtonBox>
00044 #include <QCheckBox>
00045 #include <QPushButton>
00046
00047 namespace rtabmap {
00048
00049
00050 CameraViewer::CameraViewer(QWidget * parent, const ParametersMap & parameters) :
00051 QDialog(parent),
00052 imageView_(new ImageView(this)),
00053 cloudView_(new CloudViewer(this)),
00054 processingImages_(false),
00055 validDecimationValue_(1),
00056 parameters_(parameters)
00057 {
00058 qRegisterMetaType<rtabmap::SensorData>("rtabmap::SensorData");
00059
00060 imageView_->setImageDepthShown(true);
00061 imageView_->setMinimumSize(320, 240);
00062 QHBoxLayout * layout = new QHBoxLayout();
00063 layout->setMargin(0);
00064 layout->addWidget(imageView_,1);
00065 layout->addWidget(cloudView_,1);
00066
00067 QLabel * decimationLabel = new QLabel("Decimation", this);
00068 decimationSpin_ = new QSpinBox(this);
00069 decimationSpin_->setMinimum(1);
00070 decimationSpin_->setMaximum(16);
00071 decimationSpin_->setValue(1);
00072
00073 pause_ = new QPushButton("Pause", this);
00074 pause_->setCheckable(true);
00075 showCloudCheckbox_ = new QCheckBox("Show RGB-D cloud", this);
00076 showCloudCheckbox_->setEnabled(false);
00077 showCloudCheckbox_->setChecked(true);
00078 showScanCheckbox_ = new QCheckBox("Show scan", this);
00079 showScanCheckbox_->setEnabled(false);
00080
00081 QDialogButtonBox * buttonBox = new QDialogButtonBox(this);
00082 buttonBox->setStandardButtons(QDialogButtonBox::Close);
00083 connect(buttonBox, SIGNAL(rejected()), this, SLOT(reject()));
00084
00085 QHBoxLayout * layout2 = new QHBoxLayout();
00086 layout2->addWidget(pause_);
00087 layout2->addWidget(decimationLabel);
00088 layout2->addWidget(decimationSpin_);
00089 layout2->addWidget(showCloudCheckbox_);
00090 layout2->addWidget(showScanCheckbox_);
00091 layout2->addStretch(1);
00092 layout2->addWidget(buttonBox);
00093
00094 QVBoxLayout * vlayout = new QVBoxLayout(this);
00095 vlayout->setMargin(0);
00096 vlayout->setSpacing(0);
00097 vlayout->addLayout(layout, 1);
00098 vlayout->addLayout(layout2);
00099
00100 this->setLayout(vlayout);
00101 }
00102
00103 CameraViewer::~CameraViewer()
00104 {
00105 this->unregisterFromEventsManager();
00106 }
00107
00108 void CameraViewer::showImage(const rtabmap::SensorData & data)
00109 {
00110 if(!data.imageRaw().empty() || !data.depthOrRightRaw().empty())
00111 {
00112 if(data.imageRaw().cols % decimationSpin_->value() == 0 &&
00113 data.imageRaw().rows % decimationSpin_->value() == 0 &&
00114 data.depthOrRightRaw().cols % decimationSpin_->value() == 0 &&
00115 data.depthOrRightRaw().rows % decimationSpin_->value() == 0)
00116 {
00117 validDecimationValue_ = decimationSpin_->value();
00118 }
00119 else
00120 {
00121 UWARN("Decimation (%d) must be a denominator of the width and height of "
00122 "the image (color=%d/%d depth=%d/%d). Using last valid decimation value (%d).",
00123 decimationSpin_->value(),
00124 data.imageRaw().cols,
00125 data.imageRaw().rows,
00126 data.depthOrRightRaw().cols,
00127 data.depthOrRightRaw().rows,
00128 validDecimationValue_);
00129 }
00130 }
00131
00132 processingImages_ = true;
00133 if(!data.imageRaw().empty())
00134 {
00135 imageView_->setImage(uCvMat2QImage(util2d::decimate(data.imageRaw(), validDecimationValue_)));
00136 }
00137 if(!data.depthOrRightRaw().empty())
00138 {
00139 imageView_->setImageDepth(uCvMat2QImage(util2d::decimate(data.depthOrRightRaw(), validDecimationValue_)));
00140 }
00141
00142 if(!data.depthOrRightRaw().empty() &&
00143 (data.stereoCameraModel().isValidForProjection() || (data.cameraModels().size() && data.cameraModels().at(0).isValidForProjection())))
00144 {
00145 if(showCloudCheckbox_->isChecked())
00146 {
00147 if(!data.imageRaw().empty() && !data.depthOrRightRaw().empty())
00148 {
00149 showCloudCheckbox_->setEnabled(true);
00150 cloudView_->addCloud("cloud", util3d::cloudRGBFromSensorData(data, validDecimationValue_, 0, 0, 0, parameters_));
00151 }
00152 else if(!data.depthOrRightRaw().empty())
00153 {
00154 showCloudCheckbox_->setEnabled(true);
00155 cloudView_->addCloud("cloud", util3d::cloudFromSensorData(data, validDecimationValue_, 0, 0, 0, parameters_));
00156 }
00157 }
00158 }
00159
00160 if(!data.laserScanRaw().empty())
00161 {
00162 showScanCheckbox_->setEnabled(true);
00163 if(showScanCheckbox_->isChecked())
00164 {
00165 cloudView_->addCloud("scan", util3d::downsample(util3d::laserScanToPointCloud(data.laserScanRaw()), validDecimationValue_), Transform::getIdentity(), Qt::yellow);
00166 }
00167 }
00168
00169 cloudView_->setVisible((showCloudCheckbox_->isEnabled() && showCloudCheckbox_->isChecked()) ||
00170 (showScanCheckbox_->isEnabled() && showScanCheckbox_->isChecked()));
00171 if(cloudView_->isVisible())
00172 {
00173 cloudView_->update();
00174 }
00175 if(cloudView_->getAddedClouds().contains("cloud"))
00176 {
00177 cloudView_->setCloudVisibility("cloud", showCloudCheckbox_->isChecked());
00178 }
00179 if(cloudView_->getAddedClouds().contains("scan"))
00180 {
00181 cloudView_->setCloudVisibility("scan", showScanCheckbox_->isChecked());
00182 }
00183
00184 processingImages_ = false;
00185 }
00186
00187 void CameraViewer::handleEvent(UEvent * event)
00188 {
00189 if(!pause_->isChecked())
00190 {
00191 if(event->getClassName().compare("CameraEvent") == 0)
00192 {
00193 CameraEvent * camEvent = (CameraEvent*)event;
00194 if(camEvent->getCode() == CameraEvent::kCodeData)
00195 {
00196 if(camEvent->data().isValid())
00197 {
00198 if(!processingImages_ && this->isVisible() && camEvent->data().isValid())
00199 {
00200 processingImages_ = true;
00201 QMetaObject::invokeMethod(this, "showImage",
00202 Q_ARG(rtabmap::SensorData, camEvent->data()));
00203 }
00204 }
00205 }
00206 }
00207 }
00208 }
00209
00210 }