From 880879cb3d8d476e7d5dfaf88f1a48ff5ca97b6c Mon Sep 17 00:00:00 2001 From: pixhawk Date: Mon, 12 Dec 2011 18:48:16 +0100 Subject: [PATCH] Added widgets to display RGBD data. --- qgroundcontrol.pri | 10 +- qgroundcontrol.pro | 6 +- src/comm/MAVLinkProtocol.cc | 1 + src/uas/UAS.cc | 2 + src/uas/UAS.h | 4 + src/ui/HUD.cc | 4 + src/ui/HUD.h | 1 + src/ui/MainWindow.cc | 31 +++- src/ui/MainWindow.h | 2 + src/ui/QGCRGBDView.cc | 269 ++++++++++++++++++++++++++++++++ src/ui/QGCRGBDView.h | 29 ++++ src/ui/map3D/Pixhawk3DWidget.cc | 63 ++++---- 12 files changed, 382 insertions(+), 40 deletions(-) create mode 100644 src/ui/QGCRGBDView.cc create mode 100644 src/ui/QGCRGBDView.h diff --git a/qgroundcontrol.pri b/qgroundcontrol.pri index 002f18fc8..774a185be 100644 --- a/qgroundcontrol.pri +++ b/qgroundcontrol.pri @@ -142,7 +142,7 @@ linux-g++ { #CONFIG -= console } - QMAKE_POST_LINK += cp -rf $$BASEDIR/audio $$DESTDIR/. + #QMAKE_POST_LINK += cp -rf $$BASEDIR/audio $$DESTDIR/. message("Compiling for linux 32") @@ -172,10 +172,11 @@ message("Compiling for linux 32") -losgViewer \ -losgGA \ -losgDB \ - -losgQt \ -losgText \ -lOpenThreads + #-losgQt \ + DEFINES += QGC_OSG_ENABLED } @@ -225,7 +226,7 @@ linux-g++-64 { #CONFIG -= console } - QMAKE_POST_LINK += cp -rf $$BASEDIR/audio $$DESTDIR/. + #QMAKE_POST_LINK += cp -rf $$BASEDIR/audio $$DESTDIR/. INCLUDEPATH += /usr/include \ /usr/include/qt4/phonon @@ -253,10 +254,11 @@ linux-g++-64 { -losgViewer \ -losgGA \ -losgDB \ - -losgQt \ -losgText \ -lOpenThreads +# -losgQt \ + DEFINES += QGC_OSG_ENABLED } diff --git a/qgroundcontrol.pro b/qgroundcontrol.pro index 9ea136f98..f7884d72c 100644 --- a/qgroundcontrol.pro +++ b/qgroundcontrol.pro @@ -350,7 +350,8 @@ HEADERS += src/MG.h \ src/ui/WaypointViewOnlyView.h \ src/ui/WaypointViewOnlyView.h \ src/ui/WaypointEditableView.h \ - src/ui/UnconnectedUASInfoWidget.h + src/ui/UnconnectedUASInfoWidget.h \ + src/ui/QGCRGBDView.h # Google Earth is only supported on Mac OS and Windows with Visual Studio Compiler macx|win32-msvc2008|win32-msvc2010::HEADERS += src/ui/map3D/QGCGoogleEarthView.h @@ -477,7 +478,8 @@ SOURCES += src/main.cc \ src/ui/MAVLinkDecoder.cc \ src/ui/WaypointViewOnlyView.cc \ src/ui/WaypointEditableView.cc \ - src/ui/UnconnectedUASInfoWidget.cc + src/ui/UnconnectedUASInfoWidget.cc \ + src/ui/QGCRGBDView.cc # Enable Google Earth only on Mac OS and Windows with Visual Studio compiler macx|win32-msvc2008|win32-msvc2010::SOURCES += src/ui/map3D/QGCGoogleEarthView.cc diff --git a/src/comm/MAVLinkProtocol.cc b/src/comm/MAVLinkProtocol.cc index 5bd705b52..82fbe2b2c 100644 --- a/src/comm/MAVLinkProtocol.cc +++ b/src/comm/MAVLinkProtocol.cc @@ -31,6 +31,7 @@ #include "QGCMAVLinkUASFactory.h" #include "QGC.h" + /** * The default constructor will create a new MAVLink object sending heartbeats at * the MAVLINK_HEARTBEAT_DEFAULT_RATE to all connected links. diff --git a/src/uas/UAS.cc b/src/uas/UAS.cc index 707076dc1..21f7516c7 100644 --- a/src/uas/UAS.cc +++ b/src/uas/UAS.cc @@ -982,10 +982,12 @@ void UAS::receiveExtendedMessage(LinkInterface* link, std::tr1::shared_ptrGetTypeName() == pointCloud.GetTypeName()) { pointCloud.CopyFrom(*message); + emit pointCloudChanged(this); } else if (message->GetTypeName() == rgbdImage.GetTypeName()) { rgbdImage.CopyFrom(*message); + emit rgbdImageChanged(this); } } diff --git a/src/uas/UAS.h b/src/uas/UAS.h index f0d9b2918..502918403 100644 --- a/src/uas/UAS.h +++ b/src/uas/UAS.h @@ -554,6 +554,10 @@ signals: void imageStarted(quint64 timestamp); /** @brief A new camera image has arrived */ void imageReady(UASInterface* uas); + /** @brief Point cloud data has been changed */ + void pointCloudChanged(UASInterface* uas); + /** @brief RGBD image data has been changed */ + void rgbdImageChanged(UASInterface* uas); /** @brief HIL controls have changed */ void hilControlsChanged(uint64_t time, float rollAilerons, float pitchElevator, float yawRudder, float throttle, uint8_t systemMode, uint8_t navMode); diff --git a/src/ui/HUD.cc b/src/ui/HUD.cc index 251d0fa4d..42bca86d5 100644 --- a/src/ui/HUD.cc +++ b/src/ui/HUD.cc @@ -661,6 +661,10 @@ void HUD::paintHUD() nextOfflineImage = ""; } + } + + if (dataStreamEnabled || videoEnabled) + { glRasterPos2i(0, 0); xImageFactor = width() / (float)glImage.width(); diff --git a/src/ui/HUD.h b/src/ui/HUD.h index a6412ba7d..1f5eebdbd 100644 --- a/src/ui/HUD.h +++ b/src/ui/HUD.h @@ -214,6 +214,7 @@ protected: QString nextOfflineImage; bool hudInstrumentsEnabled; bool videoEnabled; + bool dataStreamEnabled; float xImageFactor; float yImageFactor; QAction* enableHUDAction; diff --git a/src/ui/MainWindow.cc b/src/ui/MainWindow.cc index bfda0a0d3..efd74116d 100644 --- a/src/ui/MainWindow.cc +++ b/src/ui/MainWindow.cc @@ -51,6 +51,7 @@ This file is part of the QGROUNDCONTROL project #include "QGCSettingsWidget.h" #include "QGCMapTool.h" #include "MAVLinkDecoder.h" +#include "QGCRGBDView.h" #ifdef QGC_OSG_ENABLED #include "Q3DWidgetFactory.h" @@ -459,9 +460,9 @@ void MainWindow::buildCommonWidgets() if (!video1DockWidget) { video1DockWidget = new QDockWidget(tr("Video Stream 1"), this); - HUD* video1 = new HUD(160, 120, this); + QGCRGBDView* video1 = new QGCRGBDView(160, 120, this); video1->enableHUDInstruments(false); - video1->enableVideo(true); + video1->enableVideo(false); // FIXME select video stream as well video1DockWidget->setWidget(video1); video1DockWidget->setObjectName("VIDEO_STREAM_1_DOCK_WIDGET"); @@ -470,15 +471,37 @@ void MainWindow::buildCommonWidgets() if (!video2DockWidget) { video2DockWidget = new QDockWidget(tr("Video Stream 2"), this); - HUD* video2 = new HUD(160, 120, this); + QGCRGBDView* video2 = new QGCRGBDView(160, 120, this); video2->enableHUDInstruments(false); - video2->enableVideo(true); + video2->enableVideo(false); // FIXME select video stream as well video2DockWidget->setWidget(video2); video2DockWidget->setObjectName("VIDEO_STREAM_2_DOCK_WIDGET"); addTool(video2DockWidget, tr("Video Stream 2"), Qt::LeftDockWidgetArea); } +// if (!rgbd1DockWidget) { +// rgbd1DockWidget = new QDockWidget(tr("Video Stream 1"), this); +// HUD* video1 = new HUD(160, 120, this); +// video1->enableHUDInstruments(false); +// video1->enableVideo(true); +// // FIXME select video stream as well +// video1DockWidget->setWidget(video1); +// video1DockWidget->setObjectName("VIDEO_STREAM_1_DOCK_WIDGET"); +// addTool(video1DockWidget, tr("Video Stream 1"), Qt::LeftDockWidgetArea); +// } + +// if (!rgbd2DockWidget) { +// video2DockWidget = new QDockWidget(tr("Video Stream 2"), this); +// HUD* video2 = new HUD(160, 120, this); +// video2->enableHUDInstruments(false); +// video2->enableVideo(true); +// // FIXME select video stream as well +// video2DockWidget->setWidget(video2); +// video2DockWidget->setObjectName("VIDEO_STREAM_2_DOCK_WIDGET"); +// addTool(video2DockWidget, tr("Video Stream 2"), Qt::LeftDockWidgetArea); +// } + // Custom widgets, added last to all menus and layouts buildCustomWidget(); diff --git a/src/ui/MainWindow.h b/src/ui/MainWindow.h index 1f6195cdf..72e036124 100644 --- a/src/ui/MainWindow.h +++ b/src/ui/MainWindow.h @@ -338,6 +338,8 @@ protected: QPointer headUpDockWidget; QPointer video1DockWidget; QPointer video2DockWidget; + QPointer rgbd1DockWidget; + QPointer rgbd2DockWidget; QPointer logPlayerDockWidget; QPointer hsiDockWidget; diff --git a/src/ui/QGCRGBDView.cc b/src/ui/QGCRGBDView.cc new file mode 100644 index 000000000..4813b3d0c --- /dev/null +++ b/src/ui/QGCRGBDView.cc @@ -0,0 +1,269 @@ +#include +#include + +#include "QGCRGBDView.h" +#include "UASManager.h" + +QGCRGBDView::QGCRGBDView(int width, int height, QWidget *parent) : + HUD(width, height, parent), + rgbEnabled(false), + depthEnabled(false) +{ + enableRGBAction = new QAction(tr("Enable RGB Image"), this); + enableRGBAction->setStatusTip(tr("Show the RGB image live stream in this window")); + enableRGBAction->setCheckable(true); + enableRGBAction->setChecked(rgbEnabled); + connect(enableRGBAction, SIGNAL(triggered(bool)), this, SLOT(enableRGB(bool))); + + enableDepthAction = new QAction(tr("Enable Depthmap"), this); + enableDepthAction->setStatusTip(tr("Show the Depthmap in this window")); + enableDepthAction->setCheckable(true); + enableDepthAction->setChecked(depthEnabled); + connect(enableDepthAction, SIGNAL(triggered(bool)), this, SLOT(enableDepth(bool))); + + connect(UASManager::instance(), SIGNAL(UASCreated(UASInterface*)), this, SLOT(addUAS(UASInterface*))); +} + +void QGCRGBDView::addUAS(UASInterface *uas) +{ + // TODO Enable multi-uas support + connect(uas, SIGNAL(rgbdImageChanged(UASInterface*)), this, SLOT(updateData(UASInterface*))); +} + +void QGCRGBDView::contextMenuEvent (QContextMenuEvent* event) +{ + QMenu menu(this); + // Update actions + enableHUDAction->setChecked(hudInstrumentsEnabled); + //enableVideoAction->setChecked(videoEnabled); + enableRGBAction->setChecked(rgbEnabled); + enableDepthAction->setChecked(depthEnabled); + + menu.addAction(enableHUDAction); + menu.addAction(enableRGBAction); + menu.addAction(enableDepthAction); + //menu.addAction(selectHUDColorAction); + //menu.addAction(enableVideoAction); + //menu.addAction(selectOfflineDirectoryAction); + //menu.addAction(selectVideoChannelAction); + menu.exec(event->globalPos()); +} + +void QGCRGBDView::enableRGB(bool enabled) +{ + rgbEnabled = enabled; + dataStreamEnabled = rgbEnabled | depthEnabled; + resize(size()); +} + +void QGCRGBDView::enableDepth(bool enabled) +{ + depthEnabled = enabled; + dataStreamEnabled = rgbEnabled | depthEnabled; + resize(size()); +} + +float colormapJet[128][3] = { + {0.0f,0.0f,0.53125f}, + {0.0f,0.0f,0.5625f}, + {0.0f,0.0f,0.59375f}, + {0.0f,0.0f,0.625f}, + {0.0f,0.0f,0.65625f}, + {0.0f,0.0f,0.6875f}, + {0.0f,0.0f,0.71875f}, + {0.0f,0.0f,0.75f}, + {0.0f,0.0f,0.78125f}, + {0.0f,0.0f,0.8125f}, + {0.0f,0.0f,0.84375f}, + {0.0f,0.0f,0.875f}, + {0.0f,0.0f,0.90625f}, + {0.0f,0.0f,0.9375f}, + {0.0f,0.0f,0.96875f}, + {0.0f,0.0f,1.0f}, + {0.0f,0.03125f,1.0f}, + {0.0f,0.0625f,1.0f}, + {0.0f,0.09375f,1.0f}, + {0.0f,0.125f,1.0f}, + {0.0f,0.15625f,1.0f}, + {0.0f,0.1875f,1.0f}, + {0.0f,0.21875f,1.0f}, + {0.0f,0.25f,1.0f}, + {0.0f,0.28125f,1.0f}, + {0.0f,0.3125f,1.0f}, + {0.0f,0.34375f,1.0f}, + {0.0f,0.375f,1.0f}, + {0.0f,0.40625f,1.0f}, + {0.0f,0.4375f,1.0f}, + {0.0f,0.46875f,1.0f}, + {0.0f,0.5f,1.0f}, + {0.0f,0.53125f,1.0f}, + {0.0f,0.5625f,1.0f}, + {0.0f,0.59375f,1.0f}, + {0.0f,0.625f,1.0f}, + {0.0f,0.65625f,1.0f}, + {0.0f,0.6875f,1.0f}, + {0.0f,0.71875f,1.0f}, + {0.0f,0.75f,1.0f}, + {0.0f,0.78125f,1.0f}, + {0.0f,0.8125f,1.0f}, + {0.0f,0.84375f,1.0f}, + {0.0f,0.875f,1.0f}, + {0.0f,0.90625f,1.0f}, + {0.0f,0.9375f,1.0f}, + {0.0f,0.96875f,1.0f}, + {0.0f,1.0f,1.0f}, + {0.03125f,1.0f,0.96875f}, + {0.0625f,1.0f,0.9375f}, + {0.09375f,1.0f,0.90625f}, + {0.125f,1.0f,0.875f}, + {0.15625f,1.0f,0.84375f}, + {0.1875f,1.0f,0.8125f}, + {0.21875f,1.0f,0.78125f}, + {0.25f,1.0f,0.75f}, + {0.28125f,1.0f,0.71875f}, + {0.3125f,1.0f,0.6875f}, + {0.34375f,1.0f,0.65625f}, + {0.375f,1.0f,0.625f}, + {0.40625f,1.0f,0.59375f}, + {0.4375f,1.0f,0.5625f}, + {0.46875f,1.0f,0.53125f}, + {0.5f,1.0f,0.5f}, + {0.53125f,1.0f,0.46875f}, + {0.5625f,1.0f,0.4375f}, + {0.59375f,1.0f,0.40625f}, + {0.625f,1.0f,0.375f}, + {0.65625f,1.0f,0.34375f}, + {0.6875f,1.0f,0.3125f}, + {0.71875f,1.0f,0.28125f}, + {0.75f,1.0f,0.25f}, + {0.78125f,1.0f,0.21875f}, + {0.8125f,1.0f,0.1875f}, + {0.84375f,1.0f,0.15625f}, + {0.875f,1.0f,0.125f}, + {0.90625f,1.0f,0.09375f}, + {0.9375f,1.0f,0.0625f}, + {0.96875f,1.0f,0.03125f}, + {1.0f,1.0f,0.0f}, + {1.0f,0.96875f,0.0f}, + {1.0f,0.9375f,0.0f}, + {1.0f,0.90625f,0.0f}, + {1.0f,0.875f,0.0f}, + {1.0f,0.84375f,0.0f}, + {1.0f,0.8125f,0.0f}, + {1.0f,0.78125f,0.0f}, + {1.0f,0.75f,0.0f}, + {1.0f,0.71875f,0.0f}, + {1.0f,0.6875f,0.0f}, + {1.0f,0.65625f,0.0f}, + {1.0f,0.625f,0.0f}, + {1.0f,0.59375f,0.0f}, + {1.0f,0.5625f,0.0f}, + {1.0f,0.53125f,0.0f}, + {1.0f,0.5f,0.0f}, + {1.0f,0.46875f,0.0f}, + {1.0f,0.4375f,0.0f}, + {1.0f,0.40625f,0.0f}, + {1.0f,0.375f,0.0f}, + {1.0f,0.34375f,0.0f}, + {1.0f,0.3125f,0.0f}, + {1.0f,0.28125f,0.0f}, + {1.0f,0.25f,0.0f}, + {1.0f,0.21875f,0.0f}, + {1.0f,0.1875f,0.0f}, + {1.0f,0.15625f,0.0f}, + {1.0f,0.125f,0.0f}, + {1.0f,0.09375f,0.0f}, + {1.0f,0.0625f,0.0f}, + {1.0f,0.03125f,0.0f}, + {1.0f,0.0f,0.0f}, + {0.96875f,0.0f,0.0f}, + {0.9375f,0.0f,0.0f}, + {0.90625f,0.0f,0.0f}, + {0.875f,0.0f,0.0f}, + {0.84375f,0.0f,0.0f}, + {0.8125f,0.0f,0.0f}, + {0.78125f,0.0f,0.0f}, + {0.75f,0.0f,0.0f}, + {0.71875f,0.0f,0.0f}, + {0.6875f,0.0f,0.0f}, + {0.65625f,0.0f,0.0f}, + {0.625f,0.0f,0.0f}, + {0.59375f,0.0f,0.0f}, + {0.5625f,0.0f,0.0f}, + {0.53125f,0.0f,0.0f}, + {0.5f,0.0f,0.0f} +}; + +void QGCRGBDView::updateData(UASInterface *uas) +{ +#ifdef QGC_PROTOBUF_ENABLED + px::RGBDImage rgbdImage = uas->getRGBDImage(); + + if (rgbdImage.rows() == 0 || rgbdImage.cols() == 0 || (!rgbEnabled && !depthEnabled)) + { + return; + } + + QImage fill; + + if (rgbEnabled) + { +// fill = QImage(reinterpret_cast(rgbdImage.imagedata1().c_str()), +// rgbdImage.cols(), rgbdImage.rows(), QImage::Format_Mono); + + + // Construct PGM header + QString header("P5\n%1 %2\n%3\n"); + int imgColors = 255; + header = header.arg(rgbdImage.cols()).arg(rgbdImage.rows()).arg(imgColors); + + //QByteArray tmpImage(rgbdImage.imagedata1().c_str(), rgbdImage.cols()*rgbdImage.rows()); + QByteArray tmpImage(header.toStdString().c_str(), header.toStdString().size()); + tmpImage.append(rgbdImage.imagedata1().c_str(), rgbdImage.cols()*rgbdImage.rows()); + + //qDebug() << "IMAGE SIZE:" << tmpImage.size() << "HEADER SIZE: (15):" << header.size() << "HEADER: " << header; + +// if (imageRecBuffer.isNull()) +// { +// qDebug()<< "could not convertToPGM()"; +// return QImage(); +// } + + if (!fill.loadFromData(tmpImage, "PGM")) + { + qDebug()<< "could not create extracted image"; +// return QImage(); + } + } + + if (depthEnabled) + { + QByteArray coloredDepth(rgbdImage.cols() * rgbdImage.rows() * 3, 0); + + for (uint32_t r = 0; r < rgbdImage.rows(); ++r) + { + const float* depth = reinterpret_cast(rgbdImage.imagedata2().c_str() + r * rgbdImage.step2()); + uint8_t* pixel = reinterpret_cast(coloredDepth.data()) + r * rgbdImage.cols() * 3; + for (uint32_t c = 0; c < rgbdImage.cols(); ++c) + { + if (depth[c] != 0) + { + int idx = fminf(depth[c], 10.0f) / 10.0f * 127.0f; + idx = 127 - idx; + + pixel[0] = colormapJet[idx][2] * 255.0f; + pixel[1] = colormapJet[idx][1] * 255.0f; + pixel[2] = colormapJet[idx][0] * 255.0f; + } + + pixel += 3; + } + } + + fill = QImage(reinterpret_cast(coloredDepth.constData()), + rgbdImage.cols(), rgbdImage.rows(), QImage::Format_RGB888); + } + + glImage = QGLWidget::convertToGLFormat(fill); +#endif +} diff --git a/src/ui/QGCRGBDView.h b/src/ui/QGCRGBDView.h new file mode 100644 index 000000000..586bba762 --- /dev/null +++ b/src/ui/QGCRGBDView.h @@ -0,0 +1,29 @@ +#ifndef QGCRGBDVIEW_H +#define QGCRGBDVIEW_H + +#include "HUD.h" + +class QGCRGBDView : public HUD +{ + Q_OBJECT +public: + explicit QGCRGBDView(int width=640, int height=480, QWidget *parent = 0); + +signals: + +public slots: + void addUAS(UASInterface* uas); + void enableRGB(bool enabled); + void enableDepth(bool enabled); + void updateData(UASInterface *uas); + +protected: + bool rgbEnabled; + bool depthEnabled; + QAction* enableRGBAction; + QAction* enableDepthAction; + + void contextMenuEvent (QContextMenuEvent* event); +}; + +#endif // QGCRGBDVIEW_H diff --git a/src/ui/map3D/Pixhawk3DWidget.cc b/src/ui/map3D/Pixhawk3DWidget.cc index 4890b3acf..a13dcc1e6 100644 --- a/src/ui/map3D/Pixhawk3DWidget.cc +++ b/src/ui/map3D/Pixhawk3DWidget.cc @@ -59,11 +59,11 @@ Pixhawk3DWidget::Pixhawk3DWidget(QWidget* parent) , displayImagery(true) , displayWaypoints(true) , displayRGBD2D(false) - , displayRGBD3D(false) - , enableRGBDColor(true) + , displayRGBD3D(true) + , enableRGBDColor(false) , enableTarget(false) , followCamera(true) - , frame(MAV_FRAME_GLOBAL) + , frame(MAV_FRAME_LOCAL_NED) , lastRobotX(0.0f) , lastRobotY(0.0f) , lastRobotZ(0.0f) @@ -433,8 +433,8 @@ void Pixhawk3DWidget::buildLayout(void) { QComboBox* frameComboBox = new QComboBox(this); - frameComboBox->addItem("Global"); frameComboBox->addItem("Local"); + frameComboBox->addItem("Global"); frameComboBox->setFixedWidth(70); QCheckBox* gridCheckBox = new QCheckBox(this); @@ -643,7 +643,6 @@ Pixhawk3DWidget::getPose(double& x, double& y, double& z, z = uas->getLocalZ(); } - roll = uas->getRoll(); pitch = uas->getPitch(); yaw = uas->getYaw(); @@ -1222,38 +1221,41 @@ Pixhawk3DWidget::updateRGBD(double robotX, double robotY, double robotZ) px::RGBDImage rgbdImage = uas->getRGBDImage(); px::PointCloudXYZRGB pointCloud = uas->getPointCloud(); - rgbImage->setImage(rgbdImage.cols(), rgbdImage.rows(), 1, - GL_LUMINANCE, GL_LUMINANCE, GL_UNSIGNED_BYTE, - reinterpret_cast(&(*(rgbdImage.mutable_imagedata1()))[0]), - osg::Image::NO_DELETE); - rgbImage->dirty(); - - QByteArray coloredDepth(rgbdImage.cols() * rgbdImage.rows() * 3, 0); - for (uint32_t r = 0; r < rgbdImage.rows(); ++r) + if (rgbdImage.rows() > 0 && rgbdImage.cols() > 0) { - const float* depth = reinterpret_cast(rgbdImage.imagedata2().c_str() + r * rgbdImage.step2()); - uint8_t* pixel = reinterpret_cast(coloredDepth.data()) + r * rgbdImage.cols() * 3; - for (uint32_t c = 0; c < rgbdImage.cols(); ++c) + rgbImage->setImage(rgbdImage.cols(), rgbdImage.rows(), 1, + GL_LUMINANCE, GL_LUMINANCE, GL_UNSIGNED_BYTE, + reinterpret_cast(&(*(rgbdImage.mutable_imagedata1()))[0]), + osg::Image::NO_DELETE); + rgbImage->dirty(); + + QByteArray coloredDepth(rgbdImage.cols() * rgbdImage.rows() * 3, 0); + for (uint32_t r = 0; r < rgbdImage.rows(); ++r) { - if (depth[c] != 0) + const float* depth = reinterpret_cast(rgbdImage.imagedata2().c_str() + r * rgbdImage.step2()); + uint8_t* pixel = reinterpret_cast(coloredDepth.data()) + r * rgbdImage.cols() * 3; + for (uint32_t c = 0; c < rgbdImage.cols(); ++c) { - int idx = fminf(depth[c], 10.0f) / 10.0f * 127.0f; - idx = 127 - idx; + if (depth[c] != 0) + { + int idx = fminf(depth[c], 10.0f) / 10.0f * 127.0f; + idx = 127 - idx; + + pixel[0] = colormap_jet[idx][2] * 255.0f; + pixel[1] = colormap_jet[idx][1] * 255.0f; + pixel[2] = colormap_jet[idx][0] * 255.0f; + } - pixel[0] = colormap_jet[idx][2] * 255.0f; - pixel[1] = colormap_jet[idx][1] * 255.0f; - pixel[2] = colormap_jet[idx][0] * 255.0f; + pixel += 3; } - - pixel += 3; } - } - depthImage->setImage(rgbdImage.cols(), rgbdImage.rows(), 1, - GL_RGB, GL_RGB, GL_UNSIGNED_BYTE, - reinterpret_cast(coloredDepth.data()), - osg::Image::NO_DELETE); - depthImage->dirty(); + depthImage->setImage(rgbdImage.cols(), rgbdImage.rows(), 1, + GL_RGB, GL_RGB, GL_UNSIGNED_BYTE, + reinterpret_cast(coloredDepth.data()), + osg::Image::NO_DELETE); + depthImage->dirty(); + } osg::Geometry* geometry = rgbd3DNode->getDrawable(0)->asGeometry(); @@ -1266,6 +1268,7 @@ Pixhawk3DWidget::updateRGBD(double robotX, double robotY, double robotZ) double y = p.y() - robotY; double z = p.z() - robotZ; + (*vertices)[i].set(y, x, -z); if (enableRGBDColor) { -- 2.22.0