add，深度相机：
1、采集惯导数据并写入文件； 2、轮播看板添加深度图像； 3、植物表型场景控制深度相机看板； fix： 优化调焦时，图像像素的填充方式；
This commit is contained in:
tangchao0503
@ -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()
|
||||
|
||||
@ -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;
|
||||
|
||||
@ -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;
|
||||
)");
|
||||
m_carousel->addWidget(tmp8);
|
||||
//m_carousel->setContentsMargins(0, 0, 0, 0);
|
||||
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;
|
||||
)");
|
||||
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);
|
||||
|
||||
@ -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();
|
||||
|
||||
|
||||
@ -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<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;
|
||||
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<unsigned char>(srcRow[j] * scale);
|
||||
scanLine[j] = qRgb(gray, gray, gray);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
Reference in New Issue
Block a user