From d326dabff71c168895d24891c0a372e435848879 Mon Sep 17 00:00:00 2001 From: tangchao0503 <735056338@qq.com> Date: Thu, 9 Apr 2026 16:34:08 +0800 Subject: [PATCH] =?UTF-8?q?add=EF=BC=8C=E6=B7=B1=E5=BA=A6=E7=9B=B8?= =?UTF-8?q?=E6=9C=BA=EF=BC=9A=201=E3=80=81=E9=87=87=E9=9B=86=E6=83=AF?= =?UTF-8?q?=E5=AF=BC=E6=95=B0=E6=8D=AE=E5=B9=B6=E5=86=99=E5=85=A5=E6=96=87?= =?UTF-8?q?=E4=BB=B6=EF=BC=9B=202=E3=80=81=E8=BD=AE=E6=92=AD=E7=9C=8B?= =?UTF-8?q?=E6=9D=BF=E6=B7=BB=E5=8A=A0=E6=B7=B1=E5=BA=A6=E5=9B=BE=E5=83=8F?= =?UTF-8?q?=EF=BC=9B=203=E3=80=81=E6=A4=8D=E7=89=A9=E8=A1=A8=E5=9E=8B?= =?UTF-8?q?=E5=9C=BA=E6=99=AF=E6=8E=A7=E5=88=B6=E6=B7=B1=E5=BA=A6=E7=9B=B8?= =?UTF-8?q?=E6=9C=BA=E7=9C=8B=E6=9D=BF=EF=BC=9B?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit fix： 优化调焦时，图像像素的填充方式； --- HPPA/DepthCameraWindow.cpp | 96 ++++++++++++++++++++++++++++++++++++-- HPPA/DepthCameraWindow.h | 10 +++- HPPA/HPPA.cpp | 47 +++++++++++++++++-- HPPA/HPPA.h | 3 ++ HPPA/image2display.cpp | 18 +++---- 5 files changed, 154 insertions(+), 20 deletions(-) diff --git a/HPPA/DepthCameraWindow.cpp b/HPPA/DepthCameraWindow.cpp index 3b3ba34..0e80437 100644 --- a/HPPA/DepthCameraWindow.cpp +++ b/HPPA/DepthCameraWindow.cpp @@ -17,6 +17,9 @@ DepthCameraWindow::DepthCameraWindow(QWidget* parent) connect(m_DepthCameraOperation, &DepthCameraOperation::CamOpenedSignal, this, &DepthCameraWindow::onCamOpened); connect(m_DepthCameraOperation, &DepthCameraOperation::CamClosedSignal, this, &DepthCameraWindow::onCamClosed); + + connect(m_DepthCameraOperation, &DepthCameraOperation::PlotSignal, this, &DepthCameraWindow::PlotDepthImageSignal); + connect(m_DepthCameraOperation, &DepthCameraOperation::CamClosedSignal, this, &DepthCameraWindow::DepthCamClosedSignal); } DepthCameraWindow::~DepthCameraWindow() @@ -107,6 +110,8 @@ void DepthCameraOperation::OpenDepthCamera() config->enableVideoStream(OB_STREAM_DEPTH, 640, 480, 15, OB_FORMAT_Y16); config->enableVideoStream(OB_STREAM_COLOR, 640, 480, 15, OB_FORMAT_YUYV); + config->enableAccelStream(); + config->enableGyroStream(); config->setFrameAggregateOutputMode(OB_FRAME_AGGREGATE_OUTPUT_ALL_TYPE_FRAME_REQUIRE); config->setAlignMode(ALIGN_D2C_HW_MODE); @@ -127,6 +132,8 @@ void DepthCameraOperation::OpenDepthCamera() int frameIndex = 0; record = true; QString fileNamePrefix = AppSettings::instance().dataFolder() + QDir::separator() + AppSettings::instance().fileName(); + QString imuFilePath = fileNamePrefix + "_IMU.txt"; + std::ofstream imuFile(imuFilePath.toStdString(), std::ios::out | std::ios::trunc); while (record) { if(frameIndex==0) @@ -142,7 +149,9 @@ void DepthCameraOperation::OpenDepthCamera() continue; } - // Get the depth and color frames. + std::cout << "DepthCamera frameIndex："<< frameIndex << std::endl; + + // 彩色和深度图像 auto depthFrame = frameSet->getFrame(OB_FRAME_DEPTH)->as(); auto colorFrame = frameSet->getFrame(OB_FRAME_COLOR)->as(); @@ -170,15 +179,65 @@ void DepthCameraOperation::OpenDepthCamera() saveDepthFrame(depthFrame, frameIndex, fileNamePrefix.toStdString()); saveColorFrame(colorFrame, frameIndex, fileNamePrefix.toStdString()); + cv::Mat colorMat(colorFrame->height(), colorFrame->width(), CV_8UC3, colorFrame->data()); + cv::Mat rgbMat; + cv::cvtColor(colorMat, rgbMat, cv::COLOR_BGR2RGB); + m_colorImage = QImage(rgbMat.data, rgbMat.cols, rgbMat.rows, static_cast(rgbMat.step), QImage::Format_RGB888).copy(); + + cv::Mat depthMat(depthFrame->height(), depthFrame->width(), CV_16UC1, depthFrame->data()); + cv::Mat depthMat8U; + depthMat.convertTo(depthMat8U, CV_8UC1, 255.0 / 4096.0); + cv::Mat depthColorMap; + cv::applyColorMap(depthMat8U, depthColorMap, cv::COLORMAP_JET); + cv::Mat depthRgbMat; + cv::cvtColor(depthColorMap, depthRgbMat, cv::COLOR_BGR2RGB); + m_depthImage = QImage(depthRgbMat.data, depthRgbMat.cols, depthRgbMat.rows, static_cast(depthRgbMat.step), QImage::Format_RGB888).copy(); + //m_depthImage = QImage(depthMat.data, depthMat.cols, depthMat.rows, static_cast(depthMat.step), QImage::Format_Grayscale16).copy(); + + emit PlotSignal(); + + //点云 pointCloud->setCreatePointFormat(OB_FORMAT_RGB_POINT); std::shared_ptr frame = pointCloud->process(frameSet); QString plyPath = fileNamePrefix + "_"+ QString::number(frameIndex) + ".ply"; ob::PointCloudHelper::savePointcloudToPly(plyPath.toStdString().c_str(), frame, false, false, 50); + //惯导数据 + auto accelFrameRaw = frameSet->getFrame(OB_FRAME_ACCEL); + auto accelFrame = accelFrameRaw->as(); + auto accelIndex = accelFrame->getIndex(); + auto accelTimeStampUs = accelFrame->getTimeStampUs(); + auto accelTemperature = accelFrame->getTemperature(); + auto accelType = accelFrame->getType(); + //if (frameIndex % 50 == 0) + //{ // print information every 50 frames. + // auto accelValue = accelFrame->getValue(); + // printImuValue(accelValue, accelIndex, accelTimeStampUs, accelTemperature, accelType, "m/s^2"); + //} + //auto accelValue = accelFrame->getValue(); + //printImuValue(accelValue, accelIndex, accelTimeStampUs, accelTemperature, accelType, "m/s^2"); + + auto gyroFrameRaw = frameSet->getFrame(OB_FRAME_GYRO); + auto gyroFrame = gyroFrameRaw->as(); + auto gyroIndex = gyroFrame->getIndex(); + auto gyroTimeStampUs = gyroFrame->getTimeStampUs(); + auto gyroTemperature = gyroFrame->getTemperature(); + auto gyroType = gyroFrame->getType(); + //if (frameIndex % 50 == 0) { // print information every 50 frames. + // auto gyroValue = gyroFrame->getValue(); + // printImuValue(gyroValue, gyroIndex, gyroTimeStampUs, gyroTemperature, gyroType, "rad/s"); + //} + //auto gyroValue = gyroFrame->getValue(); + //printImuValue(gyroValue, gyroIndex, gyroTimeStampUs, gyroTemperature, gyroType, "rad/s"); + + saveImuData(imuFile, accelFrame, gyroFrame); + frameIndex++; } + imuFile.close(); + m_pipe->stop(); delete m_pipe; @@ -196,7 +255,7 @@ void DepthCameraOperation::saveDepthFrame(const std::shared_ptr + std::to_string(depthFrame->timeStamp()) + "ms.png"; cv::Mat depthMat(depthFrame->height(), depthFrame->width(), CV_16UC1, depthFrame->data()); cv::imwrite(depthName, depthMat, params); - std::cout << "Depth saved:" << depthName << std::endl; + //std::cout << "Depth saved:" << depthName << std::endl; } void DepthCameraOperation::saveColorFrame(const std::shared_ptr colorFrame, const uint32_t frameIndex, std::string fileNamePrefix_) @@ -210,7 +269,38 @@ void DepthCameraOperation::saveColorFrame(const std::shared_ptr + std::to_string(colorFrame->timeStamp()) + "ms.png"; cv::Mat depthMat(colorFrame->height(), colorFrame->width(), CV_8UC3, colorFrame->data()); cv::imwrite(colorName, depthMat, params); - std::cout << "Color saved:" << colorName << std::endl; + //std::cout << "Color saved:" << colorName << std::endl; +} + +void DepthCameraOperation::printImuValue(OBFloat3D obFloat3d, uint64_t index, uint64_t timeStampUs, float temperature, OBFrameType type, const std::string& unitStr) +{ + std::cout << "frame index: " << index << std::endl; + auto typeStr = ob::TypeHelper::convertOBFrameTypeToString(type); + std::cout << typeStr << " Frame: \n\r{\n\r" + << " tsp = " << timeStampUs << "\n\r" + << " temperature = " << temperature << "\n\r" + << " " << typeStr << ".x = " << obFloat3d.x << unitStr << "\n\r" + << " " << typeStr << ".y = " << obFloat3d.y << unitStr << "\n\r" + << " " << typeStr << ".z = " << obFloat3d.z << unitStr << "\n\r" + << "}\n\r" << std::endl; +} + +void DepthCameraOperation::saveImuData(std::ofstream& imuFile, const std::shared_ptr& accelFrame, const std::shared_ptr& gyroFrame) +{ + if (!imuFile.is_open()) + return; + + auto accelValue = accelFrame->getValue(); + auto gyroValue = gyroFrame->getValue(); + + // position (acceleration): ax, ay, az + // attitude (angular velocity): gx, gy, gz + imuFile << accelFrame->getIndex() << "," + << accelFrame->getTimeStampUs() << "," + << accelValue.x << "," << accelValue.y << "," << accelValue.z << "," + << gyroFrame->getIndex() << "," + << gyroFrame->getTimeStampUs() << "," + << gyroValue.x << "," << gyroValue.y << "," << gyroValue.z << "\n"; } void DepthCameraOperation::OpenDepthCamera_callback() diff --git a/HPPA/DepthCameraWindow.h b/HPPA/DepthCameraWindow.h index 318f5e6..02f2901 100644 --- a/HPPA/DepthCameraWindow.h +++ b/HPPA/DepthCameraWindow.h @@ -12,6 +12,7 @@ #include "ui_DepthCamera.h" #include "AppSettings.h" +#include #include #include #include "libobsensor/hpp/Utils.hpp" @@ -25,7 +26,8 @@ public: DepthCameraOperation(); ~DepthCameraOperation(); - QImage m_qImage; + QImage m_colorImage; + QImage m_depthImage; void setCallback(void(*func)()); bool getRecordStatus() const { return record; } @@ -37,6 +39,8 @@ private: void saveDepthFrame(const std::shared_ptr depthFrame, const uint32_t frameIndex, std::string fileNamePrefix_); void saveColorFrame(const std::shared_ptr colorFrame, const uint32_t frameIndex, std::string fileNamePrefix_); + void printImuValue(OBFloat3D obFloat3d, uint64_t index, uint64_t timeStampUs, float temperature, OBFrameType type, const std::string& unitStr); + void saveImuData(std::ofstream& imuFile, const std::shared_ptr& accelFrame, const std::shared_ptr& gyroFrame); bool record; @@ -69,7 +73,9 @@ public Q_SLOTS: void onCamClosed(); signals: - void openDepthCameraSignal(); +void openDepthCameraSignal(); +void PlotDepthImageSignal(); +void DepthCamClosedSignal(); private: Ui::DepthCameraClass ui; diff --git a/HPPA/HPPA.cpp b/HPPA/HPPA.cpp index f4f1274..81cba02 100644 --- a/HPPA/HPPA.cpp +++ b/HPPA/HPPA.cpp @@ -399,12 +399,28 @@ HPPA::HPPA(QWidget* parent) m_carousel->addWidget(sa); m_carousel->setContentsMargins(0, 0, 0, 0); - QWidget* tmp8 = new QWidget(); - tmp8->setStyleSheet(R"( + //--------------------------------------------------------------------- + QScrollArea* sa_depthCamera = new QScrollArea(); + sa_depthCamera->setObjectName("sa_depthCamera"); + sa_depthCamera->setStyleSheet(R"( + border: none; + background-color: #0D1233; + )"); + QGridLayout* gridLayout_sa_depthCamera = new QGridLayout(sa_depthCamera); + gridLayout_sa_depthCamera->setSpacing(6); + gridLayout_sa_depthCamera->setObjectName(QString::fromUtf8("gridLayout_sa_depthCamera")); + gridLayout_sa_depthCamera->setVerticalSpacing(0); + gridLayout_sa_depthCamera->setContentsMargins(0, 0, 0, 0); + + m_depthCamera_label = new QLabel(); + m_depthCamera_label->setAlignment(Qt::AlignHCenter); + m_depthCamera_label->setStyleSheet(R"( background-color: #0D1233; )"); - m_carousel->addWidget(tmp8); - //m_carousel->setContentsMargins(0, 0, 0, 0); + gridLayout_sa_depthCamera->addWidget(m_depthCamera_label); + + m_carousel->addWidget(sa_depthCamera); + m_carousel->setContentsMargins(0, 0, 0, 0); m_carousel->play(); @@ -753,6 +769,7 @@ void HPPA::initControlTabwidget() this, SLOT(onBandSelectionChanged(double, double, double))); ui.controlTabWidget->addTab(m_ic, QString::fromLocal8Bit("图像控制"));//????????????????????????????????????????????????????????????????????????????????????????????????? + //深度相机 m_depthCameraWindow = new DepthCameraWindow(); ui.controlTabWidget->addTab(m_depthCameraWindow, QString::fromLocal8Bit("深度相机")); @@ -761,6 +778,9 @@ void HPPA::initControlTabwidget() connect(m_rgbCameraControlWindow, &rgbCameraWindow::PlotRgbImageSignal, this, &HPPA::onPlotRgbImage); connect(m_rgbCameraControlWindow, &rgbCameraWindow::CamClosedSignal, this, &HPPA::onClearLabel); + connect(m_depthCameraWindow, &DepthCameraWindow::PlotDepthImageSignal, this, &HPPA::onPlotDepthImage); + connect(m_depthCameraWindow, &DepthCameraWindow::DepthCamClosedSignal, this, &HPPA::onClearDepthLabel); + ui.controlTabWidget->addTab(m_rgbCameraControlWindow, QString::fromLocal8Bit("rgb相机")); //升降桌dock @@ -1213,6 +1233,7 @@ void HPPA::createOneMotorScenario() ui.mAction_1AxisMotor->setChecked(true); //右下角控制tab + m_tabManager->hideTab(m_depthCameraWindow); m_tabManager->hideTab(m_rgbCameraControlWindow); m_tabManager->hideTab(m_adt); m_tabManager->hideTab(m_pc); @@ -1244,6 +1265,7 @@ void HPPA::createPlantPhenotypeScenario() m_tabManager->hideTab(m_rac); m_tabManager->hideTab(m_omc); + m_tabManager->showTab(m_depthCameraWindow); m_tabManager->showTab(m_rgbCameraControlWindow); m_tabManager->showTab(m_adt); m_tabManager->showTab(m_pc); @@ -1763,6 +1785,23 @@ void HPPA::onClearLabel() m_cam_label->setText("closed"); } +void HPPA::onPlotDepthImage() +{ + QPixmap pixmap = QPixmap::fromImage(m_depthCameraWindow->m_DepthCameraOperation->m_depthImage); + + int width = m_depthCamera_label->width(); + int height = m_depthCamera_label->height(); + QPixmap fitpixmap = pixmap.scaled(width, height, Qt::KeepAspectRatio, Qt::SmoothTransformation); + + m_depthCamera_label->setPixmap(fitpixmap); +} + +void HPPA::onClearDepthLabel() +{ + m_depthCamera_label->clear(); + m_depthCamera_label->setText("closed"); +} + void HPPA::onCopyFinished() { this->setEnabled(true); diff --git a/HPPA/HPPA.h b/HPPA/HPPA.h index 5cfbf4d..2c6f816 100644 --- a/HPPA/HPPA.h +++ b/HPPA/HPPA.h @@ -271,6 +271,7 @@ private: MyCarousel* m_carousel; QLabel* m_cam_label; + QLabel* m_depthCamera_label; QPushButton* m_open_rgb_camera_btn; QPushButton* m_close_rgb_camera_btn; @@ -351,8 +352,10 @@ public Q_SLOTS: void OnSendLogToCallClass(QString str); void onPlotRgbImage(); + void onPlotDepthImage(); void onClearLabel(); + void onClearDepthLabel(); void onCopyFinished(); diff --git a/HPPA/image2display.cpp b/HPPA/image2display.cpp index 35fb018..56ca9a5 100644 --- a/HPPA/image2display.cpp +++ b/HPPA/image2display.cpp @@ -153,22 +153,18 @@ void CImage::FillFocusGrayImage(unsigned short * datacube) void CImage::FillFocusGrayQImage(unsigned short * datacube) { - float two_eight = pow(2.0, 8); - float two_sixteen = pow(2.0, 12); + constexpr float scale = 256.0f / 4096.0f; // 2^8 / 2^12 int width = m_qimageFocusGrayImage->width(); int height = m_qimageFocusGrayImage->height(); - for (unsigned short i = 0; i < height; i++) + for (int i = 0; i < height; i++) { - for (unsigned short j = 0; j < width; j++) + QRgb* scanLine = reinterpret_cast(m_qimageFocusGrayImage->scanLine(i)); + const unsigned short* srcRow = datacube + width * i; + for (int j = 0; j < width; j++) { - //uint tmp = (two_eight* *(datacube + width * i + j)) / two_sixteen; - uint tmp = (two_eight* datacube[width*i + j]) / two_sixteen; - //uint tmp = datacube[width*i + j]; - - - //m_qimageFocusGrayImage->setPixel(j, i, tmp); - m_qimageFocusGrayImage->setPixel(j, i, qRgb((unsigned char)tmp, (unsigned char)tmp, (unsigned char)tmp)); + unsigned char gray = static_cast(srcRow[j] * scale); + scanLine[j] = qRgb(gray, gray, gray); } }