jerryzeng/algoLib
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity
algoLib/fireBrickPositioning_test/fireBrickPositioning_test.cpp

517 lines
15 KiB
C++
Raw Normal View History

初次提交
2025-06-08 11:10:24 +08:00
// fireBrickPositioning_test.cpp : 此文件包含 "main" 函数。程序执行将在此处开始并结束。
//
#include <iostream>
#include <fstream>
#include <vector>
#include <stdio.h>
#include <VZNL_Types.h>
#include "direct.h"
#include <string>
#include "SG_fireBrick_Export.h"
#define DATA_VER_OLD 0
#define DATA_VER_NEW 1
#define DATA_VER_FROM_CUSTOM 2
#define VZ_LASER_LINE_PT_MAX_NUM 4096
SVzNLXYZRGBDLaserLine* vzReadLaserScanPointFromFile_XYZRGB(const char* fileName, int* scanLineNum, float* scanV,
int* dataCalib, int* scanMaxStamp, int* canClockUnit)
{
std::ifstream inputFile(fileName);
std::string linedata;
if (inputFile.is_open() == false)
return NULL;
SVzNLXYZRGBDLaserLine* _scanLines = NULL;
int lines = 0;
int dataElements = 4;
int firstIndex = -1;
int dataFileVer = DATA_VER_OLD;
std::getline(inputFile, linedata); //第一行
int lineNum = 0;
if (0 == strncmp("LineNum:", linedata.c_str(), 8))
{
dataFileVer = DATA_VER_NEW;
sscanf_s(linedata.c_str(), "LineNum:%d", &lines);
if (lines == 0)
return NULL;
lineNum = lines;
_scanLines = (SVzNLXYZRGBDLaserLine*)malloc(sizeof(SVzNLXYZRGBDLaserLine) * (lineNum + 1));
memset(_scanLines, 0, sizeof(SVzNLXYZRGBDLaserLine) * (lineNum + 1));
if (scanLineNum)
*scanLineNum = lines;
}
else if (0 == strncmp("LineNum_", linedata.c_str(), 8))
{
dataFileVer = DATA_VER_OLD;
sscanf_s(linedata.c_str(), "LineNum_%d", &lines);
if (lines == 0)
return NULL;
lineNum = lines;
_scanLines = (SVzNLXYZRGBDLaserLine*)malloc(sizeof(SVzNLXYZRGBDLaserLine) * (lineNum + 1));
memset(_scanLines, 0, sizeof(SVzNLXYZRGBDLaserLine) * (lineNum + 1));
if (scanLineNum)
*scanLineNum = lines;
}
if (_scanLines == NULL)
return NULL;
int ptNum = 0;
int lineIdx = -1;
int ptIdx = 0;
SVzNLPointXYZRGBA* p3DPoint = NULL;
if (dataFileVer == DATA_VER_NEW)
{
while (getline(inputFile, linedata))
{
if (0 == strncmp("ScanSpeed:", linedata.c_str(), 10))
{
double lineV = 0;
sscanf_s(linedata.c_str(), "ScanSpeed:%lf", &lineV);
if (scanV)
*scanV = (float)lineV;
}
else if (0 == strncmp("PointAdjust:", linedata.c_str(), 12))
{
int ptAdjusted = 0;
sscanf_s(linedata.c_str(), "PointAdjust:%d", &ptAdjusted);
if (dataCalib)
*dataCalib = ptAdjusted;
}
else if (0 == strncmp("MaxTimeStamp:", linedata.c_str(), 13))
{
unsigned int maxTimeStamp = 0;
unsigned int timePerStamp = 0;
sscanf_s(linedata.c_str(), "MaxTimeStamp:%u_%u", &maxTimeStamp, &timePerStamp);
if (scanMaxStamp)
*scanMaxStamp = maxTimeStamp;
if (canClockUnit)
*canClockUnit = timePerStamp;
}
else if (0 == strncmp("Line_", linedata.c_str(), 5))
{
int lineIndex;
unsigned int timeStamp;
sscanf_s(linedata.c_str(), "Line_%d_%u_%d", &lineIndex, &timeStamp, &ptNum);
if (firstIndex < 0)
firstIndex = lineIndex;
lineIndex = lineIndex - firstIndex;
if ((lineIndex < 0) || (lineIndex >= lines))
break;
//new Line
lineIdx++;
if (ptNum > 0)
{
p3DPoint = (SVzNLPointXYZRGBA*)malloc(sizeof(SVzNLPointXYZRGBA) * ptNum);
memset(p3DPoint, 0, sizeof(SVzNLPointXYZRGBA) * ptNum);
}
else
p3DPoint = NULL;
_scanLines[lineIdx].nPointCnt = 0;
_scanLines[lineIdx].nTimeStamp = timeStamp;
_scanLines[lineIdx].p3DPoint = p3DPoint;
}
else if (0 == strncmp("{", linedata.c_str(), 1))
{
float X, Y, Z;
int imageY = 0;
float leftX, leftY;
float rightX, rightY;
int r = -1, g = -1, b = -1;
//sscanf(linedata.c_str(), "{%f,%f,%f}-{%f,%f}-{%f,%f}", &X, &Y, &Z, &leftX, &leftY, &rightX, &rightY);
sscanf_s(linedata.c_str(), "{%f,%f,%f}-{%f,%f}-{%f,%f}-{%d,%d,%d}", &X, &Y, &Z, &leftX, &leftY, &rightX, &rightY, &r, &g, &b);
int id = _scanLines[lineIdx].nPointCnt;
if (id < ptNum)
{
p3DPoint[id].x = X;
p3DPoint[id].y = Y;
p3DPoint[id].z = Z;
p3DPoint[id].nRGB = 0;
_scanLines[lineIdx].nPointCnt = id + 1;
}
}
}
}
else if (dataFileVer == DATA_VER_OLD)
{
while (getline(inputFile, linedata))
{
if (0 == strncmp("DataElements_", linedata.c_str(), 13))
{
sscanf_s(linedata.c_str(), "DataElements_%d", &dataElements);
if ((dataElements != 3) && (dataElements != 4))
break;
}
if (0 == strncmp("LineV_", linedata.c_str(), 6))
{
double lineV = 0;
sscanf_s(linedata.c_str(), "LineV_%lf", &lineV);
}
else if (0 == strncmp("Line_", linedata.c_str(), 5))
{
int lineIndex;
unsigned int timeStamp;
sscanf_s(linedata.c_str(), "Line_%d_%u", &lineIndex, &timeStamp);
#if 0
if (scanLineListTail == NULL)
firstIndex = lineIndex;
#endif
lineIndex = lineIndex - firstIndex;
if ((lineIndex < 0) || (lineIndex >= lines))
break;
//new Line
//new Line
lineIdx++;
p3DPoint = (SVzNLPointXYZRGBA*)malloc(sizeof(SVzNLPointXYZRGBA) * VZ_LASER_LINE_PT_MAX_NUM);
memset(p3DPoint, 0, sizeof(SVzNLPointXYZRGBA) * VZ_LASER_LINE_PT_MAX_NUM);
_scanLines[lineIdx].nPointCnt = 0;
_scanLines[lineIdx].nTimeStamp = timeStamp;
_scanLines[lineIdx].p3DPoint = p3DPoint;
}
else if (0 == strncmp("(", linedata.c_str(), 1))
{
float X, Y, Z;
int imageY = 0;
if (dataElements == 4)
sscanf_s(linedata.c_str(), "(%f,%f,%f,%d)", &X, &Y, &Z, &imageY);
else
sscanf_s(linedata.c_str(), "(%f,%f,%f)", &X, &Y, &Z);
int id = _scanLines[lineIdx].nPointCnt;
if (id < VZ_LASER_LINE_PT_MAX_NUM)
{
p3DPoint[id].x = X;
p3DPoint[id].y = Y;
p3DPoint[id].z = Z;
p3DPoint[id].nRGB = 0;
_scanLines[lineIdx].nPointCnt = id + 1;
}
}
}
}
inputFile.close();
return _scanLines;
}
SVzNL3DLaserLine* vzReadLaserScanPointFromFile_XYZ(const char* fileName, int* scanLineNum, float* scanV,
int* dataCalib, int* scanMaxStamp, int* canClockUnit)
{
std::ifstream inputFile(fileName);
std::string linedata;
if (inputFile.is_open() == false)
return NULL;
SVzNL3DLaserLine* _scanLines = NULL;
int lines = 0;
int dataElements = 4;
int firstIndex = -1;
int dataFileVer = DATA_VER_OLD;
std::getline(inputFile, linedata); //第一行
int lineNum = 0;
if (0 == strncmp("LineNum:", linedata.c_str(), 8))
{
dataFileVer = DATA_VER_NEW;
sscanf_s(linedata.c_str(), "LineNum:%d", &lines);
if (lines == 0)
return NULL;
lineNum = lines;
_scanLines = (SVzNL3DLaserLine*)malloc(sizeof(SVzNL3DLaserLine) * (lineNum + 1));
memset(_scanLines, 0, sizeof(SVzNL3DLaserLine) * (lineNum + 1));
if (scanLineNum)
*scanLineNum = lines;
}
else if (0 == strncmp("LineNum_", linedata.c_str(), 8))
{
dataFileVer = DATA_VER_OLD;
sscanf_s(linedata.c_str(), "LineNum_%d", &lines);
if (lines == 0)
return NULL;
lineNum = lines;
_scanLines = (SVzNL3DLaserLine*)malloc(sizeof(SVzNL3DLaserLine) * (lineNum + 1));
memset(_scanLines, 0, sizeof(SVzNL3DLaserLine) * (lineNum + 1));
if (scanLineNum)
*scanLineNum = lines;
}
if (_scanLines == NULL)
return NULL;
int ptNum = 0;
int lineIdx = -1;
int ptIdx = 0;
SVzNL3DPosition* p3DPoint = NULL;
if (dataFileVer == DATA_VER_NEW)
{
while (getline(inputFile, linedata))
{
if (0 == strncmp("ScanSpeed:", linedata.c_str(), 10))
{
double lineV = 0;
sscanf_s(linedata.c_str(), "ScanSpeed:%lf", &lineV);
if (scanV)
*scanV = (float)lineV;
}
else if (0 == strncmp("PointAdjust:", linedata.c_str(), 12))
{
int ptAdjusted = 0;
sscanf_s(linedata.c_str(), "PointAdjust:%d", &ptAdjusted);
if (dataCalib)
*dataCalib = ptAdjusted;
}
else if (0 == strncmp("MaxTimeStamp:", linedata.c_str(), 13))
{
unsigned int maxTimeStamp = 0;
unsigned int timePerStamp = 0;
sscanf_s(linedata.c_str(), "MaxTimeStamp:%u_%u", &maxTimeStamp, &timePerStamp);
if (scanMaxStamp)
*scanMaxStamp = maxTimeStamp;
if (canClockUnit)
*canClockUnit = timePerStamp;
}
else if (0 == strncmp("Line_", linedata.c_str(), 5))
{
int lineIndex;
unsigned int timeStamp;
sscanf_s(linedata.c_str(), "Line_%d_%u_%d", &lineIndex, &timeStamp, &ptNum);
if (firstIndex < 0)
firstIndex = lineIndex;
lineIndex = lineIndex - firstIndex;
if ((lineIndex < 0) || (lineIndex >= lines))
break;
//new Line
lineIdx++;
if (ptNum > 0)
{
p3DPoint = (SVzNL3DPosition*)malloc(sizeof(SVzNL3DPosition) * ptNum);
memset(p3DPoint, 0, sizeof(SVzNL3DPosition) * ptNum);
}
else
p3DPoint = NULL;
_scanLines[lineIdx].nPositionCnt = 0;
_scanLines[lineIdx].nTimeStamp = timeStamp;
_scanLines[lineIdx].p3DPosition = p3DPoint;
}
else if (0 == strncmp("{", linedata.c_str(), 1))
{
float X, Y, Z;
int imageY = 0;
float leftX, leftY;
float rightX, rightY;
sscanf_s(linedata.c_str(), "{%f,%f,%f}-{%f,%f}-{%f,%f}", &X, &Y, &Z, &leftX, &leftY, &rightX, &rightY);
int id = _scanLines[lineIdx].nPositionCnt;
if (id < ptNum)
{
p3DPoint[id].pt3D.x = X;
p3DPoint[id].pt3D.y = Y;
p3DPoint[id].pt3D.z = Z;
_scanLines[lineIdx].nPositionCnt = id + 1;
}
}
}
}
else if (dataFileVer == DATA_VER_OLD)
{
while (getline(inputFile, linedata))
{
if (0 == strncmp("DataElements_", linedata.c_str(), 13))
{
sscanf_s(linedata.c_str(), "DataElements_%d", &dataElements);
if ((dataElements != 3) && (dataElements != 4))
break;
}
if (0 == strncmp("LineV_", linedata.c_str(), 6))
{
double lineV = 0;
sscanf_s(linedata.c_str(), "LineV_%lf", &lineV);
}
else if (0 == strncmp("Line_", linedata.c_str(), 5))
{
int lineIndex;
unsigned int timeStamp;
sscanf_s(linedata.c_str(), "Line_%d_%u", &lineIndex, &timeStamp);
#if 0
if (scanLineListTail == NULL)
firstIndex = lineIndex;
#endif
lineIndex = lineIndex - firstIndex;
if ((lineIndex < 0) || (lineIndex >= lines))
break;
//new Line
//new Line
lineIdx++;
p3DPoint = (SVzNL3DPosition*)malloc(sizeof(SVzNL3DPosition) * VZ_LASER_LINE_PT_MAX_NUM);
memset(p3DPoint, 0, sizeof(SVzNL3DPosition) * VZ_LASER_LINE_PT_MAX_NUM);
_scanLines[lineIdx].nPositionCnt = 0;
_scanLines[lineIdx].nTimeStamp = timeStamp;
_scanLines[lineIdx].p3DPosition = p3DPoint;
}
else if (0 == strncmp("(", linedata.c_str(), 1))
{
float X, Y, Z;
int imageY = 0;
if (dataElements == 4)
sscanf_s(linedata.c_str(), "(%f,%f,%f,%d)", &X, &Y, &Z, &imageY);
else
sscanf_s(linedata.c_str(), "(%f,%f,%f)", &X, &Y, &Z);
int id = _scanLines[lineIdx].nPositionCnt;
if (id < VZ_LASER_LINE_PT_MAX_NUM)
{
p3DPoint[id].pt3D.x = X;
p3DPoint[id].pt3D.y = Y;
p3DPoint[id].pt3D.z = Z;
_scanLines[lineIdx].nPositionCnt = id + 1;
}
}
}
}
inputFile.close();
return _scanLines;
}
SVzNL3DLaserLine* _convertToGridData_XYZRGB(SVzNLXYZRGBDLaserLine* laser3DPoints, int lineNum, double _F)
{
int min_y = 100000000;
int max_y = -10000000;
for (int line = 0; line < lineNum; line++)
{
for (int i = 0; i < laser3DPoints[line].nPointCnt; i++)
{
SVzNLPointXYZRGBA* a_pt = &laser3DPoints[line].p3DPoint[i];
if (a_pt->z > 1e-4)
{
a_pt->nRGB = (int)(_F * a_pt->y / a_pt->z + 2000 + 0.5);
max_y = max_y < (int)a_pt->nRGB ? (int)a_pt->nRGB : max_y;
min_y = min_y > (int)a_pt->nRGB ? (int)a_pt->nRGB : min_y;
}
}
}
if (min_y == 100000000)
return NULL;
int pt_counter = max_y - min_y + 1;
SVzNL3DLaserLine* gridData = (SVzNL3DLaserLine*)malloc(sizeof(SVzNL3DLaserLine) * (lineNum + 1));
memset(gridData, 0, sizeof(SVzNL3DLaserLine) * (lineNum + 1));
for (int line = 0; line < lineNum; line++)
{
gridData[line].nPositionCnt = pt_counter;
gridData[line].nTimeStamp = laser3DPoints[line].nTimeStamp;
gridData[line].p3DPosition = (SVzNL3DPosition*)malloc(sizeof(SVzNL3DPosition) * pt_counter);
memset(gridData[line].p3DPosition, 0, sizeof(SVzNL3DPosition) * pt_counter);
for (int i = 0; i < laser3DPoints[line].nPointCnt; i++)
{
SVzNLPointXYZRGBA a_pt = laser3DPoints[line].p3DPoint[i];
if (a_pt.z > 1e-4)
{
int pt_id = a_pt.nRGB - min_y;
gridData[line].p3DPosition[pt_id].pt3D.x = a_pt.x;
gridData[line].p3DPosition[pt_id].pt3D.y = a_pt.y;
gridData[line].p3DPosition[pt_id].pt3D.z = a_pt.z;
}
}
}
return gridData;
}
void _outputScanDataFile_self(char* fileName, SVzNL3DLaserLine* scanData, int lineNum,
float lineV, int dataCalib, int maxTimeStamp, int clockPerSecond)
{
std::ofstream sw(fileName);
sw << "LineNum:" << lineNum << std::endl;
sw << "DataType: 0" << std::endl;
sw << "ScanSpeed:" << lineV << std::endl;
sw << "PointAdjust: 1" << std::endl;
sw << "MaxTimeStamp:" << maxTimeStamp << "_" << clockPerSecond << std::endl;
for (int line = 0; line < lineNum; line++)
{
sw << "Line_" << line << "_" << scanData[line].nTimeStamp << "_" << scanData[line].nPositionCnt << std::endl;
for (int i = 0; i < scanData[line].nPositionCnt; i++)
{
SVzNL3DPosition* pt3D = &scanData[line].p3DPosition[i];
float x = (float)pt3D->pt3D.x;
float y = (float)pt3D->pt3D.y;
float z = (float)pt3D->pt3D.z;
sw << "{" << x << "," << y << "," << z << "}-";
sw << "{0,0}-{0,0}" << std::endl;
}
}
sw.close();
}
int main()
{
#if 0
//将数据转换成栅格格式格式
const double f = 1729.0;
char _scan_file[256];
int lineNum = 0;
float lineV = 0.0f;
int dataCalib = 0;
int maxTimeStamp = 0;
int clockPerSecond = 0;
sprintf_s(_scan_file, "E:\\上古\\耐火砖\\耐火砖3D扫描数据\\1.txt");
SVzNLXYZRGBDLaserLine* laser3DPoints_RGBD = vzReadLaserScanPointFromFile_XYZRGB(_scan_file, &lineNum, &lineV, &dataCalib, &maxTimeStamp, &clockPerSecond);
if (laser3DPoints_RGBD == NULL)
return 0;
double _F = 1729.0;; //f
SVzNL3DLaserLine* gridData = _convertToGridData_XYZRGB(laser3DPoints_RGBD, lineNum, _F);
char _out_file[256];
sprintf_s(_out_file, "E:\\上古\\耐火砖\\耐火砖3D扫描数据\\1_grid.txt");
_outputScanDataFile_self(_out_file, gridData, lineNum,
lineV, dataCalib, maxTimeStamp, clockPerSecond);
#endif
char _scan_file[256];
int lineNum = 0;
float lineV = 0.0f;
int dataCalib = 0;
int maxTimeStamp = 0;
int clockPerSecond = 0;
sprintf_s(_scan_file, "E:\\上古\\耐火砖\\耐火砖3D扫描数据\\1_grid.txt");
SVzNL3DLaserLine* laser3DPoints = vzReadLaserScanPointFromFile_XYZ(_scan_file, &lineNum, &lineV, &dataCalib, &maxTimeStamp, &clockPerSecond);
ISGFireBrick* pFireBrick = nullptr;
if (false == SGCreateFireBrick(0.5, &pFireBrick))
{
return -1;
}
printf("Ver %s\n", pFireBrick->GetVersion());
double camPose[9] = {
1.0, 0.0, 0.0,
0.0, 1.0, 0.0,
0.0, 0.0, 1.0 };
int errCode = 0;
for (int i = 0; i < lineNum; i++)
{
pFireBrick->sgScanLineProc(&laser3DPoints[i], camPose, &errCode);
if (errCode)
{
printf("Error! errCode= %d\n", errCode);
return -1;
}
}
std::vector<SSG_6AxisAttitude> brickPoses;
errCode = 0;
pFireBrick->sgGetBrickPose(laser3DPoints, lineNum, brickPoses, &errCode);
}
Reference in New Issue Copy Permalink