920 lines
31 KiB
C++
920 lines
31 KiB
C++
#include <vtkAxesActor.h>
|
||
#include <vtkOrientationMarkerWidget.h>
|
||
#include <vtkRenderWindow.h>
|
||
#include <vtkRenderWindowInteractor.h>
|
||
#include <vtkTextActor.h>
|
||
#include <vtkTextProperty.h>
|
||
#include <vtkCoordinate.h>
|
||
#include <vtkRendererCollection.h>
|
||
|
||
#include "CloudShow.h"
|
||
#include "LaserDataLoader.h"
|
||
|
||
#include <pcl/io/pcd_io.h>
|
||
|
||
#include <chrono>
|
||
#include <cmath>
|
||
#include <cstdio>
|
||
#include <ctime>
|
||
#include <exception>
|
||
#include <filesystem>
|
||
#include <iostream>
|
||
#include <new>
|
||
#include <sstream>
|
||
|
||
// ============================================================
|
||
// 工厂方法
|
||
// ============================================================
|
||
std::unique_ptr<ICloudShow> ICloudShow::Create()
|
||
{
|
||
return std::make_unique<CCloudShow>();
|
||
}
|
||
|
||
CCloudShow::CCloudShow() = default;
|
||
|
||
CCloudShow::~CCloudShow()
|
||
{
|
||
Stop();
|
||
}
|
||
|
||
// ============================================================
|
||
// 内部：根据数据类型返回单点字节数
|
||
// ============================================================
|
||
static size_t pointSize(EVzResultDataType t)
|
||
{
|
||
switch (t)
|
||
{
|
||
case keResultDataType_PointXYZI: return sizeof(SVzNLPointXYZI);
|
||
case keResultDataType_PointXYZRGBA: return sizeof(SVzNLPointXYZRGBA);
|
||
case keResultDataType_PointF: return sizeof(SVzNL3DPointF);
|
||
case keResultDataType_Position: return sizeof(SVzNL3DPosition);
|
||
default: return 0;
|
||
}
|
||
}
|
||
|
||
// ============================================================
|
||
// 内部：CloudData → PCL（仅处理 PointXYZI 类型，其余跳过）
|
||
// ============================================================
|
||
static pcl::PointCloud<pcl::PointXYZI>::Ptr toPcl(const ICloudShow::CloudData& cloudData)
|
||
{
|
||
size_t total = 0;
|
||
for (const auto& p : cloudData)
|
||
{
|
||
if (p.first == keResultDataType_PointXYZI && p.second.p3DPoint)
|
||
total += p.second.nPointCount;
|
||
}
|
||
if (total == 0)
|
||
return nullptr;
|
||
|
||
auto cloud = std::make_shared<pcl::PointCloud<pcl::PointXYZI>>();
|
||
cloud->is_dense = true;
|
||
cloud->points.resize(total); // 一次分配，下标赋值，避免 40 万次 push_back
|
||
size_t idx = 0;
|
||
for (const auto& p : cloudData)
|
||
{
|
||
if (p.first != keResultDataType_PointXYZI || !p.second.p3DPoint)
|
||
continue;
|
||
const auto* src = static_cast<const SVzNLPointXYZI*>(p.second.p3DPoint);
|
||
for (int i = 0; i < p.second.nPointCount; ++i)
|
||
{
|
||
cloud->points[idx].x = src[i].fData[0];
|
||
cloud->points[idx].y = src[i].fData[1];
|
||
cloud->points[idx].z = src[i].fData[2];
|
||
cloud->points[idx].intensity = src[i].fData_c[0];
|
||
++idx;
|
||
}
|
||
}
|
||
cloud->width = static_cast<uint32_t>(idx);
|
||
cloud->height = 1;
|
||
return cloud;
|
||
}
|
||
|
||
// ============================================================
|
||
// Show — 单次显示，阻塞直到关闭窗口
|
||
// ============================================================
|
||
int CCloudShow::Show(const CloudData& cloudData,
|
||
const CloudFrameInfo& info,
|
||
const std::string& windowName)
|
||
{
|
||
auto pclCloud = toPcl(cloudData);
|
||
if (!pclCloud)
|
||
return -1;
|
||
|
||
pcl::visualization::PCLVisualizer viewer(windowName);
|
||
viewer.setSize(670, 450);
|
||
viewer.setBackgroundColor(0.05, 0.05, 0.05);
|
||
viewer.addPointCloud<pcl::PointXYZI>(pclCloud, windowName);
|
||
viewer.setPointCloudRenderingProperties(
|
||
pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2.0, windowName);
|
||
viewer.initCameraParameters();
|
||
viewer.setCameraPosition(0, -100000, -50000, 0, 0, 50000, 0, -1, 0);
|
||
|
||
// 右下角坐标轴
|
||
vtkAxesActor* axes = vtkAxesActor::New();
|
||
vtkOrientationMarkerWidget* widget = vtkOrientationMarkerWidget::New();
|
||
widget->SetOrientationMarker(axes);
|
||
vtkRenderWindowInteractor* iren = viewer.getRenderWindow()->GetInteractor();
|
||
if (!iren)
|
||
{
|
||
iren = vtkRenderWindowInteractor::New();
|
||
viewer.getRenderWindow()->SetInteractor(iren);
|
||
iren->Delete();
|
||
}
|
||
widget->SetInteractor(iren);
|
||
widget->SetViewport(0.85, 0.0, 1.0, 0.15);
|
||
widget->SetEnabled(1);
|
||
widget->InteractiveOff();
|
||
|
||
// SAVE 按钮
|
||
vtkTextActor* textActor = vtkTextActor::New();
|
||
textActor->SetInput("SAVE");
|
||
textActor->GetTextProperty()->SetColor(0.2, 0.8, 0.2);
|
||
textActor->GetTextProperty()->SetFontSize(20);
|
||
textActor->GetPositionCoordinate()->SetCoordinateSystemToNormalizedViewport();
|
||
textActor->SetPosition(0.88, 0.48);
|
||
vtkRenderer* renderer = viewer.getRendererCollection()->GetFirstRenderer();
|
||
if (renderer)
|
||
renderer->AddActor2D(textActor);
|
||
textActor->Delete();
|
||
|
||
viewer.registerMouseCallback([&viewer, this](const pcl::visualization::MouseEvent& event) {
|
||
if (event.getType() != pcl::visualization::MouseEvent::MouseButtonPress)
|
||
return;
|
||
if (event.getButton() != pcl::visualization::MouseEvent::LeftButton)
|
||
return;
|
||
int* sz = viewer.getRenderWindow()->GetSize();
|
||
if (sz[0] <= 0 || sz[1] <= 0)
|
||
return;
|
||
float rx = (float)event.getX() / sz[0];
|
||
float ry = (float)event.getY() / sz[1];
|
||
if (rx >= 0.83f && rx <= 0.94f && ry >= 0.43f && ry <= 0.53f)
|
||
m_bSaveRequested = true;
|
||
});
|
||
|
||
viewer.spin();
|
||
widget->Delete();
|
||
axes->Delete();
|
||
return 0;
|
||
}
|
||
|
||
// ============================================================
|
||
// Start — 主线程创建 viewer，注册键盘/鼠标回调
|
||
// ============================================================
|
||
int CCloudShow::Start(const std::string& windowName, void* hwnd)
|
||
{
|
||
if (m_bRunning)
|
||
return 0;
|
||
|
||
m_windowName = windowName;
|
||
m_bFirstCloud = true;
|
||
m_bQuitRequested = false;
|
||
m_bSaveRequested = false;
|
||
|
||
try
|
||
{
|
||
m_viewer = std::make_shared<pcl::visualization::PCLVisualizer>(windowName);
|
||
m_viewer->setBackgroundColor(0.05, 0.05, 0.05);
|
||
m_viewer->setSize(670, 450);
|
||
m_viewer->initCameraParameters();
|
||
m_viewer->setCameraPosition(0, -100000, -50000, 0, 0, 50000, 0, -1, 0);
|
||
m_viewer->registerKeyboardCallback(&CCloudShow::keyboardCallback, this);
|
||
|
||
#ifdef _WIN32
|
||
if (hwnd)
|
||
m_viewer->getRenderWindow()->SetParentId(hwnd);
|
||
#endif
|
||
|
||
// 右下角坐标轴
|
||
vtkAxesActor* axes = vtkAxesActor::New();
|
||
m_axesWidget = vtkOrientationMarkerWidget::New();
|
||
m_axesWidget->SetOrientationMarker(axes);
|
||
vtkRenderWindowInteractor* iren = m_viewer->getRenderWindow()->GetInteractor();
|
||
if (!iren)
|
||
{
|
||
iren = vtkRenderWindowInteractor::New();
|
||
m_viewer->getRenderWindow()->SetInteractor(iren);
|
||
iren->Delete();
|
||
}
|
||
m_axesWidget->SetInteractor(iren);
|
||
m_axesWidget->SetViewport(0.85, 0.0, 1.0, 0.15);
|
||
m_axesWidget->SetEnabled(1);
|
||
m_axesWidget->InteractiveOff();
|
||
axes->Delete();
|
||
|
||
vtkRenderer* renderer = m_viewer->getRendererCollection()->GetFirstRenderer();
|
||
|
||
// SAVE 按钮
|
||
vtkTextActor* saveActor = vtkTextActor::New();
|
||
saveActor->SetInput("SAVE");
|
||
saveActor->GetTextProperty()->SetColor(0.2, 0.8, 0.2);
|
||
saveActor->GetTextProperty()->SetFontSize(20);
|
||
saveActor->GetPositionCoordinate()->SetCoordinateSystemToNormalizedViewport();
|
||
saveActor->SetPosition(0.88, 0.45);
|
||
if (renderer)
|
||
renderer->AddActor2D(saveActor);
|
||
saveActor->Delete();
|
||
|
||
// START/STOP 按钮
|
||
m_pStartStopBtnActor = vtkTextActor::New();
|
||
m_pStartStopBtnActor->SetInput("START");
|
||
m_pStartStopBtnActor->GetTextProperty()->SetColor(0.2, 0.8, 0.2);
|
||
m_pStartStopBtnActor->GetTextProperty()->SetFontSize(20);
|
||
m_pStartStopBtnActor->GetPositionCoordinate()->SetCoordinateSystemToNormalizedViewport();
|
||
m_pStartStopBtnActor->SetPosition(0.87, 0.35);
|
||
if (renderer)
|
||
renderer->AddActor2D(m_pStartStopBtnActor);
|
||
|
||
// 保存进度文字
|
||
m_pProgressActor = vtkTextActor::New();
|
||
m_pProgressActor->SetInput("");
|
||
m_pProgressActor->GetTextProperty()->SetColor(0.8, 0.8, 0.2);
|
||
m_pProgressActor->GetTextProperty()->SetFontSize(14);
|
||
m_pProgressActor->GetPositionCoordinate()->SetCoordinateSystemToNormalizedViewport();
|
||
m_pProgressActor->SetPosition(0.85, 0.25);
|
||
if (renderer)
|
||
renderer->AddActor2D(m_pProgressActor);
|
||
|
||
m_viewer->registerMouseCallback([this](const pcl::visualization::MouseEvent& event) {
|
||
if (event.getType() != pcl::visualization::MouseEvent::MouseButtonPress)
|
||
return;
|
||
if (event.getButton() != pcl::visualization::MouseEvent::LeftButton)
|
||
return;
|
||
int* sz = m_viewer->getRenderWindow()->GetSize();
|
||
if (sz[0] <= 0 || sz[1] <= 0)
|
||
return;
|
||
float rx = (float)event.getX() / sz[0];
|
||
float ry = (float)event.getY() / sz[1];
|
||
// SAVE 按钮区域
|
||
if (rx >= 0.83f && rx <= 0.94f && ry >= 0.40f && ry <= 0.50f)
|
||
m_bSaveRequested = true;
|
||
// START/STOP 按钮区域
|
||
else if (rx >= 0.83f && rx <= 0.94f && ry >= 0.30f && ry <= 0.40f)
|
||
m_bStartStopRequested = true;
|
||
});
|
||
}
|
||
catch (const std::exception& e)
|
||
{
|
||
std::cerr << "[CloudShow] 创建 PCL 窗口失败: " << e.what() << std::endl;
|
||
return -1;
|
||
}
|
||
catch (...)
|
||
{
|
||
std::cerr << "[CloudShow] 创建 PCL 窗口失败: 未知异常" << std::endl;
|
||
return -1;
|
||
}
|
||
|
||
// 预分配复用缓冲区，避免热路径 realloc 导致卡顿
|
||
{
|
||
// 显示缓冲：~500k 点 × 16B = 8MB
|
||
m_renderPool = std::make_shared<pcl::PointCloud<pcl::PointXYZI>>();
|
||
m_renderPool->points.reserve(500000);
|
||
|
||
// S 键缓存缓冲：600 线 × 1500 点 × 32B ≈ 29MB
|
||
m_lastCloudBuffers.resize(600);
|
||
for (auto& buf : m_lastCloudBuffers)
|
||
buf.reserve(1500 * sizeof(SVzNLPointXYZI));
|
||
}
|
||
|
||
m_bRunning = true;
|
||
return 0;
|
||
}
|
||
|
||
// ============================================================
|
||
// PushFrame — 更新显示 + 入保存队列
|
||
// ============================================================
|
||
void CCloudShow::PushFrame(const CloudData& cloudData, const CloudFrameInfo& info)
|
||
{
|
||
if (!m_bRunning)
|
||
return;
|
||
|
||
try
|
||
{
|
||
// 保存进行中（m_saveTotal > 0 表示 STOP 已点击，saveLoop 正在写盘）
|
||
// 此时跳过显示更新和 S 键缓存，避免与写盘争抢内存/IO 导致 UI 卡死
|
||
const bool bSaving = (m_saveTotal.load() > 0);
|
||
|
||
if (!bSaving)
|
||
{
|
||
// 仅非连续保存时缓存最近一帧 CloudData 的深拷贝（供 S 键单帧保存）
|
||
if (!m_bSaveActive.load())
|
||
{
|
||
// 1. 锁内：取出旧数据（swap 只交换指针，极快）
|
||
CloudData oldData;
|
||
std::vector<std::vector<uint8_t>> oldBuffers;
|
||
{
|
||
std::lock_guard<std::mutex> lk(m_lastCloudMutex);
|
||
m_lastFrameInfo = info;
|
||
oldData.swap(m_lastCloudData);
|
||
oldBuffers.swap(m_lastCloudBuffers);
|
||
}
|
||
|
||
// 2. 锁外：复用 oldBuffers，resize + memcpy（零分配）
|
||
oldBuffers.resize(cloudData.size());
|
||
oldData.clear();
|
||
oldData.reserve(cloudData.size());
|
||
for (size_t i = 0; i < cloudData.size(); ++i)
|
||
{
|
||
const auto& src = cloudData[i];
|
||
oldData.push_back(src);
|
||
auto& ld = oldData.back().second;
|
||
ld.p2DPoint = nullptr;
|
||
if (ld.p3DPoint && ld.nPointCount > 0)
|
||
{
|
||
size_t sz = pointSize(src.first) * static_cast<size_t>(ld.nPointCount);
|
||
if (sz > 0)
|
||
{
|
||
// 加 4KB margin，避免点数的帧间微小波动导致反复 realloc
|
||
if (oldBuffers[i].capacity() < sz)
|
||
oldBuffers[i].reserve(sz + 4096);
|
||
oldBuffers[i].resize(sz);
|
||
std::memcpy(oldBuffers[i].data(), ld.p3DPoint, sz);
|
||
ld.p3DPoint = oldBuffers[i].data();
|
||
}
|
||
else { ld.p3DPoint = nullptr; }
|
||
}
|
||
}
|
||
|
||
// 3. 锁内：写入新数据（swap 只交换指针，极快）
|
||
{
|
||
std::lock_guard<std::mutex> lk(m_lastCloudMutex);
|
||
m_lastCloudData.swap(oldData);
|
||
m_lastCloudBuffers.swap(oldBuffers);
|
||
}
|
||
}
|
||
|
||
// 显示：复用 m_renderPool，resize + 下标赋值（零分配）
|
||
{
|
||
size_t total = 0;
|
||
for (const auto& p : cloudData)
|
||
if (p.first == keResultDataType_PointXYZI && p.second.p3DPoint)
|
||
total += p.second.nPointCount;
|
||
|
||
if (total > 0)
|
||
{
|
||
if (!m_renderPool)
|
||
m_renderPool = std::make_shared<pcl::PointCloud<pcl::PointXYZI>>();
|
||
auto& cloud = *m_renderPool;
|
||
cloud.is_dense = true;
|
||
if (cloud.points.capacity() < total)
|
||
cloud.points.reserve(total + 2048);
|
||
cloud.points.resize(total);
|
||
size_t idx = 0;
|
||
for (const auto& p : cloudData)
|
||
{
|
||
if (p.first != keResultDataType_PointXYZI || !p.second.p3DPoint)
|
||
continue;
|
||
const auto* src = static_cast<const SVzNLPointXYZI*>(p.second.p3DPoint);
|
||
for (int i = 0; i < p.second.nPointCount; ++i)
|
||
{
|
||
cloud.points[idx].x = src[i].fData[0];
|
||
cloud.points[idx].y = src[i].fData[1];
|
||
cloud.points[idx].z = src[i].fData[2];
|
||
cloud.points[idx].intensity = src[i].fData_c[0];
|
||
++idx;
|
||
}
|
||
}
|
||
cloud.width = static_cast<uint32_t>(idx);
|
||
cloud.height = 1;
|
||
|
||
std::lock_guard<std::mutex> lk(m_cloudMutex);
|
||
m_displayCloud.swap(m_renderPool); // 旧 displayCloud 变成新的 renderPool
|
||
m_bUpdated = true;
|
||
}
|
||
}
|
||
}
|
||
|
||
// 保存：按帧间隔缓存（默认每10帧存1帧）
|
||
// 注意：m_nPushFrameCount 仅回调线程独占写入，无需原子操作
|
||
m_nPushFrameCount++;
|
||
if (m_bSaveActive.load() && (m_nPushFrameCount % m_nSaveFrameInterval == 0))
|
||
{
|
||
SaveTask task = acquireTask(); // 从池取预分配 task
|
||
packIntoTask(task, cloudData, info); // memcpy 到预分配 buffer
|
||
|
||
{
|
||
std::lock_guard<std::mutex> lk(m_saveMutex);
|
||
if (m_bSaveActive.load()) // 双重检查：STOP 可能在上一步期间触发
|
||
{
|
||
if (!task.cloudData.empty())
|
||
m_saveQueue.emplace_back(std::move(task));
|
||
else
|
||
releaseTask(task);
|
||
m_saveCv.notify_one();
|
||
}
|
||
else
|
||
{
|
||
releaseTask(task); // STOP 已点击，丢弃此帧
|
||
}
|
||
}
|
||
}
|
||
}
|
||
catch (const std::bad_alloc&)
|
||
{
|
||
m_nSaveDropped.fetch_add(1, std::memory_order_relaxed);
|
||
}
|
||
catch (const std::exception&)
|
||
{
|
||
m_nSaveDropped.fetch_add(1, std::memory_order_relaxed);
|
||
}
|
||
catch (...)
|
||
{
|
||
m_nSaveDropped.fetch_add(1, std::memory_order_relaxed);
|
||
}
|
||
}
|
||
|
||
// ============================================================
|
||
// Stop — 关闭窗口，停止保存线程
|
||
// ============================================================
|
||
int CCloudShow::Stop()
|
||
{
|
||
if (!m_bRunning)
|
||
return 0;
|
||
|
||
m_bRunning = false;
|
||
m_bSaveActive = false;
|
||
m_bFlushAndSave = false;
|
||
m_saveExit = true;
|
||
m_saveCv.notify_all();
|
||
|
||
for (auto& t : m_saveThreads)
|
||
{
|
||
if (t.joinable())
|
||
t.join();
|
||
}
|
||
m_saveThreads.clear();
|
||
|
||
// 释放缓冲池 + 保存队列残余内存
|
||
{
|
||
std::lock_guard<std::mutex> lk(m_taskPoolMutex);
|
||
m_taskPool.clear();
|
||
}
|
||
{
|
||
std::lock_guard<std::mutex> lk(m_saveMutex);
|
||
m_saveQueue.clear();
|
||
}
|
||
|
||
if (m_axesWidget)
|
||
{
|
||
m_axesWidget->Delete();
|
||
m_axesWidget = nullptr;
|
||
}
|
||
if (m_pStartStopBtnActor)
|
||
{
|
||
m_pStartStopBtnActor->Delete();
|
||
m_pStartStopBtnActor = nullptr;
|
||
}
|
||
if (m_pProgressActor)
|
||
{
|
||
m_pProgressActor->Delete();
|
||
m_pProgressActor = nullptr;
|
||
}
|
||
if (m_viewer)
|
||
{
|
||
m_viewer->close();
|
||
m_viewer.reset();
|
||
}
|
||
|
||
return 0;
|
||
}
|
||
|
||
bool CCloudShow::IsRunning() const
|
||
{
|
||
return m_bRunning;
|
||
}
|
||
|
||
bool CCloudShow::IsQuitRequested() const
|
||
{
|
||
return m_bQuitRequested;
|
||
}
|
||
|
||
bool CCloudShow::IsSaveRequested() const
|
||
{
|
||
return m_bSaveRequested;
|
||
}
|
||
|
||
void CCloudShow::ClearSaveRequest()
|
||
{
|
||
m_bSaveRequested = false;
|
||
}
|
||
|
||
bool CCloudShow::IsSaveActive() const
|
||
{
|
||
return m_bSaveActive.load();
|
||
}
|
||
|
||
void CCloudShow::SetSaveBaseDir(const std::string& dir)
|
||
{
|
||
m_saveBaseDir = dir;
|
||
|
||
// 扫描已有 LaserData_*.txt，从最大序号 +1 开始
|
||
int maxSeq = 0;
|
||
std::error_code ec;
|
||
for (const auto& entry : std::filesystem::directory_iterator(m_saveBaseDir, ec))
|
||
{
|
||
if (ec) break;
|
||
std::string name = entry.path().filename().string();
|
||
int n = 0;
|
||
if (sscanf(name.c_str(), "LaserData_%d.txt", &n) == 1)
|
||
{
|
||
if (n > maxSeq) maxSeq = n;
|
||
}
|
||
}
|
||
m_nSingleSaveSeq = maxSeq;
|
||
m_nSaveSeq = maxSeq;
|
||
std::cout << "[CloudShow] 保存基目录: " << m_saveBaseDir
|
||
<< " 已有最大序号: " << maxSeq << std::endl;
|
||
}
|
||
|
||
void CCloudShow::SetSaveFrameInterval(int interval)
|
||
{
|
||
if (interval < 1) interval = 1;
|
||
m_nSaveFrameInterval = interval;
|
||
std::cout << "[CloudShow] 保存帧间隔: " << interval << std::endl;
|
||
}
|
||
|
||
// ============================================================
|
||
// 内部：START/STOP 切换
|
||
// ============================================================
|
||
void CCloudShow::handleStartStopToggle()
|
||
{
|
||
if (m_saveTotal.load() > 0 && !m_bSaveActive.load())
|
||
return; // 刷新中，忽略
|
||
|
||
if (!m_bSaveActive.load())
|
||
{
|
||
// === START：开始缓存数据到内存（不写盘） ===
|
||
m_nSaveSeq = 0; // 每次会话从 1 开始
|
||
m_saveCompleted = 0;
|
||
m_saveTotal = 0;
|
||
m_bFlushAndSave = false; // 保存线程等待，不写盘
|
||
|
||
auto now = std::time(nullptr);
|
||
std::ostringstream dirOss;
|
||
dirOss << m_saveBaseDir << "/"
|
||
<< std::put_time(std::localtime(&now), "%Y%m%d_%H%M%S");
|
||
m_currentSessionDir = dirOss.str();
|
||
|
||
std::error_code ec;
|
||
std::filesystem::create_directories(m_currentSessionDir, ec);
|
||
if (ec)
|
||
{
|
||
std::cerr << "[CloudShow] 创建目录失败: "
|
||
<< m_currentSessionDir << std::endl;
|
||
return;
|
||
}
|
||
|
||
if (m_saveThreads.empty())
|
||
{
|
||
m_saveExit = false;
|
||
for (int i = 0; i < SAVE_THREAD_COUNT; ++i)
|
||
m_saveThreads.emplace_back(&CCloudShow::saveLoop, this);
|
||
}
|
||
|
||
m_bSaveActive = true;
|
||
updateStartStopButton();
|
||
updateProgressText();
|
||
std::cout << "[CloudShow] 开始缓存 -> "
|
||
<< m_currentSessionDir << std::endl;
|
||
}
|
||
else
|
||
{
|
||
// === STOP：停止缓存，2线程写盘 ===
|
||
m_bSaveActive = false;
|
||
{
|
||
std::lock_guard<std::mutex> lk(m_saveMutex);
|
||
m_saveTotal = m_saveCompleted.load()
|
||
+ static_cast<int>(m_saveQueue.size());
|
||
}
|
||
m_bFlushAndSave = true; // 通知保存线程开始写盘
|
||
m_saveCv.notify_all(); // 唤醒 2 个保存线程
|
||
updateStartStopButton();
|
||
std::cout << "[CloudShow] 停止缓存，写盘 "
|
||
<< m_saveTotal.load() << " 帧..." << std::endl;
|
||
}
|
||
}
|
||
|
||
void CCloudShow::updateStartStopButton()
|
||
{
|
||
if (!m_pStartStopBtnActor)
|
||
return;
|
||
|
||
std::lock_guard<std::mutex> lk(m_textMutex);
|
||
if (m_bSaveActive.load())
|
||
{
|
||
m_pStartStopBtnActor->SetInput("STOP");
|
||
m_pStartStopBtnActor->GetTextProperty()->SetColor(1.0, 0.3, 0.3);
|
||
}
|
||
else
|
||
{
|
||
m_pStartStopBtnActor->SetInput("START");
|
||
m_pStartStopBtnActor->GetTextProperty()->SetColor(0.2, 0.8, 0.2);
|
||
}
|
||
m_pStartStopBtnActor->Modified();
|
||
}
|
||
|
||
void CCloudShow::updateProgressText()
|
||
{
|
||
if (!m_pProgressActor)
|
||
return;
|
||
|
||
std::lock_guard<std::mutex> lk(m_textMutex);
|
||
int done = m_saveCompleted.load();
|
||
int total = m_saveTotal.load();
|
||
int dropped = m_nSaveDropped.load();
|
||
|
||
std::ostringstream oss;
|
||
if (total > 0)
|
||
oss << "Saved: " << done << "/" << total;
|
||
else if (done > 0)
|
||
oss << "Saved: " << done;
|
||
|
||
{
|
||
std::lock_guard<std::mutex> lk2(m_saveMutex);
|
||
if (!m_saveQueue.empty())
|
||
oss << " Q:" << m_saveQueue.size();
|
||
}
|
||
if (dropped > 0)
|
||
oss << " drop:" << dropped;
|
||
|
||
m_pProgressActor->SetInput(oss.str().c_str());
|
||
m_pProgressActor->Modified();
|
||
}
|
||
|
||
// ============================================================
|
||
// 内部：保存线程（多线程共享队列）
|
||
// ============================================================
|
||
void CCloudShow::saveLoop()
|
||
{
|
||
while (!m_saveExit.load())
|
||
{
|
||
SaveTask task; // 在 try 外声明，catch 块可访问
|
||
bool haveTask = false;
|
||
try
|
||
{
|
||
{
|
||
std::unique_lock<std::mutex> lk(m_saveMutex);
|
||
// 等待条件：退出 或 (STOP已点击 且 队列非空)
|
||
m_saveCv.wait(lk, [this] {
|
||
return m_saveExit.load()
|
||
|| (m_bFlushAndSave.load() && !m_saveQueue.empty());
|
||
});
|
||
if (m_saveQueue.empty() || !m_bFlushAndSave.load())
|
||
{
|
||
if (m_saveExit.load())
|
||
return;
|
||
continue; // 虚假唤醒或 START 期间，继续等待
|
||
}
|
||
task = std::move(m_saveQueue.front());
|
||
m_saveQueue.pop_front();
|
||
haveTask = true;
|
||
}
|
||
|
||
const int seq = ++m_nSaveSeq;
|
||
std::ostringstream ofn;
|
||
ofn << m_currentSessionDir << "/LaserData_" << seq << ".txt";
|
||
|
||
// 统计总点数
|
||
size_t totalPts = 0;
|
||
for (const auto& p : task.cloudData)
|
||
totalPts += p.second.nPointCount;
|
||
auto t0 = std::chrono::steady_clock::now();
|
||
LaserDataLoader loader;
|
||
loader.SaveLaserScanData(ofn.str(), task.cloudData,
|
||
static_cast<int>(task.info.height), 0.0f, 0, 0);
|
||
auto t1 = std::chrono::steady_clock::now();
|
||
auto ms = std::chrono::duration_cast<std::chrono::milliseconds>(t1 - t0).count();
|
||
|
||
std::cout << "[CloudShow] 保存 " << ofn.str()
|
||
<< " 行=" << task.cloudData.size()
|
||
<< " 点=" << totalPts
|
||
<< " 耗时=" << ms << "ms"
|
||
<< " Q=" << m_saveQueue.size()
|
||
<< std::endl;
|
||
|
||
m_saveCompleted.fetch_add(1, std::memory_order_relaxed);
|
||
|
||
// 归还 task 到缓冲池
|
||
releaseTask(task);
|
||
haveTask = false;
|
||
}
|
||
catch (const std::exception& e)
|
||
{
|
||
std::cerr << "[CloudShow] 保存异常: " << e.what()
|
||
<< " Q=" << m_saveQueue.size() << std::endl;
|
||
m_nSaveDropped.fetch_add(1, std::memory_order_relaxed);
|
||
if (haveTask) releaseTask(task);
|
||
}
|
||
catch (...)
|
||
{
|
||
std::cerr << "[CloudShow] 保存未知异常 Q="
|
||
<< m_saveQueue.size() << std::endl;
|
||
m_nSaveDropped.fetch_add(1, std::memory_order_relaxed);
|
||
if (haveTask) releaseTask(task);
|
||
}
|
||
}
|
||
}
|
||
|
||
// ============================================================
|
||
// SaveTask 缓冲池 — 消除回调线程的堆分配
|
||
// ============================================================
|
||
|
||
CCloudShow::SaveTask CCloudShow::acquireTask()
|
||
{
|
||
std::lock_guard<std::mutex> lk(m_taskPoolMutex);
|
||
if (!m_taskPool.empty())
|
||
{
|
||
SaveTask task = std::move(m_taskPool.back());
|
||
m_taskPool.pop_back();
|
||
return task;
|
||
}
|
||
return SaveTask{}; // 池空：首次/扩容时分配（稀有路径）
|
||
}
|
||
|
||
void CCloudShow::releaseTask(SaveTask& task)
|
||
{
|
||
// 清空元数据但保留 buffers 的 capacity 供下次复用
|
||
task.cloudData.clear();
|
||
task.info = CloudFrameInfo{};
|
||
|
||
std::lock_guard<std::mutex> lk(m_taskPoolMutex);
|
||
m_taskPool.push_back(std::move(task));
|
||
}
|
||
|
||
void CCloudShow::packIntoTask(SaveTask& task,
|
||
const ICloudShow::CloudData& cloudData,
|
||
const CloudFrameInfo& info)
|
||
{
|
||
task.info = info;
|
||
task.cloudData.clear();
|
||
task.cloudData.reserve(cloudData.size());
|
||
|
||
// 确保 buffers 足够（仅首次或帧结构变化时 resize）
|
||
if (task.buffers.size() < cloudData.size())
|
||
task.buffers.resize(cloudData.size());
|
||
|
||
for (size_t i = 0; i < cloudData.size(); ++i)
|
||
{
|
||
const auto& src = cloudData[i];
|
||
task.cloudData.push_back(src);
|
||
auto& ld = task.cloudData.back().second;
|
||
ld.p2DPoint = nullptr;
|
||
|
||
if (ld.p3DPoint && ld.nPointCount > 0)
|
||
{
|
||
size_t sz = pointSize(src.first) * static_cast<size_t>(ld.nPointCount);
|
||
if (sz > 0)
|
||
{
|
||
if (task.buffers[i].capacity() < sz)
|
||
task.buffers[i].reserve(sz + 4096);
|
||
task.buffers[i].resize(sz);
|
||
std::memcpy(task.buffers[i].data(), ld.p3DPoint, sz);
|
||
ld.p3DPoint = task.buffers[i].data();
|
||
}
|
||
else
|
||
{
|
||
ld.p3DPoint = nullptr;
|
||
}
|
||
}
|
||
}
|
||
|
||
task.cloudData.resize(cloudData.size());
|
||
}
|
||
|
||
// ============================================================
|
||
// keyboardCallback — PCL 键盘事件 → 原子标志
|
||
// ============================================================
|
||
void CCloudShow::keyboardCallback(
|
||
const pcl::visualization::KeyboardEvent& event, void* cookie)
|
||
{
|
||
if (!event.keyDown())
|
||
return;
|
||
|
||
auto* self = static_cast<CCloudShow*>(cookie);
|
||
if (!self)
|
||
return;
|
||
|
||
std::string key = event.getKeySym();
|
||
if (key == "q" || key == "Q")
|
||
self->m_bQuitRequested = true;
|
||
else if (key == "s" || key == "S")
|
||
self->m_bSaveRequested = true;
|
||
}
|
||
|
||
// ============================================================
|
||
// SpinOnce — 消费点云 + 驱动渲染 + 保存状态维护
|
||
// ============================================================
|
||
int CCloudShow::SpinOnce(int ms)
|
||
{
|
||
if (!m_bRunning || !m_viewer)
|
||
return -1;
|
||
|
||
if (m_viewer->wasStopped())
|
||
{
|
||
m_bQuitRequested = true;
|
||
Stop();
|
||
return -1;
|
||
}
|
||
|
||
// START/STOP 按钮处理
|
||
if (m_bStartStopRequested.exchange(false))
|
||
handleStartStopToggle();
|
||
|
||
// S 键单帧保存（需持有 m_lastCloudMutex 与 PushFrame 互斥）
|
||
if (m_bSaveRequested.exchange(false))
|
||
{
|
||
// 锁内：取走数据所有权（swap，极快），避免持锁做文件 IO
|
||
CloudData localData;
|
||
std::vector<std::vector<uint8_t>> localBuffers;
|
||
CloudFrameInfo localInfo{};
|
||
{
|
||
std::lock_guard<std::mutex> lk(m_lastCloudMutex);
|
||
if (!m_lastCloudData.empty())
|
||
{
|
||
localData.swap(m_lastCloudData);
|
||
localBuffers.swap(m_lastCloudBuffers);
|
||
localInfo = m_lastFrameInfo;
|
||
}
|
||
}
|
||
// 锁外：执行文件写入（不阻塞 PushFrame）
|
||
if (!localData.empty())
|
||
{
|
||
const int seq = ++m_nSingleSaveSeq;
|
||
std::ostringstream oss;
|
||
oss << m_saveBaseDir << "/LaserData_" << seq << ".txt";
|
||
LaserDataLoader loader;
|
||
loader.SaveLaserScanData(oss.str(), localData,
|
||
static_cast<int>(localInfo.height), 0.0f, 0, 0);
|
||
std::cout << "[CloudShow] 单帧保存: " << oss.str() << std::endl;
|
||
}
|
||
}
|
||
|
||
// 实时进度更新
|
||
if (m_bSaveActive.load() && m_saveCompleted.load() > 0)
|
||
{
|
||
updateProgressText();
|
||
}
|
||
else if (!m_bSaveActive.load() && m_saveTotal.load() > 0)
|
||
{
|
||
int done = m_saveCompleted.load();
|
||
int total = m_saveTotal.load();
|
||
updateProgressText();
|
||
if (done >= total)
|
||
{
|
||
std::cout << "[CloudShow] 保存完成: " << m_currentSessionDir
|
||
<< " | 共 " << total << " 帧" << std::endl;
|
||
m_saveTotal = 0;
|
||
m_saveCompleted = 0;
|
||
m_bFlushAndSave = false; // 写盘结束，保存线程回到等待
|
||
updateProgressText();
|
||
|
||
// 释放全部内存：缓冲池 + 保存队列 + S键缓存
|
||
{
|
||
std::lock_guard<std::mutex> lk(m_taskPoolMutex);
|
||
m_taskPool.clear();
|
||
}
|
||
{
|
||
std::lock_guard<std::mutex> lk(m_saveMutex);
|
||
m_saveQueue.clear();
|
||
}
|
||
{
|
||
std::lock_guard<std::mutex> lk(m_lastCloudMutex);
|
||
m_lastCloudData.clear();
|
||
m_lastCloudBuffers.clear();
|
||
m_lastCloudBuffers.shrink_to_fit();
|
||
}
|
||
}
|
||
}
|
||
|
||
// 渲染最新点云
|
||
pcl::PointCloud<pcl::PointXYZI>::Ptr cloud;
|
||
{
|
||
std::lock_guard<std::mutex> lk(m_cloudMutex);
|
||
if (m_bUpdated)
|
||
{
|
||
cloud = m_displayCloud;
|
||
m_bUpdated = false;
|
||
}
|
||
}
|
||
|
||
if (cloud && !cloud->points.empty())
|
||
{
|
||
if (m_bFirstCloud)
|
||
{
|
||
m_viewer->addPointCloud<pcl::PointXYZI>(cloud, m_windowName);
|
||
m_viewer->setPointCloudRenderingProperties(
|
||
pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2.0, m_windowName);
|
||
m_viewer->resetCamera();
|
||
m_viewer->setCameraPosition(0, -100000, -50000, 0, 0, 50000, 0, -1, 0);
|
||
m_bFirstCloud = false;
|
||
}
|
||
else
|
||
{
|
||
m_viewer->updatePointCloud<pcl::PointXYZI>(cloud, m_windowName);
|
||
}
|
||
}
|
||
|
||
m_viewer->spinOnce(ms);
|
||
return 0;
|
||
}
|
||
|