add，深度相机：

1、采集惯导数据并写入文件；
2、轮播看板添加深度图像；
3、植物表型场景控制深度相机看板；

fix：
优化调焦时，图像像素的填充方式；

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<ob::DepthFrame>();
         auto colorFrame = frameSet->getFrame(OB_FRAME_COLOR)->as<ob::ColorFrame>();
 
@@ -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<int>(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<int>(depthRgbMat.step), QImage::Format_RGB888).copy();
+        //m_depthImage = QImage(depthMat.data, depthMat.cols, depthMat.rows, static_cast<int>(depthMat.step), QImage::Format_Grayscale16).copy();
+
+        emit PlotSignal();
+
+        //点云
         pointCloud->setCreatePointFormat(OB_FORMAT_RGB_POINT);
         std::shared_ptr<ob::Frame> 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<ob::AccelFrame>();
+        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<ob::GyroFrame>();
+        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<ob::DepthFrame>
         + 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<ob::ColorFrame> colorFrame, const uint32_t frameIndex, std::string fileNamePrefix_)
@@ -210,7 +269,38 @@ void DepthCameraOperation::saveColorFrame(const std::shared_ptr<ob::ColorFrame>
         + 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<ob::AccelFrame>& accelFrame, const std::shared_ptr<ob::GyroFrame>& 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()

HPPA/DepthCameraWindow.h

@@ -12,6 +12,7 @@
 #include "ui_DepthCamera.h"
 #include "AppSettings.h"
 
+#include <fstream>
 #include <opencv2/opencv.hpp>
 #include <libobsensor/ObSensor.hpp>
 #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<ob::DepthFrame> depthFrame, const uint32_t frameIndex, std::string fileNamePrefix_);
 	void saveColorFrame(const std::shared_ptr<ob::ColorFrame> 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<ob::AccelFrame>& accelFrame, const std::shared_ptr<ob::GyroFrame>& 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;

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);
+	gridLayout_sa_depthCamera->addWidget(m_depthCamera_label);
-	//m_carousel->setContentsMargins(0, 0, 0, 0);
+
+	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);

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();
 

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);
+	constexpr float scale = 256.0f / 4096.0f; // 2^8 / 2^12
-	float two_sixteen = pow(2.0, 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<QRgb*>(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;
+			unsigned char gray = static_cast<unsigned char>(srcRow[j] * scale);
-			uint tmp = (two_eight* datacube[width*i + j]) / two_sixteen;
+			scanLine[j] = qRgb(gray, gray, gray);
-			//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));
 		}
 	}