361 lines
11 KiB
C++
361 lines
11 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 <pcl/io/pcd_io.h>
|
||
|
||
#include <fstream>
|
||
#include <iomanip>
|
||
#include <iostream>
|
||
#include <cmath>
|
||
|
||
// ============================================================
|
||
// 工厂方法
|
||
// ============================================================
|
||
std::unique_ptr<ICloudShow> ICloudShow::Create()
|
||
{
|
||
return std::make_unique<CCloudShow>();
|
||
}
|
||
|
||
// ============================================================
|
||
// 构造 / 析构
|
||
// ============================================================
|
||
CCloudShow::CCloudShow()
|
||
{
|
||
}
|
||
|
||
CCloudShow::~CCloudShow()
|
||
{
|
||
Stop();
|
||
}
|
||
|
||
// ============================================================
|
||
// Show — 单次显示，阻塞直到关闭窗口
|
||
// ============================================================
|
||
int CCloudShow::Show(PointCloudPtr cloud, const std::string& windowName)
|
||
{
|
||
if (!cloud || cloud->points.empty())
|
||
{
|
||
return -1;
|
||
}
|
||
|
||
pcl::visualization::PCLVisualizer viewer(windowName);
|
||
viewer.setSize(670, 450);
|
||
viewer.setBackgroundColor(0.05, 0.05, 0.05);
|
||
viewer.addPointCloud<pcl::PointXYZI>(cloud, windowName);
|
||
viewer.setPointCloudRenderingProperties(
|
||
pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2.0, windowName);
|
||
viewer.initCameraParameters();
|
||
viewer.setCameraPosition(-10000, 0, 8000, 0, 0, 0, 0, 0, 1);
|
||
|
||
// 右下角坐标轴
|
||
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();
|
||
|
||
// 右侧保存按钮（viewport 归一化坐标，始终在右侧中部）
|
||
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)
|
||
{
|
||
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(-10000, 0, 8000, 0, 0, 0, 0, 0, 1);
|
||
m_viewer->registerKeyboardCallback(&CCloudShow::keyboardCallback, this);
|
||
|
||
// 右下角坐标轴
|
||
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();
|
||
|
||
// 右侧保存按钮（viewport 归一化坐标，始终在右侧中部）
|
||
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 = m_viewer->getRendererCollection()->GetFirstRenderer();
|
||
if (renderer) renderer->AddActor2D(textActor);
|
||
textActor->Delete();
|
||
|
||
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];
|
||
if (rx >= 0.83f && rx <= 0.94f && ry >= 0.43f && ry <= 0.53f)
|
||
m_bSaveRequested = 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;
|
||
}
|
||
|
||
m_bRunning = true;
|
||
return 0;
|
||
}
|
||
|
||
// ============================================================
|
||
// UpdateCloud — 更新当前点云（线程安全，可跨线程调用）
|
||
// ============================================================
|
||
int CCloudShow::UpdateCloud(PointCloudPtr cloud)
|
||
{
|
||
if (!m_bRunning || !cloud)
|
||
{
|
||
return -1;
|
||
}
|
||
|
||
{
|
||
std::lock_guard<std::mutex> lock(m_cloudMutex);
|
||
m_currentCloud = cloud;
|
||
m_bUpdated = true;
|
||
}
|
||
|
||
return 0;
|
||
}
|
||
|
||
// ============================================================
|
||
// Stop — 关闭窗口
|
||
// ============================================================
|
||
int CCloudShow::Stop()
|
||
{
|
||
if (!m_bRunning)
|
||
{
|
||
return 0;
|
||
}
|
||
|
||
m_bRunning = false;
|
||
|
||
if (m_axesWidget)
|
||
{
|
||
m_axesWidget->Delete();
|
||
m_axesWidget = 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;
|
||
}
|
||
|
||
// ============================================================
|
||
// 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 — 消费点云 + 驱动一次渲染（必须与 Start 同线程）
|
||
// ============================================================
|
||
int CCloudShow::SpinOnce(int ms)
|
||
{
|
||
if (!m_bRunning || !m_viewer)
|
||
{
|
||
return -1;
|
||
}
|
||
|
||
if (m_viewer->wasStopped())
|
||
{
|
||
m_bQuitRequested = true;
|
||
Stop();
|
||
return -1;
|
||
}
|
||
|
||
PointCloudPtr cloud;
|
||
{
|
||
std::lock_guard<std::mutex> lock(m_cloudMutex);
|
||
if (m_bUpdated)
|
||
{
|
||
cloud = m_currentCloud;
|
||
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(-10000, 0, 8000, 0, 0, 0, 0, 0, 1);
|
||
m_bFirstCloud = false;
|
||
}
|
||
else
|
||
{
|
||
m_viewer->updatePointCloud<pcl::PointXYZI>(cloud, m_windowName);
|
||
}
|
||
}
|
||
|
||
m_viewer->spinOnce(ms);
|
||
return 0;
|
||
}
|
||
|
||
// ============================================================
|
||
// SaveToTxt — 保存为 LaserDataLoader::DebugSaveLaser 兼容 TXT 格式
|
||
// 格式: LineNum/DataType:0/ScanSpeed:0/PointAdjust:1/MaxTimeStamp:0_0
|
||
// Line_<line>_0_<count>
|
||
// { x, y, z } - { 0, 0 } - { 0, 0 }
|
||
// ============================================================
|
||
int CCloudShow::SaveToTxt(const std::string& fileName, PointCloudPtr cloud)
|
||
{
|
||
if (!cloud || cloud->points.empty())
|
||
{
|
||
return -1;
|
||
}
|
||
|
||
std::ofstream ofs(fileName);
|
||
if (!ofs.is_open())
|
||
{
|
||
return -2;
|
||
}
|
||
|
||
const auto& pts = cloud->points;
|
||
const size_t N = pts.size();
|
||
const size_t height = (cloud->height > 0) ? cloud->height : 1;
|
||
const size_t width = (cloud->width > 0) ? cloud->width : N;
|
||
|
||
ofs << "LineNum:" << height << std::endl;
|
||
ofs << "DataType: 0" << std::endl;
|
||
ofs << "ScanSpeed:0" << std::endl;
|
||
ofs << "PointAdjust: 1" << std::endl;
|
||
ofs << "MaxTimeStamp:0_0" << std::endl;
|
||
|
||
for (size_t line = 0; line < height; ++line)
|
||
{
|
||
size_t start = line * width;
|
||
size_t end = (line + 1 == height) ? N : (start + width);
|
||
size_t count = end - start;
|
||
|
||
ofs << "Line_" << line << "_0_" << count << std::endl;
|
||
|
||
for (size_t i = start; i < end; ++i)
|
||
{
|
||
float x = pts[i].x;
|
||
float y = pts[i].y;
|
||
float z = pts[i].z;
|
||
ofs << "{ "
|
||
<< std::fixed << std::setprecision(3) << (std::isnan(x) ? 0.0f : x) << ", "
|
||
<< std::fixed << std::setprecision(3) << (std::isnan(y) ? 0.0f : y) << ", "
|
||
<< std::fixed << std::setprecision(3) << (std::isnan(z) ? 0.0f : z)
|
||
<< " } - { 0, 0 } - { 0, 0 }" << std::endl;
|
||
}
|
||
}
|
||
|
||
ofs.close();
|
||
return 0;
|
||
}
|