342 lines
13 KiB
C++
342 lines
13 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(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());
|
||
|
||
if (!dir.isEmpty())
|
||
{
|
||
ui.dataFolderLineEdit->setText(dir);
|
||
AppSettings::instance().setDepthCameraDataFolder(dir);
|
||
}
|
||
}
|
||
|
||
void DepthCameraWindow::openDepthCamera()
|
||
{
|
||
if (!m_DepthCameraOperation->getRecordStatus())
|
||
{
|
||
emit openDepthCameraSignal();
|
||
}
|
||
}
|
||
|
||
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;
|
||
}
|
||
|
||
DepthCameraOperation::~DepthCameraOperation()
|
||
{
|
||
if (m_pipe)
|
||
{
|
||
m_pipe->stop();
|
||
|
||
delete m_pipe;
|
||
m_pipe = nullptr;
|
||
}
|
||
}
|
||
|
||
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(100);
|
||
}
|
||
|
||
auto pointCloud = std::make_shared<ob::PointCloudFilter>();
|
||
|
||
int frameIndex = 0;
|
||
record = true;
|
||
QString fileNamePrefix = AppSettings::instance().depthCameraDataFolder() + 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)
|
||
{
|
||
emit CamOpenedSignal();
|
||
std::cout << "Start recording..." << std::endl;
|
||
}
|
||
|
||
auto frameSet = m_pipe->waitForFrameset(100);
|
||
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 + "_"+ 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::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();
|
||
}
|