636 lines
24 KiB
C++
636 lines
24 KiB
C++
#include "DepthCameraWindow.h"
|
||
|
||
DepthCameraWindow::DepthCameraWindow(QWidget* parent)
|
||
: QDialog(parent)
|
||
{
|
||
ui.setupUi(this);
|
||
|
||
m_DepthCameraThread = new QThread();
|
||
m_DepthCameraOperation = new DepthCameraOperation();
|
||
m_DepthCameraOperation->moveToThread(m_DepthCameraThread);
|
||
m_DepthCameraThread->start();
|
||
|
||
connect(ui.openDepthCamera_btn, &QPushButton::clicked, this, &DepthCameraWindow::openDepthCamera);
|
||
connect(ui.closeDepthCamera_btn, &QPushButton::clicked, this, &DepthCameraWindow::closeDepthCamera);
|
||
|
||
connect(this, &DepthCameraWindow::openDepthCameraSignal, m_DepthCameraOperation, &DepthCameraOperation::OpenDepthCamera);
|
||
connect(this, &DepthCameraWindow::OpenDepthCamera_getDepthValueSignal, m_DepthCameraOperation, &DepthCameraOperation::OpenDepthCamera_getDepthValue);
|
||
|
||
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);
|
||
|
||
connect(this->ui.dataFolderBtn, SIGNAL(clicked()), this, SLOT(onSelectDataFolder()));
|
||
|
||
// 初始化数据保存路径显示（从 AppSettings 恢复或使用默认）
|
||
ui.dataFolderLineEdit->setText(AppSettings::instance().depthCameraDataFolder());
|
||
}
|
||
|
||
DepthCameraWindow::~DepthCameraWindow()
|
||
{
|
||
m_DepthCameraThread->quit();
|
||
m_DepthCameraThread->wait();
|
||
delete m_DepthCameraOperation;
|
||
m_DepthCameraOperation = nullptr;
|
||
}
|
||
|
||
void DepthCameraWindow::onSelectDataFolder()
|
||
{
|
||
QString dir = QFileDialog::getExistingDirectory(this,
|
||
QString::fromLocal8Bit("选择数据保存路径"),
|
||
ui.dataFolderLineEdit->text());
|
||
|
||
setDataFolder(dir);
|
||
}
|
||
|
||
void DepthCameraWindow::setDataFolder(QString dir)
|
||
{
|
||
if (!dir.isEmpty())
|
||
{
|
||
ui.dataFolderLineEdit->setText(dir);
|
||
AppSettings::instance().setDepthCameraDataFolder(dir);
|
||
}
|
||
}
|
||
|
||
void DepthCameraWindow::setCaptureInterval(int captureIntervalSeconds)
|
||
{
|
||
m_DepthCameraOperation->setCaptureInterval(captureIntervalSeconds);
|
||
}
|
||
|
||
void DepthCameraWindow::openDepthCamera()
|
||
{
|
||
if (!m_DepthCameraOperation->getRecordStatus())
|
||
{
|
||
emit openDepthCameraSignal();
|
||
}
|
||
}
|
||
|
||
void DepthCameraWindow::OpenDepthCamera_getDepthValue()
|
||
{
|
||
if (!m_DepthCameraOperation->getRecordStatus())
|
||
{
|
||
emit OpenDepthCamera_getDepthValueSignal();
|
||
}
|
||
}
|
||
|
||
void DepthCameraWindow::onCamOpened()
|
||
{
|
||
ui.openDepthCamera_btn->setEnabled(false);
|
||
ui.closeDepthCamera_btn->setEnabled(true);
|
||
|
||
ui.openDepthCamera_btn->setText(QString::fromLocal8Bit("已打开"));
|
||
}
|
||
|
||
void DepthCameraWindow::closeDepthCamera()
|
||
{
|
||
m_DepthCameraOperation->CloseDepthCamera();
|
||
}
|
||
|
||
void DepthCameraWindow::onCamClosed()
|
||
{
|
||
ui.openDepthCamera_btn->setEnabled(true);
|
||
ui.closeDepthCamera_btn->setEnabled(false);
|
||
|
||
ui.openDepthCamera_btn->setText(QString::fromLocal8Bit("打 开"));
|
||
}
|
||
|
||
//-------------------------------------------------------------------------------------------------------------------------------
|
||
DepthCameraOperation::DepthCameraOperation()
|
||
{
|
||
m_pipe = nullptr;
|
||
m_func = nullptr;
|
||
record = false;
|
||
|
||
m_captureIntervalMilliseconds = 3000;
|
||
}
|
||
|
||
DepthCameraOperation::~DepthCameraOperation()
|
||
{
|
||
if (m_pipe)
|
||
{
|
||
m_pipe->stop();
|
||
|
||
delete m_pipe;
|
||
m_pipe = nullptr;
|
||
}
|
||
}
|
||
|
||
void DepthCameraOperation::setCaptureInterval(int captureIntervalSeconds)
|
||
{
|
||
m_captureIntervalMilliseconds = captureIntervalSeconds * 1000;
|
||
}
|
||
|
||
void DepthCameraOperation::OpenDepthCamera()
|
||
{
|
||
if (m_pipe)
|
||
{
|
||
return;
|
||
}
|
||
m_pipe = new ob::Pipeline();
|
||
|
||
std::shared_ptr<ob::Config> config = std::make_shared<ob::Config>();
|
||
|
||
// Get device from pipeline.
|
||
auto device = m_pipe->getDevice();
|
||
auto devInfo = device->getDeviceInfo();
|
||
auto pid = devInfo->getPid();
|
||
auto vid = devInfo->getVid();
|
||
|
||
//// Get sensorList from device.
|
||
//auto sensorList = device->getSensorList();
|
||
|
||
//for (uint32_t index = 0; index < sensorList->getCount(); index++) {
|
||
// // Query all supported infrared sensor type and enable the infrared stream.
|
||
// // For dual infrared device, enable the left and right infrared streams.
|
||
// // For single infrared device, enable the infrared stream.
|
||
// OBSensorType sensorType = sensorList->getSensorType(index);
|
||
// std::cout << "Supported Sensor type: " << sensorType << std::endl;
|
||
|
||
// // Enable the stream for the sensor type.
|
||
// config->enableStream(sensorType);
|
||
//}
|
||
|
||
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);
|
||
|
||
m_pipe->enableFrameSync();
|
||
|
||
// Create a format converter filter.
|
||
auto formatConverter = std::make_shared<ob::FormatConvertFilter>();
|
||
|
||
m_pipe->start(config);
|
||
|
||
// Drop several frames
|
||
for (int i = 0; i < 15; ++i) {
|
||
auto lost = m_pipe->waitForFrameset(m_captureIntervalMilliseconds);
|
||
}
|
||
|
||
auto pointCloud = std::make_shared<ob::PointCloudFilter>();
|
||
|
||
int frameIndex = 0;
|
||
record = true;
|
||
QString fileNamePrefix = AppSettings::instance().depthCameraDataFolder() + QDir::separator() + "Gemini336L";
|
||
QString imuFilePath = fileNamePrefix + "_IMU.txt";
|
||
std::ofstream imuFile(imuFilePath.toStdString(), std::ios::out | std::ios::trunc);
|
||
while (record)
|
||
{
|
||
if(frameIndex==0)
|
||
{
|
||
emit CamOpenedSignal();
|
||
std::cout << "Start recording..." << std::endl;
|
||
}
|
||
|
||
auto frameSet = m_pipe->waitForFrameset(m_captureIntervalMilliseconds);
|
||
if (frameSet == nullptr)
|
||
{
|
||
std::cout << "No frames received in 100ms..." << std::endl;
|
||
continue;
|
||
}
|
||
|
||
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>();
|
||
|
||
// Convert the color frame to RGB format.
|
||
if (colorFrame->format() != OB_FORMAT_RGB) {
|
||
if (colorFrame->format() == OB_FORMAT_MJPG) {
|
||
formatConverter->setFormatConvertType(FORMAT_MJPG_TO_RGB);
|
||
}
|
||
else if (colorFrame->format() == OB_FORMAT_UYVY) {
|
||
formatConverter->setFormatConvertType(FORMAT_UYVY_TO_RGB);
|
||
}
|
||
else if (colorFrame->format() == OB_FORMAT_YUYV) {
|
||
formatConverter->setFormatConvertType(FORMAT_YUYV_TO_RGB);
|
||
}
|
||
else {
|
||
std::cout << "Color format is not support!" << std::endl;
|
||
continue;
|
||
}
|
||
colorFrame = formatConverter->process(colorFrame)->as<ob::ColorFrame>();
|
||
}
|
||
// Processed the color frames to BGR format, use OpenCV to save to disk.
|
||
formatConverter->setFormatConvertType(FORMAT_RGB_TO_BGR);
|
||
colorFrame = formatConverter->process(colorFrame)->as<ob::ColorFrame>();
|
||
|
||
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 + "_PointCloud_"+ 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;
|
||
m_pipe = nullptr;
|
||
}
|
||
|
||
void DepthCameraOperation::OpenDepthCamera_getDepthValue()
|
||
{
|
||
if (m_pipe)
|
||
{
|
||
return;
|
||
}
|
||
m_pipe = new ob::Pipeline();
|
||
|
||
std::shared_ptr<ob::Config> config = std::make_shared<ob::Config>();
|
||
|
||
// Get device from pipeline.
|
||
auto device = m_pipe->getDevice();
|
||
auto devInfo = device->getDeviceInfo();
|
||
auto pid = devInfo->getPid();
|
||
auto vid = devInfo->getVid();
|
||
|
||
config->enableVideoStream(OB_STREAM_DEPTH, 640, 480, 15, OB_FORMAT_Y16);
|
||
config->enableVideoStream(OB_STREAM_COLOR, 640, 480, 15, OB_FORMAT_YUYV);
|
||
config->setFrameAggregateOutputMode(OB_FRAME_AGGREGATE_OUTPUT_ALL_TYPE_FRAME_REQUIRE);
|
||
|
||
m_pipe->enableFrameSync();
|
||
|
||
// Create a format converter filter.
|
||
auto formatConverter = std::make_shared<ob::FormatConvertFilter>();
|
||
|
||
m_pipe->start(config);
|
||
|
||
// Drop several frames
|
||
for (int i = 0; i < 15; ++i) {
|
||
auto lost = m_pipe->waitForFrameset(m_captureIntervalMilliseconds);
|
||
}
|
||
|
||
auto pointCloud = std::make_shared<ob::PointCloudFilter>();
|
||
|
||
int frameIndex = 0;
|
||
record = true;
|
||
QString fileNamePrefix = AppSettings::instance().depthCameraDataFolder() + QDir::separator() + "Gemini336L";
|
||
|
||
// 增量平均所需的变量
|
||
cv::Mat avgRgbMat, avgDepthMat;
|
||
int avgFrameCount = 0;
|
||
|
||
for (size_t i = 0; i < m_averageNumberOfTimes; i++)
|
||
{
|
||
if(frameIndex==0)
|
||
{
|
||
emit CamOpenedSignal();
|
||
std::cout << "Start recording..." << std::endl;
|
||
}
|
||
|
||
auto frameSet = m_pipe->waitForFrameset(m_captureIntervalMilliseconds);
|
||
if (frameSet == nullptr)
|
||
{
|
||
std::cout << "No frames received in 100ms..." << std::endl;
|
||
continue;
|
||
}
|
||
|
||
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>();
|
||
|
||
// Convert the color frame to RGB format.
|
||
if (colorFrame->format() != OB_FORMAT_RGB) {
|
||
if (colorFrame->format() == OB_FORMAT_MJPG) {
|
||
formatConverter->setFormatConvertType(FORMAT_MJPG_TO_RGB);
|
||
}
|
||
else if (colorFrame->format() == OB_FORMAT_UYVY) {
|
||
formatConverter->setFormatConvertType(FORMAT_UYVY_TO_RGB);
|
||
}
|
||
else if (colorFrame->format() == OB_FORMAT_YUYV) {
|
||
formatConverter->setFormatConvertType(FORMAT_YUYV_TO_RGB);
|
||
}
|
||
else {
|
||
std::cout << "Color format is not support!" << std::endl;
|
||
continue;
|
||
}
|
||
colorFrame = formatConverter->process(colorFrame)->as<ob::ColorFrame>();
|
||
}
|
||
// Processed the color frames to BGR format, use OpenCV to save to disk.
|
||
formatConverter->setFormatConvertType(FORMAT_RGB_TO_BGR);
|
||
colorFrame = formatConverter->process(colorFrame)->as<ob::ColorFrame>();
|
||
|
||
//用于测试：保存深度图像
|
||
//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());
|
||
|
||
// 增量平均计算
|
||
if (avgFrameCount == 0)
|
||
{
|
||
avgRgbMat = cv::Mat::zeros(rgbMat.size(), CV_32FC3);
|
||
avgDepthMat = cv::Mat::zeros(depthMat.size(), CV_32F);
|
||
}
|
||
avgFrameCount++;
|
||
cv::Mat rgbFloat;
|
||
rgbMat.convertTo(rgbFloat, CV_32FC3);
|
||
avgRgbMat = avgRgbMat + (rgbFloat - avgRgbMat) / avgFrameCount;
|
||
|
||
cv::Mat depthMatTmp;
|
||
depthMat.convertTo(depthMatTmp, CV_32F);
|
||
avgDepthMat = avgDepthMat + (depthMatTmp - avgDepthMat) / avgFrameCount;
|
||
|
||
|
||
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();
|
||
|
||
frameIndex++;
|
||
}
|
||
m_pipe->stop();
|
||
|
||
// 对累积平均后的图像进行处理
|
||
double depthValue;
|
||
if (avgFrameCount > 0)
|
||
{
|
||
cv::Mat avgRgbResult, avgDepthResult;
|
||
avgRgbMat.convertTo(avgRgbResult, CV_8UC3);
|
||
avgDepthMat.convertTo(avgDepthResult, CV_16UC1);
|
||
|
||
// 保存平均结果图像
|
||
std::vector<int> pngParams;
|
||
pngParams.push_back(cv::IMWRITE_PNG_COMPRESSION);
|
||
pngParams.push_back(0);
|
||
pngParams.push_back(cv::IMWRITE_PNG_STRATEGY);
|
||
pngParams.push_back(cv::IMWRITE_PNG_STRATEGY_DEFAULT);
|
||
cv::imwrite(getTestFilePath("_AvgRGB_").toStdString(), avgRgbResult, pngParams);
|
||
cv::imwrite(getTestFilePath("_AvgDepth_").toStdString(), avgDepthResult, pngParams);
|
||
|
||
// 创建掩膜，排除深度值为0的区域
|
||
cv::Mat mask = avgDepthResult != 0;
|
||
|
||
// 保存掩膜
|
||
cv::Mat mask8U;
|
||
mask.convertTo(mask8U, CV_8UC1, 255.0);
|
||
std::string maskName = fileNamePrefix.toStdString() + "_Mask_" + std::to_string(mask.cols) + "x" + std::to_string(mask.rows) + ".png";
|
||
cv::imwrite(maskName, mask8U, pngParams);
|
||
|
||
if (m_depthAlgorithm == 0)
|
||
{
|
||
depthValue = processAveragedImages_roiAvg(avgDepthResult, mask);
|
||
}
|
||
else if (m_depthAlgorithm == 1)
|
||
{
|
||
depthValue = processAveragedImages_depthRangePercentage(avgDepthResult, mask);
|
||
}
|
||
else if (m_depthAlgorithm == 2)
|
||
{
|
||
depthValue = processAveragedImages_segmentation(avgRgbResult, avgDepthResult, mask);
|
||
}
|
||
}
|
||
|
||
//计算平均深度值
|
||
std::cout << "Depth value: " << depthValue << " m" << std::endl;
|
||
emit DepthValueSignal(depthValue);
|
||
|
||
delete m_pipe;
|
||
m_pipe = nullptr;
|
||
|
||
record = false;
|
||
}
|
||
|
||
double DepthCameraOperation::processAveragedImages_roiAvg(const cv::Mat& avgDepthResult, const cv::Mat& mask)
|
||
{
|
||
//裁剪边缘区域
|
||
int cropRows = static_cast<int>(avgDepthResult.rows * (1 - m_percentageOfEffectiveArea) / 2);
|
||
int cropCols = static_cast<int>(avgDepthResult.cols * (1 - m_percentageOfEffectiveArea) / 2);
|
||
cv::Rect roi(cropCols, cropRows,
|
||
avgDepthResult.cols - 2 * cropCols,
|
||
avgDepthResult.rows - 2 * cropRows);
|
||
cv::Mat depthRoi = avgDepthResult(roi);
|
||
cv::Mat maskRoi = mask(roi);
|
||
//计算平均深度值，使用掩膜排除深度值为0的区域
|
||
cv::Scalar meanDepth = cv::mean(depthRoi, maskRoi);
|
||
double depthValue = meanDepth[0] / 1000.0; // 转换为米
|
||
|
||
return depthValue;
|
||
}
|
||
|
||
double DepthCameraOperation::processAveragedImages_depthRangePercentage(const cv::Mat& avgDepthResult, const cv::Mat& mask)
|
||
{
|
||
// 找出最大最小值
|
||
double minVal, maxVal;
|
||
cv::minMaxLoc(avgDepthResult, &minVal, &maxVal, nullptr, nullptr, mask);
|
||
|
||
// 检查是否有有效数据
|
||
if (minVal == std::numeric_limits<double>::max())
|
||
{
|
||
std::cout << "Warning: No valid depth pixels found in masked region!" << std::endl;
|
||
return 0.0;
|
||
}
|
||
|
||
// 返回最小值乘以m_depthRangePercentage
|
||
double depthValue = minVal * m_depthRangePercentage / 1000.0; // 转换为米
|
||
|
||
return depthValue;
|
||
}
|
||
|
||
double DepthCameraOperation::processAveragedImages_segmentation(const cv::Mat& avgRgbResult, const cv::Mat& avgDepthResult, const cv::Mat& mask)
|
||
{
|
||
// 转换到 HSV 颜色空间进行植被分割
|
||
cv::Mat hsvMat;
|
||
cv::cvtColor(avgRgbResult, hsvMat, cv::COLOR_RGB2HSV);
|
||
|
||
// 分离通道
|
||
std::vector<cv::Mat> hsvChannels;
|
||
cv::split(hsvMat, hsvChannels);
|
||
cv::Mat hue = hsvChannels[0];
|
||
cv::Mat sat = hsvChannels[1];
|
||
cv::Mat val = hsvChannels[2];
|
||
|
||
// 定义绿色植被的Hue范围 (OpenCV中Hue范围是0-180，实际绿色约35-90度)
|
||
// 扩展范围以覆盖不同光照条件下的绿色
|
||
cv::Mat hueMask1 = (hue >= 35) & (hue <= 85);
|
||
cv::Mat satMask = sat > 30; // 饱和度阈值，去除灰色区域
|
||
cv::Mat valMask = val > 50; // 亮度阈值，去除过暗区域
|
||
|
||
// 组合条件生成植被掩膜
|
||
cv::Mat vmask;
|
||
cv::bitwise_and(hueMask1, satMask, vmask);
|
||
cv::bitwise_and(vmask, valMask, vmask);
|
||
|
||
// 结合深度掩膜，计算两个mask的交集
|
||
cv::Mat combinedMask;
|
||
cv::bitwise_and(mask, vmask, combinedMask);
|
||
|
||
// 保存 vmask 和 combinedMask 到 exe 所在文件夹的文件夹
|
||
cv::Mat vmask8U, combinedMask8U;
|
||
vmask.convertTo(vmask8U, CV_8UC1, 255.0);
|
||
combinedMask.convertTo(combinedMask8U, CV_8UC1, 255.0);
|
||
cv::imwrite(getTestFilePath("vmask").toStdString(), vmask8U);
|
||
cv::imwrite(getTestFilePath("combinedMask").toStdString(), combinedMask8U);
|
||
|
||
// 计算 avgRgbResult 在 combinedMask 区域内的平均深度值
|
||
double depthValue = 0.0;
|
||
if (cv::countNonZero(combinedMask) > 0) {
|
||
cv::Scalar meanDepth = cv::mean(avgDepthResult, combinedMask);
|
||
depthValue = meanDepth[0] / 1000.0; // 转换为米
|
||
} else {
|
||
std::cout << "Warning: No valid pixels in combined mask!" << std::endl;
|
||
}
|
||
|
||
return depthValue;
|
||
}
|
||
|
||
QString DepthCameraOperation::getTestFilePath(const QString& fileName)
|
||
{
|
||
QString testFolder = QCoreApplication::applicationDirPath() + QDir::separator() + "depthValueTest";
|
||
QDir().mkpath(testFolder);
|
||
QString timestamp = QString::number(QDateTime::currentMSecsSinceEpoch());
|
||
return testFolder + QDir::separator() + fileName + "_" + timestamp + ".png";
|
||
}
|
||
|
||
void DepthCameraOperation::saveDepthFrame(const std::shared_ptr<ob::DepthFrame> depthFrame, const uint32_t frameIndex, std::string fileNamePrefix_)
|
||
{
|
||
std::vector<int> params;
|
||
params.push_back(cv::IMWRITE_PNG_COMPRESSION);
|
||
params.push_back(0);
|
||
params.push_back(cv::IMWRITE_PNG_STRATEGY);
|
||
params.push_back(cv::IMWRITE_PNG_STRATEGY_DEFAULT);
|
||
std::string depthName = fileNamePrefix_ + "_Depth_" + std::to_string(depthFrame->width()) + "x" + std::to_string(depthFrame->height()) + "_" + std::to_string(frameIndex) + "_"
|
||
+ 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;
|
||
}
|
||
|
||
void DepthCameraOperation::saveColorFrame(const std::shared_ptr<ob::ColorFrame> colorFrame, const uint32_t frameIndex, std::string fileNamePrefix_)
|
||
{
|
||
std::vector<int> params;
|
||
params.push_back(cv::IMWRITE_PNG_COMPRESSION);
|
||
params.push_back(0);
|
||
params.push_back(cv::IMWRITE_PNG_STRATEGY);
|
||
params.push_back(cv::IMWRITE_PNG_STRATEGY_DEFAULT);
|
||
std::string colorName = fileNamePrefix_ + "_Color_" + std::to_string(colorFrame->width()) + "x" + std::to_string(colorFrame->height()) + "_" + std::to_string(frameIndex) + "_"
|
||
+ 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;
|
||
}
|
||
|
||
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()
|
||
{
|
||
|
||
}
|
||
|
||
void DepthCameraOperation::setCallback(void(*func)())
|
||
{
|
||
m_func = func;
|
||
}
|
||
|
||
void DepthCameraOperation::CloseDepthCamera()
|
||
{
|
||
std::cout << "DepthCameraOperation::CloseDepthCamera，关闭深度相机" << std::endl;
|
||
|
||
record = false;
|
||
|
||
emit CamClosedSignal();
|
||
}
|