zhangben1207/CommonLibraries
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

点云补齐双目2D像素坐标(SVzNL2DLRPoint)/每线encodeNo+timestamp/摆镜速度,供官方 LaserData 格式导出

Browse Source
This commit is contained in:
Ben 2026-07-06 00:12:44 -07:00
parent 9d1aea516e
commit ff7552a147
2 changed files with 69 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

60
src/VizumCamera.cpp
View File

@ -73,6 +73,8 @@ struct VizumCamera::Impl {
} cachedFrame;
std::vector<std::vector<Point3D>> accumulatedScanLines;
std::vector<std::vector<PixelLR>> accumulatedScanLines2D; // 与扫描线一一对应的双目2D坐标
std::vector<ScanLineInfo> accumulatedLineMeta; // 每线 encodeNo/timestamp
std::mutex scanLineMutex;
std::atomic<bool> autoDetectRunning{false};
std::atomic<bool> isPaused{true};
@ -92,6 +94,8 @@ struct VizumCamera::Impl {
#define m_frameCounter m_impl->frameCounter
#define m_cachedFrame m_impl->cachedFrame
#define m_accumulatedScanLines m_impl->accumulatedScanLines
#define m_accumulatedScanLines2D m_impl->accumulatedScanLines2D
#define m_accumulatedLineMeta m_impl->accumulatedLineMeta
#define m_scanLineMutex m_impl->scanLineMutex
#define m_autoDetectRunning m_impl->autoDetectRunning
#define m_isPaused m_impl->isPaused
@ -176,17 +180,22 @@ void VizumCamera::ConvertLaserLineDataToPointCloud(void* laserLinePoint)
// 将 p3DPoint 转换为 SVzNLPointXYZRGBA* 类型
SVzNLPointXYZRGBA* pPoints = static_cast<SVzNLPointXYZRGBA*>(pLaserLinePoint->p3DPoint);
// keResultDataType_PointXYZRGBA 对应的 2D 数据类型为 SVzNL2DLRPoint(左右目像素坐标)
SVzNL2DLRPoint* pPoints2D = static_cast<SVzNL2DLRPoint*>(pLaserLinePoint->p2DPoint);
// 使用锁保护对累积点云的访问
std::lock_guard<std::mutex> lock(m_scanLineMutex);
// 预先分配内存
m_accumulatedCloud.points.reserve(m_accumulatedCloud.points.size() + nPoints);
// 同时保存扫描线数据
std::vector<Point3D> scanLine;
scanLine.reserve(nPoints);
std::vector<PixelLR> scanLine2D;
scanLine2D.reserve(nPoints);
int validCount = 0;
for (int i = 0; i < nPoints; ++i) {
SVzNLPointXYZRGBA& point = pPoints[i];
@ -207,14 +216,30 @@ void VizumCamera::ConvertLaserLineDataToPointCloud(void* laserLinePoint)
scanPt.y = point.y;
scanPt.z = point.z;
scanLine.push_back(scanPt);
// 同步保存双目 2D 像素坐标(无 2D 数据时补零占位,保持与 3D 点对齐)
PixelLR px;
if (pPoints2D != nullptr) {
px.leftX = pPoints2D[i].sLeft.x;
px.leftY = pPoints2D[i].sLeft.y;
px.rightX = pPoints2D[i].sRight.x;
px.rightY = pPoints2D[i].sRight.y;
}
scanLine2D.push_back(px);
}
// 统计信息
LOG_INFO("[VizumCamera] 收到扫描线: {} 个点, 有效点: {}", nPoints, validCount);
// 将扫描线添加到累积的扫描线数据中
if (!scanLine.empty()) {
m_accumulatedScanLines.push_back(std::move(scanLine));
m_accumulatedScanLines2D.push_back(std::move(scanLine2D));
ScanLineInfo meta;
meta.pointCount = (size_t)nPoints;
meta.timestamp = pLaserLinePoint->llTimeStamp;
meta.encodeNo = pLaserLinePoint->nEncodeNo;
m_accumulatedLineMeta.push_back(meta);
LOG_INFO("[VizumCamera] 累积扫描线数量: {}", m_accumulatedScanLines.size());
}
#else
@ -234,6 +259,8 @@ void VizumCamera::ClearAccumulatedScanLines()
{
std::lock_guard<std::mutex> lock(m_scanLineMutex);
m_accumulatedScanLines.clear();
m_accumulatedScanLines2D.clear();
m_accumulatedLineMeta.clear();
m_accumulatedCloud.clear();
}
@ -889,6 +916,8 @@ bool VizumCamera::CapturePointCloudWithAutoDetect(PointCloud& pointCloud, int ti
{
std::lock_guard<std::mutex> lock(m_scanLineMutex);
m_accumulatedScanLines.clear();
m_accumulatedScanLines2D.clear();
m_accumulatedLineMeta.clear();
m_accumulatedCloud.clear();
}
LOG_INFO("[VizumCamera] Step 1: Cleared accumulated data");
@ -1089,16 +1118,33 @@ bool VizumCamera::CapturePointCloudWithAutoDetect(PointCloud& pointCloud, int ti
pointCloud.height = static_cast<uint32_t>(H);
pointCloud.isOrganized = true;
pointCloud.points.assign(W * H, Point3D{}); // (0,0,0) = 无效占位
pointCloud.pixels.assign(W * H, PixelLR{}); // 双目2D坐标,零占位
pointCloud.scanLines.reserve(H);
for (size_t r = 0; r < H; ++r) {
const auto& line = m_accumulatedScanLines[r];
ScanLineInfo info;
info.startIndex = r * W;
info.pointCount = line.size();
if (r < m_accumulatedLineMeta.size()) {
info.timestamp = m_accumulatedLineMeta[r].timestamp;
info.encodeNo = m_accumulatedLineMeta[r].encodeNo;
}
pointCloud.scanLines.push_back(info);
for (size_t c = 0; c < line.size() && c < W; ++c) {
pointCloud.points[r * W + c] = line[c];
}
if (r < m_accumulatedScanLines2D.size()) {
const auto& line2D = m_accumulatedScanLines2D[r];
for (size_t c = 0; c < line2D.size() && c < W; ++c) {
pointCloud.pixels[r * W + c] = line2D[c];
}
}
}
// 记录摆镜角速度(供保存元信息用,查询失败保持 0)
int angleSpeed = 0;
if (VzNL_GetSwingAngleSpeed(m_deviceHandle, &angleSpeed) == 0) {
pointCloud.scanSpeed = static_cast<float>(angleSpeed);
}
LOG_INFO("[VizumCamera] Organized grid built: {}x{} ({} cells), {} scan lines, total {} points",
@ -1116,6 +1162,8 @@ bool VizumCamera::CapturePointCloudWithAutoDetect(PointCloud& pointCloud, int ti
// 清空累积数据
m_accumulatedScanLines.clear();
m_accumulatedScanLines2D.clear();
m_accumulatedLineMeta.clear();
m_accumulatedCloud.clear();
}

15
src/core/StructuredLightTypes.h
View File

@ -44,14 +44,26 @@ struct ColorPoint3D {
struct ScanLineInfo {
size_t startIndex = 0;
size_t pointCount = 0;
unsigned long long timestamp = 0; // 该激光线时间戳(SDK llTimeStamp)
unsigned int encodeNo = 0; // 该激光线编码器值(SDK nEncodeNo)
};
// 双目 2D 像素坐标(极线校正后,左右目 y 相同,x 差为视差)
struct PixelLR {
int leftX = 0;
int leftY = 0;
int rightX = 0;
int rightY = 0;
};
struct PointCloud {
std::vector<Point3D> points;
std::vector<PixelLR> pixels; // 与 points 一一对应;空 = 无 2D 数据
std::vector<ScanLineInfo> scanLines;
uint32_t width = 0;
uint32_t height = 0;
bool isOrganized = true;
float scanSpeed = 0.0f; // 采集时的摆镜角速度(查询失败为 0)
size_t size() const { return points.size(); }
bool empty() const { return points.empty(); }
@ -61,10 +73,13 @@ struct PointCloud {
{
points.clear();
points.shrink_to_fit();
pixels.clear();
pixels.shrink_to_fit();
scanLines.clear();
scanLines.shrink_to_fit();
width = 0;
height = 0;
scanSpeed = 0.0f;
}
Point3D& at(size_t index) { return points.at(index); }