HC_chanelSpaceMeasure version 1.0.0
槽道间距检测初始版本， 包含了博清工件组装的最新修改
This commit is contained in:
parent
083a56c66b
commit
47e3a04bf0
@ -1,11 +1,798 @@
|
||||
// BQ_assemblyPosition_test.cpp : 此文件包含 "main" 函数。程序执行将在此处开始并结束。
|
||||
// BQ_workpieceCornerExtract_test.cpp : 此文件包含 "main" 函数。程序执行将在此处开始并结束。
|
||||
//
|
||||
|
||||
#include <iostream>
|
||||
#include <fstream>
|
||||
#include <vector>
|
||||
#include <stdio.h>
|
||||
#include <VZNL_Types.h>
|
||||
#include "direct.h"
|
||||
#include <string>
|
||||
#include "BQ_assemblyPosition_Export.h"
|
||||
#include <opencv2/opencv.hpp>
|
||||
#include <Windows.h>
|
||||
#include <limits>
|
||||
|
||||
typedef struct
|
||||
{
|
||||
int r;
|
||||
int g;
|
||||
int b;
|
||||
}SG_color;
|
||||
|
||||
typedef struct
|
||||
{
|
||||
int nPointIdx;
|
||||
double x;
|
||||
double y;
|
||||
double z;
|
||||
float r;
|
||||
float g;
|
||||
float b;
|
||||
} SPointXYZRGB;
|
||||
|
||||
void vzReadLaserScanPointFromFile_XYZ_vector(const char* fileName, std::vector<std::vector< SVzNL3DPosition>>& scanData)
|
||||
{
|
||||
std::ifstream inputFile(fileName);
|
||||
std::string linedata;
|
||||
|
||||
if (inputFile.is_open() == false)
|
||||
return;
|
||||
|
||||
std::vector< SVzNL3DPosition> a_line;
|
||||
int ptIdx = 0;
|
||||
while (getline(inputFile, linedata))
|
||||
{
|
||||
if (0 == strncmp("Line_", linedata.c_str(), 5))
|
||||
{
|
||||
int ptSize = (int)a_line.size();
|
||||
if (ptSize > 0)
|
||||
{
|
||||
scanData.push_back(a_line);
|
||||
}
|
||||
a_line.clear();
|
||||
ptIdx = 0;
|
||||
}
|
||||
else if (0 == strncmp("{", linedata.c_str(), 1))
|
||||
{
|
||||
float X, Y, Z;
|
||||
int imageY = 0;
|
||||
float leftX, leftY;
|
||||
float rightX, rightY;
|
||||
//迁移相机的XY次序和伟景相机正好相反
|
||||
sscanf_s(linedata.c_str(), "{%f,%f,%f}-{%f,%f}-{%f,%f}", &Y, &X, &Z, &leftX, &leftY, &rightX, &rightY);
|
||||
SVzNL3DPosition a_pt;
|
||||
a_pt.pt3D.x = X;
|
||||
a_pt.pt3D.y = Y;
|
||||
a_pt.pt3D.z = Z;
|
||||
a_pt.nPointIdx = ptIdx;
|
||||
ptIdx++;
|
||||
a_line.push_back(a_pt);
|
||||
}
|
||||
}
|
||||
//last line
|
||||
int ptSize = (int)a_line.size();
|
||||
if (ptSize > 0)
|
||||
{
|
||||
scanData.push_back(a_line);
|
||||
a_line.clear();
|
||||
}
|
||||
|
||||
inputFile.close();
|
||||
return;
|
||||
}
|
||||
|
||||
void wdSavePlyTxt(const char* fileName, std::vector<std::vector< SVzNL3DPosition>> scanLines)
|
||||
{
|
||||
std::ofstream sw(fileName);
|
||||
int lineNum = scanLines.size();
|
||||
for (int line = 0; line < lineNum; line++)
|
||||
{
|
||||
int nPositionCnt = scanLines[line].size();
|
||||
for (int i = 0; i < nPositionCnt; i++)
|
||||
{
|
||||
SVzNL3DPoint* pt3D = &scanLines[line][i].pt3D;
|
||||
if (pt3D->z < 1e-4)
|
||||
continue;
|
||||
double x = (double)pt3D->x;
|
||||
double y = (double)pt3D->y;
|
||||
double z = (double)pt3D->z;
|
||||
sw << x << "," << y << "," << z << std::endl;
|
||||
}
|
||||
}
|
||||
sw.close();
|
||||
}
|
||||
|
||||
void _convertToGridData_XYZ_vector(std::vector<std::vector< SVzNL3DPosition>>& scanData, double _F, std::vector<std::vector< SVzNL3DPosition>>& scanData_grid)
|
||||
{
|
||||
int min_y = 100000000;
|
||||
int max_y = -10000000;
|
||||
int lineNum = scanData.size();
|
||||
for (int line = 0; line < lineNum; line++)
|
||||
{
|
||||
std::vector< SVzNL3DPosition>& a_line = scanData[line];
|
||||
int nPointCnt = a_line.size();
|
||||
for (int i = 0; i < nPointCnt; i++)
|
||||
{
|
||||
SVzNL3DPosition* a_pt = &scanData[line][i];
|
||||
if (a_pt->pt3D.z > 1e-4)
|
||||
{
|
||||
double v = _F * a_pt->pt3D.y / a_pt->pt3D.z + 2000;
|
||||
a_pt->nPointIdx = (int)(v + 0.5);
|
||||
max_y = max_y < (int)a_pt->nPointIdx ? (int)a_pt->nPointIdx : max_y;
|
||||
min_y = min_y > (int)a_pt->nPointIdx ? (int)a_pt->nPointIdx : min_y;
|
||||
}
|
||||
}
|
||||
}
|
||||
if (min_y == 100000000)
|
||||
return;
|
||||
|
||||
int pt_counter = max_y - min_y + 1;
|
||||
for (int line = 0; line < lineNum; line++)
|
||||
{
|
||||
std::vector< SVzNL3DPosition> gridData;
|
||||
gridData.resize(pt_counter);
|
||||
for (int i = 0; i < pt_counter; i++)
|
||||
gridData[i] = { 0,{ 0.0, 0.0, 0.0} };
|
||||
|
||||
std::vector< SVzNL3DPosition>& a_line = scanData[line];
|
||||
int nPointCnt = a_line.size();
|
||||
for (int i = 0; i < nPointCnt; i++)
|
||||
{
|
||||
SVzNL3DPosition a_pt = a_line[i];
|
||||
if (a_pt.pt3D.z > 1e-4)
|
||||
{
|
||||
int pt_id = a_pt.nPointIdx - min_y;
|
||||
gridData[pt_id] = a_pt;
|
||||
}
|
||||
}
|
||||
scanData_grid.push_back(gridData);
|
||||
}
|
||||
return;
|
||||
}
|
||||
|
||||
void _outputScanDataFile_XYZ_vector(char* fileName, std::vector<std::vector< SVzNL3DPosition>>& scanData)
|
||||
{
|
||||
std::ofstream sw(fileName);
|
||||
int lineNum = scanData.size();
|
||||
sw << "LineNum:" << lineNum << std::endl;
|
||||
sw << "DataType: 0" << std::endl;
|
||||
sw << "ScanSpeed: 0" << std::endl;
|
||||
sw << "PointAdjust: 1" << std::endl;
|
||||
sw << "MaxTimeStamp: 0_0" << std::endl;
|
||||
|
||||
for (int line = 0; line < lineNum; line++)
|
||||
{
|
||||
int nPositionCnt = scanData[line].size();
|
||||
sw << "Line_" << line << "_0_" << nPositionCnt << std::endl;
|
||||
for (int i = 0; i < nPositionCnt; i++)
|
||||
{
|
||||
SVzNL3DPosition* pt3D = &scanData[line][i];
|
||||
float x = (float)pt3D->pt3D.x;
|
||||
float y = (float)pt3D->pt3D.y;
|
||||
float z = (float)pt3D->pt3D.z;
|
||||
char str[250];
|
||||
sprintf_s(str, "{ %f, %f, %f } - { 0, 0 } - { 0, 0 }", x, y, z);
|
||||
|
||||
sw << str << std::endl;
|
||||
}
|
||||
}
|
||||
sw.close();
|
||||
}
|
||||
|
||||
void _getRoiData_XYZ_vector(
|
||||
std::vector<std::vector< SVzNL3DPosition>>& scanData,
|
||||
std::vector<std::vector< SVzNL3DPosition>>& roiData,
|
||||
SVzNL3DRangeD roi)
|
||||
{
|
||||
int lineNum = scanData.size();
|
||||
for (int line = 0; line < lineNum; line++)
|
||||
{
|
||||
int nPositionCnt = scanData[line].size();
|
||||
std::vector< SVzNL3DPosition> linePts;
|
||||
for (int i = 0; i < nPositionCnt; i++)
|
||||
{
|
||||
SVzNL3DPosition pt3D = scanData[line][i];
|
||||
if ((pt3D.pt3D.z >= roi.zRange.min) &&
|
||||
(pt3D.pt3D.z <= roi.zRange.max) &&
|
||||
(pt3D.pt3D.y >= roi.yRange.min) &&
|
||||
(pt3D.pt3D.y <= roi.yRange.max))
|
||||
{
|
||||
linePts.push_back(pt3D);
|
||||
}
|
||||
}
|
||||
roiData.push_back(linePts);
|
||||
}
|
||||
}
|
||||
|
||||
void _outputCalibPara(char* fileName, SSG_planeCalibPara calibPara)
|
||||
{
|
||||
std::ofstream sw(fileName);
|
||||
char dataStr[250];
|
||||
//调平矩阵
|
||||
sprintf_s(dataStr, 250, "%g, %g, %g", calibPara.planeCalib[0], calibPara.planeCalib[1], calibPara.planeCalib[2]);
|
||||
sw << dataStr << std::endl;
|
||||
sprintf_s(dataStr, 250, "%g, %g, %g", calibPara.planeCalib[3], calibPara.planeCalib[4], calibPara.planeCalib[5]);
|
||||
sw << dataStr << std::endl;
|
||||
sprintf_s(dataStr, 250, "%g, %g, %g", calibPara.planeCalib[6], calibPara.planeCalib[7], calibPara.planeCalib[8]);
|
||||
sw << dataStr << std::endl;
|
||||
//地面高度
|
||||
sprintf_s(dataStr, 250, "%g", calibPara.planeHeight);
|
||||
sw << dataStr << std::endl;
|
||||
//反向旋转矩阵
|
||||
sprintf_s(dataStr, 250, "%g, %g, %g", calibPara.invRMatrix[0], calibPara.invRMatrix[1], calibPara.invRMatrix[2]);
|
||||
sw << dataStr << std::endl;
|
||||
sprintf_s(dataStr, 250, "%g, %g, %g", calibPara.invRMatrix[3], calibPara.invRMatrix[4], calibPara.invRMatrix[5]);
|
||||
sw << dataStr << std::endl;
|
||||
sprintf_s(dataStr, 250, "%g, %g, %g", calibPara.invRMatrix[6], calibPara.invRMatrix[7], calibPara.invRMatrix[8]);
|
||||
sw << dataStr << std::endl;
|
||||
|
||||
sw.close();
|
||||
}
|
||||
|
||||
void _outputPoseInfo(char* fileName, SSX_BQAssemblyInfo assemblyPose)
|
||||
{
|
||||
std::ofstream sw(fileName);
|
||||
char dataStr[250];
|
||||
sprintf_s(dataStr, 250, " O: (%g, %g, %g)", assemblyPose.O.x, assemblyPose.O.y, assemblyPose.O.z);
|
||||
sw << dataStr << std::endl;
|
||||
sprintf_s(dataStr, 250, " X: (%g, %g, %g)", assemblyPose.X.x, assemblyPose.X.y, assemblyPose.X.z);
|
||||
sw << dataStr << std::endl;
|
||||
sprintf_s(dataStr, 250, " Y: (%g, %g, %g)", assemblyPose.Y.x, assemblyPose.Y.y, assemblyPose.Y.z);
|
||||
sw << dataStr << std::endl;
|
||||
sprintf_s(dataStr, 250, " Z: (%g, %g, %g)", assemblyPose.Z.x, assemblyPose.Z.y, assemblyPose.Z.z);
|
||||
sw << dataStr << std::endl;
|
||||
sw.close();
|
||||
}
|
||||
|
||||
SSX_BQAssemblyInfo _readPoseInfo(char* fileName)
|
||||
{
|
||||
std::ifstream inputFile(fileName);
|
||||
std::string linedata;
|
||||
|
||||
SSX_BQAssemblyInfo pose;
|
||||
memset(&pose, 0, sizeof(SSX_BQAssemblyInfo));
|
||||
if (inputFile.is_open() == false)
|
||||
return pose;
|
||||
|
||||
getline(inputFile, linedata);
|
||||
sscanf_s(linedata.c_str(), " O: (%lf, %lf, %lf)", &pose.O.x, &pose.O.y, &pose.O.z);
|
||||
getline(inputFile, linedata);
|
||||
sscanf_s(linedata.c_str(), " X: (%lf, %lf, %lf)", &pose.X.x, &pose.X.y, &pose.X.z);
|
||||
getline(inputFile, linedata);
|
||||
sscanf_s(linedata.c_str(), " Y: (%lf, %lf, %lf)", &pose.Y.x, &pose.Y.y, &pose.Y.z);
|
||||
getline(inputFile, linedata);
|
||||
sscanf_s(linedata.c_str(), " Z: (%lf, %lf, %lf)", &pose.Z.x, &pose.Z.y, &pose.Z.z);
|
||||
|
||||
inputFile.close();
|
||||
return pose;
|
||||
}
|
||||
|
||||
void _readMarkData(char* fileName, std::vector<SWD_charuco3DMark>& marks)
|
||||
{
|
||||
std::ifstream inputFile(fileName);
|
||||
std::string linedata;
|
||||
while (getline(inputFile, linedata))
|
||||
{
|
||||
SWD_charuco3DMark a_mark;
|
||||
float x, y, z;
|
||||
sscanf_s(linedata.c_str(), "MarkID_%d: %f, %f, %f", &a_mark.markID, &x, &y, &z);
|
||||
a_mark.mark3D = { x, y, z };
|
||||
marks.push_back(a_mark);
|
||||
}
|
||||
inputFile.close();
|
||||
return;
|
||||
}
|
||||
|
||||
void _outputScanDataFile_vector(char* fileName, std::vector<std::vector<SVzNL3DPosition>>& scanLines, bool removeZeros, int* headNullLines)
|
||||
{
|
||||
std::ofstream sw(fileName);
|
||||
int lineNum = (int)scanLines.size();
|
||||
if (lineNum == 0)
|
||||
return;
|
||||
|
||||
sw << "LineNum:" << lineNum << std::endl;
|
||||
sw << "DataType: 0" << std::endl;
|
||||
sw << "ScanSpeed: 0" << std::endl;
|
||||
sw << "PointAdjust: 1" << std::endl;
|
||||
sw << "MaxTimeStamp: 0_0" << std::endl;
|
||||
|
||||
int lineIdx = 0;
|
||||
int null_lines = 0;
|
||||
bool counterNull = true;
|
||||
for (int line = 0; line < lineNum; line++)
|
||||
{
|
||||
int linePtNum = (int)scanLines[line].size();
|
||||
if (linePtNum == 0)
|
||||
continue;
|
||||
|
||||
if (true == removeZeros)
|
||||
{
|
||||
int vldPtNum = 0;
|
||||
for (int i = 0; i < linePtNum; i++)
|
||||
{
|
||||
if (scanLines[line][i].pt3D.z > 1e-4)
|
||||
vldPtNum++;
|
||||
}
|
||||
linePtNum = vldPtNum;
|
||||
}
|
||||
sw << "Line_" << lineIdx << "_0_" << linePtNum << std::endl;
|
||||
lineIdx++;
|
||||
bool isNull = true;
|
||||
for (int i = 0; i < linePtNum; i++)
|
||||
{
|
||||
SVzNL3DPoint* pt3D = &scanLines[line][i].pt3D;
|
||||
if ((pt3D->z > 1e-4) && (isNull == true))
|
||||
isNull = false;
|
||||
if ((true == removeZeros) && (pt3D->z < 1e-4))
|
||||
continue;
|
||||
float x = (float)pt3D->x;
|
||||
float y = (float)pt3D->y;
|
||||
float z = (float)pt3D->z;
|
||||
sw << "{ " << x << "," << y << "," << z << " }-";
|
||||
sw << "{0,0}-{0,0}" << std::endl;
|
||||
}
|
||||
if (true == counterNull)
|
||||
{
|
||||
if (true == isNull)
|
||||
null_lines++;
|
||||
else
|
||||
counterNull = false;
|
||||
}
|
||||
}
|
||||
*headNullLines = null_lines;
|
||||
sw.close();
|
||||
}
|
||||
|
||||
SSG_planeCalibPara _readCalibPara(char* fileName)
|
||||
{
|
||||
//设置初始结果
|
||||
double initCalib[9] = {
|
||||
1.0, 0.0, 0.0,
|
||||
0.0, 1.0, 0.0,
|
||||
0.0, 0.0, 1.0 };
|
||||
SSG_planeCalibPara planePara;
|
||||
for (int i = 0; i < 9; i++)
|
||||
planePara.planeCalib[i] = initCalib[i];
|
||||
planePara.planeHeight = -1.0;
|
||||
for (int i = 0; i < 9; i++)
|
||||
planePara.invRMatrix[i] = initCalib[i];
|
||||
|
||||
std::ifstream inputFile(fileName);
|
||||
std::string linedata;
|
||||
|
||||
if (inputFile.is_open() == false)
|
||||
return planePara;
|
||||
|
||||
//调平矩阵
|
||||
std::getline(inputFile, linedata);
|
||||
sscanf_s(linedata.c_str(), "%lf, %lf, %lf", &planePara.planeCalib[0], &planePara.planeCalib[1], &planePara.planeCalib[2]);
|
||||
std::getline(inputFile, linedata);
|
||||
sscanf_s(linedata.c_str(), "%lf, %lf, %lf", &planePara.planeCalib[3], &planePara.planeCalib[4], &planePara.planeCalib[5]);
|
||||
std::getline(inputFile, linedata);
|
||||
sscanf_s(linedata.c_str(), "%lf, %lf, %lf", &planePara.planeCalib[6], &planePara.planeCalib[7], &planePara.planeCalib[8]);
|
||||
//地面高度
|
||||
std::getline(inputFile, linedata);
|
||||
sscanf_s(linedata.c_str(), "%lf", &planePara.planeHeight);
|
||||
//反向旋转矩阵
|
||||
std::getline(inputFile, linedata);
|
||||
sscanf_s(linedata.c_str(), "%lf, %lf, %lf", &planePara.invRMatrix[0], &planePara.invRMatrix[1], &planePara.invRMatrix[2]);
|
||||
std::getline(inputFile, linedata);
|
||||
sscanf_s(linedata.c_str(), "%lf, %lf, %lf", &planePara.invRMatrix[3], &planePara.invRMatrix[4], &planePara.invRMatrix[5]);
|
||||
std::getline(inputFile, linedata);
|
||||
sscanf_s(linedata.c_str(), "%lf, %lf, %lf", &planePara.invRMatrix[6], &planePara.invRMatrix[7], &planePara.invRMatrix[8]);
|
||||
|
||||
inputFile.close();
|
||||
return planePara;
|
||||
}
|
||||
|
||||
void _outputRGBDScanLapWeld_RGBD(
|
||||
char* fileName,
|
||||
std::vector<std::vector<SVzNL3DPosition>>& scanLines,
|
||||
SSX_BQAssemblyInfo assemblyPose)
|
||||
{
|
||||
int lineNum = (int)scanLines.size();
|
||||
std::ofstream sw(fileName);
|
||||
int realLines = lineNum;
|
||||
realLines++;
|
||||
sw << "LineNum:" << realLines << std::endl;
|
||||
sw << "DataType: 0" << std::endl;
|
||||
sw << "ScanSpeed: 0" << std::endl;
|
||||
sw << "PointAdjust: 1" << std::endl;
|
||||
sw << "MaxTimeStamp: 0_0" << std::endl;
|
||||
|
||||
int maxLineIndex = 0;
|
||||
int max_stamp = 0;
|
||||
|
||||
SG_color rgb = { 0, 0, 0 };
|
||||
|
||||
SG_color objColor[8] = {
|
||||
{245,222,179},//淡黄色
|
||||
{210,105, 30},//巧克力色
|
||||
{240,230,140},//黄褐色
|
||||
{135,206,235},//天蓝色
|
||||
{250,235,215},//古董白
|
||||
{189,252,201},//薄荷色
|
||||
{221,160,221},//梅红色
|
||||
{188,143,143},//玫瑰红色
|
||||
};
|
||||
int size = 1;
|
||||
int lineIdx = 0;
|
||||
for (int line = 0; line < lineNum; line++)
|
||||
{
|
||||
int linePtNum = (int)scanLines[line].size();
|
||||
if (linePtNum == 0)
|
||||
continue;
|
||||
|
||||
sw << "Line_" << lineIdx << "_0_" << linePtNum << std::endl;
|
||||
lineIdx++;
|
||||
for (int i = 0; i < linePtNum; i++)
|
||||
{
|
||||
SVzNL3DPosition* pt3D = &scanLines[line][i];
|
||||
if (pt3D->nPointIdx > 0)
|
||||
int kkk = 1;
|
||||
int featureType_v = pt3D->nPointIdx & 0xff;
|
||||
int featureType_h = featureType_v >> 4;
|
||||
featureType_v &= 0x0f;
|
||||
|
||||
if (pt3D->nPointIdx == 1)
|
||||
{
|
||||
rgb = { 255, 97, 0 };
|
||||
size = 5;
|
||||
}
|
||||
else
|
||||
{
|
||||
rgb = { 200, 200, 200 };
|
||||
size = 1;
|
||||
}
|
||||
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}-";
|
||||
sw << "{" << rgb.r << "," << rgb.g << "," << rgb.b << "," << size << " }" << std::endl;
|
||||
}
|
||||
}
|
||||
|
||||
int linePtNum = 1;
|
||||
sw << "Line_" << lineNum << "_0_" << linePtNum + 1 << std::endl;
|
||||
lineNum++;
|
||||
|
||||
rgb = { 255, 0, 0 };
|
||||
size = 15;
|
||||
float x = (float)assemblyPose.O.x;
|
||||
float y = (float)assemblyPose.O.y;
|
||||
float z = (float)assemblyPose.O.z;
|
||||
sw << "{" << x << "," << y << "," << z << "}-";
|
||||
sw << "{0,0}-{0,0}-";
|
||||
sw << "{" << rgb.r << "," << rgb.g << "," << rgb.b << "," << size << " }" << std::endl;
|
||||
sw.close();
|
||||
}
|
||||
|
||||
SVzNL3DPoint _pointRT(SVzNL3DPoint& origin, const double* R, const double* T)
|
||||
{
|
||||
SVzNL3DPoint result;
|
||||
result.x = origin.x * R[0] + origin.y * R[1] + origin.z * R[2];
|
||||
result.y = origin.x * R[3] + origin.y * R[4] + origin.z * R[5];
|
||||
result.z = origin.x * R[6] + origin.y * R[7] + origin.z * R[8];
|
||||
result.x += T[0];
|
||||
result.y += T[1];
|
||||
result.z += T[2];
|
||||
return result;
|
||||
}
|
||||
|
||||
|
||||
void _wirteVerifyResult(char* fileName,
|
||||
std::vector<std::vector< SVzNL3DPoint>>& originMarksBuff,
|
||||
std::vector<std::vector< SVzNL3DPoint>>& currMarksBuff,
|
||||
std::vector< SSX_BQAssemblyInfo>& oiginPoseBuff,
|
||||
std::vector< SSX_BQAssemblyInfo>& computePoseBuff,
|
||||
std::vector< SSX_BQAssemblyInfo>& verifyPoseBuff)
|
||||
{
|
||||
std::ofstream sw(fileName);
|
||||
char dataStr[250];
|
||||
|
||||
for (int i = 0; i < (int)originMarksBuff.size(); i++)
|
||||
{
|
||||
int markSize = (int)originMarksBuff[i].size();
|
||||
sprintf_s(dataStr, 250, "origin: ");
|
||||
for (int j = 0; j < markSize; j++)
|
||||
{
|
||||
sprintf_s(dataStr, 250, "{ %g, %g, %g },", originMarksBuff[i][j].x, originMarksBuff[i][j].y, originMarksBuff[i][j].z);
|
||||
sw << dataStr;
|
||||
}
|
||||
sprintf_s(dataStr, 250, " operate: ");
|
||||
for (int j = 0; j < markSize; j++)
|
||||
{
|
||||
sprintf_s(dataStr, 250, "{ %g, %g, %g },", currMarksBuff[i][j].x, currMarksBuff[i][j].y, currMarksBuff[i][j].z);
|
||||
sw << dataStr;
|
||||
}
|
||||
sprintf_s(dataStr, 250, " oriPose: { %g, %g, %g },", oiginPoseBuff[i].O.x, oiginPoseBuff[i].O.y, oiginPoseBuff[i].O.z);
|
||||
sw << dataStr;
|
||||
sprintf_s(dataStr, 250, " verifyPose: { %g, %g, %g },", verifyPoseBuff[i].O.x, verifyPoseBuff[i].O.y, verifyPoseBuff[i].O.z);
|
||||
sw << dataStr;
|
||||
sprintf_s(dataStr, 250, " computePose: { %g, %g, %g },", computePoseBuff[i].O.x, computePoseBuff[i].O.y, computePoseBuff[i].O.z);
|
||||
sw << dataStr << std::endl;
|
||||
}
|
||||
sw.close();
|
||||
return;
|
||||
}
|
||||
|
||||
#define CONVERT_TO_GRID 0
|
||||
#define TEST_COMPUTE_CALIB_PARA 0
|
||||
#define TEST_COMPUTE_CORNER 0
|
||||
#define TEST_COMPUTE_POSE 1
|
||||
#define TEST_GROUP 2
|
||||
int main()
|
||||
{
|
||||
std::cout << "Hello World!\n";
|
||||
const char* dataPath[TEST_GROUP] = {
|
||||
|
||||
"F:/ShangGu/项目/冠钦_博清科技/组装/3D与Mark/21epic/", //0
|
||||
"F:/ShangGu/项目/冠钦_博清科技/组装/3D与Mark/20vizum/",
|
||||
};
|
||||
|
||||
SVzNLRange fileIdx[TEST_GROUP] = {
|
||||
{2,33},
|
||||
};
|
||||
|
||||
#ifdef TEST_COMPUTE_POSE
|
||||
double R_3D_mark[9] = {
|
||||
9.7414217523834201e-01, -6.4917977205419487e-03, -2.2584259780711641e-01,
|
||||
6.4677677434932046e-03, 9.9997872563826395e-01, - 8.4631630596682995e-04,
|
||||
2.2584328726426117e-01, - 6.3626506196937213e-04, 9.7416343842521791e-01
|
||||
};
|
||||
double T_3D_mark[3] = {
|
||||
2.0118012741488724e+02, -3.4890651052684677e+02,-2.9090717029324466e+01
|
||||
};
|
||||
double R_mark_3D[9] = {
|
||||
9.7414217523834201e-01, 6.4677677434932046e-03, 2.2584328726426117e-01,
|
||||
-6.4917977205419487e-03, 9.9997872563826395e-01,-6.3626506196937213e-04,
|
||||
-2.2584259780711641e-01, -8.4631630596682995e-04, 9.7416343842521791e-01
|
||||
};
|
||||
double T_mark_3D[3] = {
|
||||
-1.8715145749760768e+02, 3.5018659904922669e+02, 7.3478870260969714e+01
|
||||
};
|
||||
|
||||
double R_3D_mark_20[9] = {
|
||||
-9.7551111071544561e-01, 1.0398329224714965e-02,-2.1970422758802807e-01,
|
||||
- 8.3582333026743106e-03,- 9.9991291186786824e-01, -1.0213158961794689e-02,
|
||||
- 2.1979129374653508e-01, -8.1267108509691516e-03, 9.7551306693655782e-01,
|
||||
};
|
||||
double T_3D_mark_20[3] = {
|
||||
5.7873032211966097e+02, 6.3216378913452741e+02, 6.5977504397803401e+01,
|
||||
};
|
||||
double R_mark_3D_20[9] = {
|
||||
-9.7551111071544561e-01, -8.3582333026743106e-03, -2.1979129374653508e-01,
|
||||
1.0398329224714965e-02, -9.9991291186786824e-01, -8.1267108509691516e-03,
|
||||
-2.1970422758802807e-01, -1.0213158961794689e-02, 9.7551306693655782e-01
|
||||
};
|
||||
double T_mark_3D_20[3] = {
|
||||
5.8434291282050799e+02, 6.2662708685011432e+02, 6.9243970007470736e+01
|
||||
};
|
||||
|
||||
for (int grp = 0; grp <= 0; grp++)
|
||||
{
|
||||
char _scan_file[256];
|
||||
std::vector<std::vector< SVzNL3DPoint>> originMarksBuff;
|
||||
std::vector<std::vector< SVzNL3DPoint>> currMarksBuff;
|
||||
std::vector< SSX_BQAssemblyInfo> oiginPoseBuff;
|
||||
std::vector< SSX_BQAssemblyInfo> computePoseBuff;
|
||||
std::vector< SSX_BQAssemblyInfo> verifyPoseBuff;
|
||||
for (int fidx = fileIdx[grp].nMin; fidx <= fileIdx[grp].nMax; fidx++)
|
||||
{
|
||||
//fidx =4;
|
||||
char _scan_file[256];
|
||||
sprintf_s(_scan_file, "%sresult/LaserLine%d_corner_info.txt", dataPath[grp], fidx);
|
||||
SSX_BQAssemblyInfo originPose_raw = _readPoseInfo(_scan_file);
|
||||
SSX_BQAssemblyInfo originPose;
|
||||
originPose.O = _pointRT(originPose_raw.O, R_3D_mark, T_3D_mark);
|
||||
SSX_BQAssemblyInfo originPose_check;
|
||||
originPose_check.O = _pointRT(originPose.O, R_mark_3D, T_mark_3D);
|
||||
|
||||
sprintf_s(_scan_file, "%sresult/marks_%d.txt", dataPath[grp], fidx);
|
||||
std::vector<SWD_charuco3DMark> marks_origin;
|
||||
_readMarkData(_scan_file, marks_origin);
|
||||
|
||||
sprintf_s(_scan_file, "%sresult/marks_%d.txt", dataPath[grp+1], fidx);
|
||||
std::vector<SWD_charuco3DMark> marks_curr;
|
||||
_readMarkData(_scan_file, marks_curr);
|
||||
|
||||
//配对
|
||||
std::vector<SVzNL3DPoint> originMarkPos;
|
||||
std::vector<SVzNL3DPoint> currMarkPos;
|
||||
for (int i = 0; i < (int)marks_origin.size(); i++)
|
||||
{
|
||||
for (int j = 0; j < (int)marks_curr.size(); j++)
|
||||
{
|
||||
if (marks_origin[i].markID == marks_curr[j].markID)
|
||||
{
|
||||
SVzNL3DPoint pt_1 = { marks_origin[i].mark3D.x, marks_origin[i].mark3D.y, marks_origin[i].mark3D.z };
|
||||
SVzNL3DPoint pt_2 = { marks_curr[j].mark3D.x, marks_curr[j].mark3D.y, marks_curr[j].mark3D.z };
|
||||
originMarkPos.push_back(pt_1);
|
||||
currMarkPos.push_back(pt_2);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
SSX_BQAssemblyInfo testPose_origin, testPose_verify;
|
||||
|
||||
if (originMarkPos.size() >= 4) //同时使用最后一个作验证
|
||||
{
|
||||
testPose_origin.O = originMarkPos.back();
|
||||
testPose_verify.O = currMarkPos.back();
|
||||
originMarkPos.pop_back();
|
||||
currMarkPos.pop_back();
|
||||
|
||||
originMarksBuff.push_back(originMarkPos);
|
||||
currMarksBuff.push_back(currMarkPos);
|
||||
|
||||
int errCode = 0;
|
||||
SSX_BQAssemblyInfo currPose = sx_BQ_computeAssemblyInfoFromMark(
|
||||
originPose, originMarkPos, currMarkPos, &errCode);
|
||||
|
||||
SSX_BQAssemblyInfo compute_testPose = sx_BQ_computeAssemblyInfoFromMark(
|
||||
testPose_origin, originMarkPos, currMarkPos, &errCode);
|
||||
#if 0
|
||||
SSX_BQAssemblyInfo checkPose;
|
||||
checkPose.O = _pointRT(originPose_raw.O, R_3D_mark_20, T_3D_mark_20);
|
||||
SSX_BQAssemblyInfo checkPose_check;
|
||||
checkPose_check.O = _pointRT(checkPose.O, R_mark_3D_20, T_mark_3D_20);
|
||||
#endif
|
||||
if (errCode != 0)
|
||||
continue;
|
||||
|
||||
//输出
|
||||
oiginPoseBuff.push_back(testPose_origin);
|
||||
computePoseBuff.push_back(compute_testPose);
|
||||
verifyPoseBuff.push_back(testPose_verify);
|
||||
}
|
||||
|
||||
}
|
||||
sprintf_s(_scan_file, "%sresult/group_verify_data.txt", dataPath[grp]);
|
||||
_wirteVerifyResult(_scan_file,
|
||||
originMarksBuff,
|
||||
currMarksBuff,
|
||||
oiginPoseBuff,
|
||||
computePoseBuff,
|
||||
verifyPoseBuff);
|
||||
}
|
||||
|
||||
#endif
|
||||
#if TEST_COMPUTE_CALIB_PARA
|
||||
char _calib_datafile[256];
|
||||
sprintf_s(_calib_datafile, "F:/ShangGu/项目/冠钦_博清科技/组装/3D与Mark/21epic/调平/scan3D_0.txt");
|
||||
int lineNum = 0;
|
||||
float lineV = 0.0f;
|
||||
int dataCalib = 0;
|
||||
int maxTimeStamp = 0;
|
||||
int clockPerSecond = 0;
|
||||
std::vector<std::vector< SVzNL3DPosition>> scanData;
|
||||
vzReadLaserScanPointFromFile_XYZ_vector(_calib_datafile, scanData);
|
||||
|
||||
|
||||
SVzNL3DRangeD roi;
|
||||
roi.xRange.min = -DBL_MAX;
|
||||
roi.xRange.max = DBL_MAX;
|
||||
roi.yRange.min = -DBL_MAX;
|
||||
roi.yRange.max = 580.0;
|
||||
roi.zRange.min = 2380.0;
|
||||
roi.zRange.max = 2460.0;
|
||||
|
||||
std::vector<std::vector< SVzNL3DPosition>> roiData;
|
||||
_getRoiData_XYZ_vector(scanData, roiData, roi);
|
||||
|
||||
lineNum = (int)scanData.size();
|
||||
if (scanData.size() > 0)
|
||||
{
|
||||
SSG_planeCalibPara calibPara = sx_BQ_getHoleBaseCalibPara(scanData);
|
||||
//结果进行验证
|
||||
for (int i = 0; i < lineNum; i++)
|
||||
{
|
||||
if (i == 14)
|
||||
int kkk = 1;
|
||||
//行处理
|
||||
//调平，去除地面
|
||||
sx_BQ_lineDataR(scanData[i], calibPara.planeCalib, -1); // calibPara.planeHeight);
|
||||
}
|
||||
//
|
||||
char calibFile[250];
|
||||
sprintf_s(calibFile, "F:/ShangGu/项目/冠钦_博清科技/组装/3D与Mark/21epic/ground_calib_para.txt");
|
||||
_outputCalibPara(calibFile, calibPara);
|
||||
char _out_file[256];
|
||||
sprintf_s(_out_file, "F:/ShangGu/项目/冠钦_博清科技/组装/3D与Mark/21epic/调平/scanData_ground_calib.txt");
|
||||
int headNullLines = 0;
|
||||
_outputScanDataFile_vector(_out_file, scanData, false, &headNullLines);
|
||||
printf("%s: calib done!\n", _calib_datafile);
|
||||
}
|
||||
#endif
|
||||
|
||||
#if TEST_COMPUTE_CORNER
|
||||
const char* ver = wd_BQAssemblyPositionVersion();
|
||||
printf("ver:%s\n", ver);
|
||||
|
||||
for (int grp = 0; grp <= 0; grp++)
|
||||
{
|
||||
SSG_planeCalibPara poseCalibPara;
|
||||
//初始化成单位阵
|
||||
poseCalibPara.planeCalib[0] = 1.0;
|
||||
poseCalibPara.planeCalib[1] = 0.0;
|
||||
poseCalibPara.planeCalib[2] = 0.0;
|
||||
poseCalibPara.planeCalib[3] = 0.0;
|
||||
poseCalibPara.planeCalib[4] = 1.0;
|
||||
poseCalibPara.planeCalib[5] = 0.0;
|
||||
poseCalibPara.planeCalib[6] = 0.0;
|
||||
poseCalibPara.planeCalib[7] = 0.0;
|
||||
poseCalibPara.planeCalib[8] = 1.0;
|
||||
poseCalibPara.planeHeight = -1.0;
|
||||
for (int i = 0; i < 9; i++)
|
||||
poseCalibPara.invRMatrix[i] = poseCalibPara.planeCalib[i];
|
||||
char calibFile[250];
|
||||
sprintf_s(calibFile, "%sground_calib_para.txt", dataPath[grp]);
|
||||
poseCalibPara = _readCalibPara(calibFile);
|
||||
|
||||
for (int fidx = fileIdx[grp].nMin; fidx <= fileIdx[grp].nMax; fidx++)
|
||||
{
|
||||
//fidx =4;
|
||||
char _scan_file[256];
|
||||
sprintf_s(_scan_file, "%sscan3D_%d.txt", dataPath[grp], fidx);
|
||||
std::vector<std::vector< SVzNL3DPosition>> scanLines;
|
||||
vzReadLaserScanPointFromFile_XYZ_vector(_scan_file, scanLines);
|
||||
if (scanLines.size() == 0)
|
||||
continue;
|
||||
|
||||
//转成plyTxt格式
|
||||
//sprintf_s(_scan_file, "%s%d_ply_Hi229229.txt", dataPath[grp], fidx);
|
||||
//wdSavePlyTxt(_scan_file, scanLines);
|
||||
|
||||
long t1 = (long)GetTickCount64();//统计时间
|
||||
|
||||
|
||||
#if 0
|
||||
char _out_file[256];
|
||||
sprintf_s(_out_file, "%sscanData_%d_calib.txt", dataPath[grp], fidx);
|
||||
int headNullLines = 0;
|
||||
_outputScanDataFile_vector(_out_file, scanLines, false, &headNullLines);
|
||||
#endif
|
||||
|
||||
SSG_cornerParam cornerParam;
|
||||
cornerParam.cornerTh = 30; //45度角
|
||||
cornerParam.scale = 5; // algoParam.bagParam.bagH / 8; // 15; // algoParam.bagParam.bagH / 8;
|
||||
cornerParam.minEndingGap = 20; // algoParam.bagParam.bagW / 4;
|
||||
cornerParam.minEndingGap_z = 20;
|
||||
cornerParam.jumpCornerTh_1 = 10; //水平角度，小于此角度视为水平
|
||||
cornerParam.jumpCornerTh_2 = 60;
|
||||
SSG_outlierFilterParam filterParam;
|
||||
filterParam.continuityTh = 20.0; //噪声滤除。当相邻点的z跳变大于此门限时，检查是否为噪声。若长度小于outlierLen， 视为噪声
|
||||
filterParam.outlierTh = 5;
|
||||
SSG_treeGrowParam growParam;
|
||||
growParam.maxLineSkipNum = 10;
|
||||
growParam.yDeviation_max = 10.0;
|
||||
growParam.maxSkipDistance = 10.0;
|
||||
growParam.zDeviation_max = 10.0;// algoParam.bagParam.bagH / 2; //袋子高度1/2
|
||||
growParam.minLTypeTreeLen = 100; //mm
|
||||
growParam.minVTypeTreeLen = 100; //mm
|
||||
SSX_BQAssemblyPara workpieceParam;
|
||||
workpieceParam.type = 0;
|
||||
workpieceParam.lineLen = 160.0; //直线段长度
|
||||
int errCode = 0;
|
||||
SSX_BQAssemblyInfo assemblyPose = sx_BQ_computeAssemblyInfoFrom3D(
|
||||
scanLines,
|
||||
cornerParam,
|
||||
filterParam,
|
||||
growParam,
|
||||
poseCalibPara,
|
||||
workpieceParam,
|
||||
&errCode);
|
||||
long t2 = (long)GetTickCount64();
|
||||
printf("%s: %d(ms)!\n", _scan_file, (int)(t2 - t1));
|
||||
//输出测试结果
|
||||
//sprintf_s(_scan_file, "%sresult\\LaserLine%d_result.txt", dataPath[grp], fidx);
|
||||
//_outputRGBDScanLapWeld_RGBD(_scan_file, scanLines, assemblyPose);
|
||||
sprintf_s(calibFile, "%sresult\\LaserLine%d_corner_info.txt", dataPath[grp], fidx);
|
||||
_outputPoseInfo(calibFile, assemblyPose);
|
||||
}
|
||||
}
|
||||
#endif
|
||||
}
|
||||
|
||||
// 运行程序: Ctrl + F5 或调试 >“开始执行(不调试)”菜单
|
||||
|
||||
@ -78,9 +78,13 @@
|
||||
</PropertyGroup>
|
||||
<PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Debug|x64'">
|
||||
<LinkIncremental>true</LinkIncremental>
|
||||
<OutDir>$(SolutionDir)build\$(Platform)\$(Configuration)\</OutDir>
|
||||
<IncludePath>..\..\thirdParty\VzNLSDK\Inc;..\sourceCode;..\sourceCode\inc;$(IncludePath)</IncludePath>
|
||||
</PropertyGroup>
|
||||
<PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Release|x64'">
|
||||
<LinkIncremental>false</LinkIncremental>
|
||||
<OutDir>$(SolutionDir)build\$(Platform)\$(Configuration)\</OutDir>
|
||||
<IncludePath>..\..\thirdParty\VzNLSDK\Inc;..\sourceCode;..\sourceCode\inc;$(IncludePath)</IncludePath>
|
||||
</PropertyGroup>
|
||||
<ItemDefinitionGroup Condition="'$(Configuration)|$(Platform)'=='Debug|Win32'">
|
||||
<ClCompile>
|
||||
@ -114,12 +118,15 @@
|
||||
<ClCompile>
|
||||
<WarningLevel>Level3</WarningLevel>
|
||||
<SDLCheck>true</SDLCheck>
|
||||
<PreprocessorDefinitions>_DEBUG;_CONSOLE;%(PreprocessorDefinitions)</PreprocessorDefinitions>
|
||||
<PreprocessorDefinitions>_CRT_SECURE_NO_WARNINGS;_DEBUG;_CONSOLE;%(PreprocessorDefinitions)</PreprocessorDefinitions>
|
||||
<ConformanceMode>true</ConformanceMode>
|
||||
<AdditionalIncludeDirectories>..\..\thirdParty\opencv320\build\include;</AdditionalIncludeDirectories>
|
||||
</ClCompile>
|
||||
<Link>
|
||||
<SubSystem>Console</SubSystem>
|
||||
<GenerateDebugInformation>true</GenerateDebugInformation>
|
||||
<AdditionalLibraryDirectories>..\..\thirdParty\opencv320\build\x64\vc14\lib;..\build\x64\Debug;%(AdditionalLibraryDirectories)</AdditionalLibraryDirectories>
|
||||
<AdditionalDependencies>opencv_world320d.lib;BQ_assemblyPosition.lib;%(AdditionalDependencies)</AdditionalDependencies>
|
||||
</Link>
|
||||
</ItemDefinitionGroup>
|
||||
<ItemDefinitionGroup Condition="'$(Configuration)|$(Platform)'=='Release|x64'">
|
||||
@ -128,14 +135,17 @@
|
||||
<FunctionLevelLinking>true</FunctionLevelLinking>
|
||||
<IntrinsicFunctions>true</IntrinsicFunctions>
|
||||
<SDLCheck>true</SDLCheck>
|
||||
<PreprocessorDefinitions>NDEBUG;_CONSOLE;%(PreprocessorDefinitions)</PreprocessorDefinitions>
|
||||
<PreprocessorDefinitions>_CRT_SECURE_NO_WARNINGS;NDEBUG;_CONSOLE;%(PreprocessorDefinitions)</PreprocessorDefinitions>
|
||||
<ConformanceMode>true</ConformanceMode>
|
||||
<AdditionalIncludeDirectories>..\..\thirdParty\opencv320\build\include;</AdditionalIncludeDirectories>
|
||||
</ClCompile>
|
||||
<Link>
|
||||
<SubSystem>Console</SubSystem>
|
||||
<EnableCOMDATFolding>true</EnableCOMDATFolding>
|
||||
<OptimizeReferences>true</OptimizeReferences>
|
||||
<GenerateDebugInformation>true</GenerateDebugInformation>
|
||||
<AdditionalLibraryDirectories>..\..\thirdParty\opencv320\build\x64\vc14\lib;..\build\x64\Release;%(AdditionalLibraryDirectories)</AdditionalLibraryDirectories>
|
||||
<AdditionalDependencies>opencv_world320.lib;BQ_assemblyPosition.lib;%(AdditionalDependencies)</AdditionalDependencies>
|
||||
</Link>
|
||||
</ItemDefinitionGroup>
|
||||
<ItemGroup>
|
||||
|
||||
@ -12,22 +12,38 @@
|
||||
#include <opencv2/imgcodecs/imgcodecs.hpp>
|
||||
#include <Windows.h>
|
||||
|
||||
#define TEST_GROUP 3
|
||||
void _outputMarkData(char* fileName, std::vector<SWD_charuco3DMark>& marks)
|
||||
{
|
||||
std::ofstream sw(fileName);
|
||||
char dataStr[250];
|
||||
for (int i = 0; i < (int)marks.size(); i++)
|
||||
{
|
||||
sprintf_s(dataStr, 250, "MarkID_%d: %g, %g, %g", marks[i].markID, marks[i].mark3D.x, marks[i].mark3D.y, marks[i].mark3D.z);
|
||||
sw << dataStr << std::endl;
|
||||
}
|
||||
sw.close();
|
||||
}
|
||||
|
||||
#define TEST_GROUP 6
|
||||
int main(int argc, char** argv)
|
||||
{
|
||||
const char* dataPath[TEST_GROUP] = {
|
||||
"F:/标定采图/charucoMark图/MarkTest/", //0
|
||||
"F:/标定采图/charucoMark图/Mark_13度/",
|
||||
"F:/ShangGu/项目/冠钦_博清科技/组装/数据/mark测试/" };
|
||||
"F:/ShangGu/项目/冠钦_博清科技/组装/数据/mark测试/",
|
||||
"F:/ShangGu/项目/冠钦_博清科技/组装/数据/现场Mark数据/",
|
||||
"F:/ShangGu/项目/冠钦_博清科技/组装/3D与Mark/21epic/",
|
||||
"F:/ShangGu/项目/冠钦_博清科技/组装/3D与Mark/20vizum/",
|
||||
};
|
||||
|
||||
SVzNLRange fileIdx[TEST_GROUP] = {
|
||||
{1,4}, {1,5}, {1,9}
|
||||
{1,4}, {1,5}, {1,9}, {1, 1}, {2,33}, {2,33}
|
||||
};
|
||||
|
||||
const char* ver = wd_charuco3DMarkVersion();
|
||||
printf("ver:%s\n", ver);
|
||||
|
||||
for (int grp = 2; grp < 3; grp++)
|
||||
for (int grp = 4; grp < 6; grp++)
|
||||
{
|
||||
SWD_BQ_CharucoMarkInfo markInfo;
|
||||
markInfo.patternSize = cv::Size(3, 3);
|
||||
@ -78,7 +94,7 @@ int main(int argc, char** argv)
|
||||
cv::Mat rightimg = cv::imread(filename, cv::IMREAD_GRAYSCALE);
|
||||
|
||||
if (leftimg.empty() || rightimg.empty())
|
||||
break;
|
||||
continue;
|
||||
|
||||
double disparityOffset = 0;
|
||||
std::vector<SWD_charuco3DMark> marks;
|
||||
@ -89,6 +105,9 @@ int main(int argc, char** argv)
|
||||
disparityOffset,
|
||||
marks);
|
||||
|
||||
sprintf_s(calibFile, sizeof(calibFile), "%sresult/marks_%d.txt", dataPath[grp], index);
|
||||
_outputMarkData(calibFile, marks);
|
||||
|
||||
printf("%s:\n", filename);
|
||||
for (int i = 0; i < marks.size(); i++)
|
||||
{
|
||||
|
||||
361
HC_chanelSpaceMeasure_test/HC_chanelSpaceMeasure_test.cpp
Normal file
361
HC_chanelSpaceMeasure_test/HC_chanelSpaceMeasure_test.cpp
Normal file
@ -0,0 +1,361 @@
|
||||
// gasFillingPortPosition_test.cpp : 此文件包含 "main" 函数。程序执行将在此处开始并结束。
|
||||
//
|
||||
|
||||
#include <iostream>
|
||||
#include <fstream>
|
||||
#include <vector>
|
||||
#include <stdio.h>
|
||||
#include <VZNL_Types.h>
|
||||
#include "direct.h"
|
||||
#include <string>
|
||||
#include "channelSpaceMeasure_Export.h"
|
||||
#include <opencv2/opencv.hpp>
|
||||
#include <Windows.h>
|
||||
#include <limits>
|
||||
|
||||
typedef struct
|
||||
{
|
||||
int r;
|
||||
int g;
|
||||
int b;
|
||||
}SG_color;
|
||||
|
||||
typedef struct
|
||||
{
|
||||
int nPointIdx;
|
||||
double x;
|
||||
double y;
|
||||
double z;
|
||||
float r;
|
||||
float g;
|
||||
float b;
|
||||
} SPointXYZRGB;
|
||||
|
||||
void wdReadLaserScanPointFromFile_XYZ_vector(const char* fileName, std::vector<std::vector< SVzNL3DPosition>>& scanData)
|
||||
{
|
||||
std::ifstream inputFile(fileName);
|
||||
std::string linedata;
|
||||
|
||||
if (inputFile.is_open() == false)
|
||||
return;
|
||||
|
||||
std::vector< SVzNL3DPosition> a_line;
|
||||
int ptIdx = 0;
|
||||
while (getline(inputFile, linedata))
|
||||
{
|
||||
if (0 == strncmp("Line_", linedata.c_str(), 5))
|
||||
{
|
||||
int ptSize = (int)a_line.size();
|
||||
if (ptSize > 0)
|
||||
{
|
||||
scanData.push_back(a_line);
|
||||
}
|
||||
a_line.clear();
|
||||
ptIdx = 0;
|
||||
}
|
||||
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);
|
||||
SVzNL3DPosition a_pt;
|
||||
a_pt.pt3D.x = X;
|
||||
a_pt.pt3D.y = Y;
|
||||
a_pt.pt3D.z = Z;
|
||||
a_pt.nPointIdx = ptIdx;
|
||||
ptIdx++;
|
||||
a_line.push_back(a_pt);
|
||||
}
|
||||
}
|
||||
//last line
|
||||
int ptSize = (int)a_line.size();
|
||||
if (ptSize > 0)
|
||||
{
|
||||
scanData.push_back(a_line);
|
||||
a_line.clear();
|
||||
}
|
||||
|
||||
inputFile.close();
|
||||
return;
|
||||
}
|
||||
|
||||
void wd_gridScan_GetROIData(std::vector<std::vector< SVzNL3DPosition>>& scanData, SVzNLRangeD roi_y, std::vector<std::vector< SVzNL3DPosition>>& roiData)
|
||||
{
|
||||
int lineNum = (int)scanData.size();
|
||||
int linePtNum = (int)scanData[0].size();
|
||||
int globalPtStart = INT_MAX;
|
||||
int globalPtEnd = 0;
|
||||
int lineStart = INT_MAX;
|
||||
int lineEnd = 0;
|
||||
for (int line = 0; line < lineNum; line++)
|
||||
{
|
||||
std::vector< SVzNL3DPosition >& lineData = scanData[line];
|
||||
int ptSize = (int)lineData.size();
|
||||
int vldNum = 0;
|
||||
int ptStart = INT_MAX;
|
||||
int ptEnd = 0;
|
||||
for (int i = 0; i < ptSize; i++)
|
||||
{
|
||||
if (lineData[i].pt3D.z > 1e-4)
|
||||
{
|
||||
if ((lineData[i].pt3D.y < roi_y.min) || (lineData[i].pt3D.y > roi_y.max))
|
||||
lineData[i].pt3D = { 0.0, 0.0, 0.0 };
|
||||
}
|
||||
|
||||
if (lineData[i].pt3D.z > 1e-4)
|
||||
{
|
||||
if (ptStart > i)
|
||||
ptStart = i;
|
||||
ptEnd = i;
|
||||
vldNum++;
|
||||
}
|
||||
}
|
||||
if (vldNum > 0)
|
||||
{
|
||||
if (globalPtStart > ptStart)
|
||||
globalPtStart = ptStart;
|
||||
if (globalPtEnd < ptEnd)
|
||||
globalPtEnd = ptEnd;
|
||||
|
||||
if (lineStart > line)
|
||||
lineStart = line;
|
||||
lineEnd = line;
|
||||
}
|
||||
}
|
||||
int vldLineNum = lineEnd - lineStart + 1;
|
||||
int vldPtNum = globalPtEnd - globalPtStart + 1;
|
||||
|
||||
roiData.resize(vldLineNum);
|
||||
for (int line = 0; line < vldLineNum; line++)
|
||||
{
|
||||
roiData[line].resize(vldPtNum);
|
||||
for (int i = 0; i < vldPtNum; i++)
|
||||
roiData[line][i] = scanData[line + lineStart][i + globalPtStart];
|
||||
}
|
||||
return;
|
||||
}
|
||||
|
||||
void _outputScanDataFile(char* fileName, std::vector<std::vector< SVzNL3DPosition>>& scanData,
|
||||
float lineV, int maxTimeStamp, int clockPerSecond)
|
||||
{
|
||||
std::ofstream sw(fileName);
|
||||
|
||||
int lineNum = (int)scanData.size();
|
||||
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++)
|
||||
{
|
||||
int nPositionCnt = (int)scanData[line].size();
|
||||
sw << "Line_" << line << "_0_" << nPositionCnt << std::endl;
|
||||
for (int i = 0; i < nPositionCnt; i++)
|
||||
{
|
||||
SVzNL3DPosition& pt3D = scanData[line][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();
|
||||
}
|
||||
|
||||
void _outputChanneltInfo(char* fileName, SSX_channelInfo channelInfo)
|
||||
{
|
||||
std::ofstream sw(fileName);
|
||||
|
||||
char dataStr[250];
|
||||
sprintf_s(dataStr, 250, "槽道间距: %g", channelInfo.channelSpace);
|
||||
sw << dataStr << std::endl;
|
||||
sprintf_s(dataStr, 250, "槽道1: 宽_%g, 深_%g", channelInfo.channelWidth[0], channelInfo.channelDepth[0]);
|
||||
sw << dataStr << std::endl;
|
||||
sprintf_s(dataStr, 250, "槽道2: 宽_%g, 深_%g", channelInfo.channelWidth[1], channelInfo.channelDepth[1]);
|
||||
sw << dataStr << std::endl;
|
||||
sw.close();
|
||||
}
|
||||
|
||||
void _outputRGBDScan_RGBD(
|
||||
char* fileName,
|
||||
std::vector<std::vector<SVzNL3DPosition>>& scanLines
|
||||
)
|
||||
{
|
||||
int lineNum = (int)scanLines.size();
|
||||
std::ofstream sw(fileName);
|
||||
int realLines = lineNum;
|
||||
|
||||
sw << "LineNum:" << realLines << std::endl;
|
||||
sw << "DataType: 0" << std::endl;
|
||||
sw << "ScanSpeed: 0" << std::endl;
|
||||
sw << "PointAdjust: 1" << std::endl;
|
||||
sw << "MaxTimeStamp: 0_0" << std::endl;
|
||||
|
||||
int maxLineIndex = 0;
|
||||
int max_stamp = 0;
|
||||
|
||||
SG_color rgb = { 0, 0, 0 };
|
||||
|
||||
SG_color objColor[8] = {
|
||||
{245,222,179},//淡黄色
|
||||
{210,105, 30},//巧克力色
|
||||
{240,230,140},//黄褐色
|
||||
{135,206,235},//天蓝色
|
||||
{250,235,215},//古董白
|
||||
{189,252,201},//薄荷色
|
||||
{221,160,221},//梅红色
|
||||
{188,143,143},//玫瑰红色
|
||||
};
|
||||
int size = 1;
|
||||
int lineIdx = 0;
|
||||
for (int line = 0; line < lineNum; line++)
|
||||
{
|
||||
int linePtNum = (int)scanLines[line].size();
|
||||
if (linePtNum == 0)
|
||||
continue;
|
||||
|
||||
sw << "Line_" << lineIdx << "_0_" << linePtNum << std::endl;
|
||||
lineIdx++;
|
||||
for (int i = 0; i < linePtNum; i++)
|
||||
{
|
||||
SVzNL3DPosition* pt3D = &scanLines[line][i];
|
||||
if (pt3D->nPointIdx > 0)
|
||||
{
|
||||
rgb = objColor[pt3D->nPointIdx];
|
||||
size = 3;
|
||||
}
|
||||
else //if (pt3D->nPointIdx == 0)
|
||||
{
|
||||
rgb = { 200, 200, 200 };
|
||||
size = 1;
|
||||
}
|
||||
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}-";
|
||||
sw << "{" << rgb.r << "," << rgb.g << "," << rgb.b << "," << size << " }" << std::endl;
|
||||
}
|
||||
}
|
||||
sw.close();
|
||||
}
|
||||
|
||||
#define TEST_GET_ROI_DATA 0
|
||||
#define TEST_COMPUTE_SPACE 1
|
||||
#define TEST_GROUP 1
|
||||
int main()
|
||||
{
|
||||
const char* dataPath[TEST_GROUP] = {
|
||||
"F:/ShangGu/项目/水木宏创/槽道间距测量/模拟数据/", //0
|
||||
};
|
||||
|
||||
SVzNLRange fileIdx[TEST_GROUP] = {
|
||||
{1,9},
|
||||
};
|
||||
|
||||
const char* ver = wd_ChannelSPaceMeasureVersion();
|
||||
printf("ver:%s\n", ver);
|
||||
|
||||
#if TEST_GET_ROI_DATA
|
||||
|
||||
|
||||
for (int grp = 0; grp < TEST_GROUP; grp++)
|
||||
{
|
||||
SVzNLRangeD roi_y[10] = {
|
||||
{0, 0},
|
||||
{-405.0, 510.0}, {-302.0,640.0}, {-290.0, 640.0},
|
||||
{-290.0, 670.0}, {-290.0, 640.0}, {-305.0, 660.0},
|
||||
{-296.0, 510.0}, {-300.0, 500.0}, {-330.0, 510.0}
|
||||
};
|
||||
for (int fidx = 1; fidx <= 9; fidx++)
|
||||
{
|
||||
char _scan_file[256];
|
||||
sprintf_s(_scan_file, "%sLaserData_%d.txt", dataPath[grp], fidx);
|
||||
std::vector<std::vector< SVzNL3DPosition>> scanData;
|
||||
wdReadLaserScanPointFromFile_XYZ_vector(_scan_file, scanData);
|
||||
std::vector<std::vector< SVzNL3DPosition>> scanROIData;
|
||||
wd_gridScan_GetROIData(scanData, roi_y[fidx], scanROIData);
|
||||
sprintf_s(_scan_file, "%sLaserData_%d_obj.txt", dataPath[grp], fidx);
|
||||
_outputScanDataFile(_scan_file, scanROIData, 0, 0, 0);
|
||||
}
|
||||
}
|
||||
#endif
|
||||
|
||||
#if TEST_COMPUTE_SPACE
|
||||
for (int grp = 0; grp < TEST_GROUP; grp++)
|
||||
{
|
||||
for (int fidx = fileIdx[grp].nMin; fidx <= fileIdx[grp].nMax; fidx++)
|
||||
{
|
||||
//fidx =7;
|
||||
char _scan_file[256];
|
||||
sprintf_s(_scan_file, "%sLaserData_%d_obj.txt", dataPath[grp], fidx);
|
||||
|
||||
std::vector<std::vector< SVzNL3DPosition>> scanLines;
|
||||
wdReadLaserScanPointFromFile_XYZ_vector(_scan_file, scanLines);
|
||||
|
||||
//转成plyTxt格式
|
||||
//sprintf_s(_scan_file, "%s%d_ply_Hi229229.txt", dataPath[grp], fidx);
|
||||
//wdSavePlyTxt(_scan_file, scanLines);
|
||||
|
||||
long t1 = (long)GetTickCount64();//统计时间
|
||||
|
||||
SSG_cornerParam cornerParam;
|
||||
cornerParam.cornerTh = 45; //45度角
|
||||
cornerParam.scale = 15; // algoParam.bagParam.bagH / 8; // 15; // algoParam.bagParam.bagH / 8;
|
||||
cornerParam.minEndingGap = 20; // algoParam.bagParam.bagW / 4;
|
||||
cornerParam.minEndingGap_z = 50;
|
||||
cornerParam.jumpCornerTh_1 = 15; //水平角度，小于此角度视为水平
|
||||
cornerParam.jumpCornerTh_2 = 60;
|
||||
|
||||
SSG_outlierFilterParam filterParam;
|
||||
filterParam.continuityTh = 20.0; //噪声滤除。当相邻点的z跳变大于此门限时，检查是否为噪声。若长度小于outlierLen， 视为噪声
|
||||
filterParam.outlierTh = 5;
|
||||
|
||||
SSG_treeGrowParam growParam;
|
||||
growParam.maxLineSkipNum = 10;
|
||||
growParam.yDeviation_max = 20.0;
|
||||
growParam.maxSkipDistance = 20.0;
|
||||
growParam.zDeviation_max = 50.0;//
|
||||
growParam.minLTypeTreeLen = 100; //mm
|
||||
growParam.minVTypeTreeLen = 100; //mm
|
||||
|
||||
SSX_channelParam channelParam;
|
||||
channelParam.channleSpaceRng = { 300, 800 };
|
||||
channelParam.channelWidthRng = { 5,30 };
|
||||
|
||||
bool isHorizonScan = true; //true:激光线平行槽道；false:激光线垂直槽道
|
||||
int errCode = 0;
|
||||
SSX_channelInfo channelInfo = sx_channelSpaceMeasure(
|
||||
scanLines,
|
||||
isHorizonScan, //true:激光线平行槽道；false:激光线垂直槽道
|
||||
cornerParam,
|
||||
filterParam,
|
||||
growParam,
|
||||
channelParam,
|
||||
&errCode);
|
||||
|
||||
long t2 = (long)GetTickCount64();
|
||||
printf("%s: %d(ms)!\n", _scan_file, (int)(t2 - t1));
|
||||
//输出测试结果
|
||||
sprintf_s(_scan_file, "%sresult\\%d_result.txt", dataPath[grp], fidx);
|
||||
_outputRGBDScan_RGBD(_scan_file, scanLines);
|
||||
sprintf_s(_scan_file, "%sresult\\%d_fillingPort_info.txt", dataPath[grp], fidx);
|
||||
_outputChanneltInfo(_scan_file, channelInfo);
|
||||
}
|
||||
}
|
||||
#endif
|
||||
}
|
||||
|
||||
// 运行程序: Ctrl + F5 或调试 >“开始执行(不调试)”菜单
|
||||
// 调试程序: F5 或调试 >“开始调试”菜单
|
||||
|
||||
// 入门使用技巧:
|
||||
// 1. 使用解决方案资源管理器窗口添加/管理文件
|
||||
// 2. 使用团队资源管理器窗口连接到源代码管理
|
||||
// 3. 使用输出窗口查看生成输出和其他消息
|
||||
// 4. 使用错误列表窗口查看错误
|
||||
// 5. 转到“项目”>“添加新项”以创建新的代码文件，或转到“项目”>“添加现有项”以将现有代码文件添加到项目
|
||||
// 6. 将来，若要再次打开此项目，请转到“文件”>“打开”>“项目”并选择 .sln 文件
|
||||
158
HC_chanelSpaceMeasure_test/HC_chanelSpaceMeasure_test.vcxproj
Normal file
158
HC_chanelSpaceMeasure_test/HC_chanelSpaceMeasure_test.vcxproj
Normal file
@ -0,0 +1,158 @@
|
||||
<?xml version="1.0" encoding="utf-8"?>
|
||||
<Project DefaultTargets="Build" xmlns="http://schemas.microsoft.com/developer/msbuild/2003">
|
||||
<ItemGroup Label="ProjectConfigurations">
|
||||
<ProjectConfiguration Include="Debug|Win32">
|
||||
<Configuration>Debug</Configuration>
|
||||
<Platform>Win32</Platform>
|
||||
</ProjectConfiguration>
|
||||
<ProjectConfiguration Include="Release|Win32">
|
||||
<Configuration>Release</Configuration>
|
||||
<Platform>Win32</Platform>
|
||||
</ProjectConfiguration>
|
||||
<ProjectConfiguration Include="Debug|x64">
|
||||
<Configuration>Debug</Configuration>
|
||||
<Platform>x64</Platform>
|
||||
</ProjectConfiguration>
|
||||
<ProjectConfiguration Include="Release|x64">
|
||||
<Configuration>Release</Configuration>
|
||||
<Platform>x64</Platform>
|
||||
</ProjectConfiguration>
|
||||
</ItemGroup>
|
||||
<PropertyGroup Label="Globals">
|
||||
<VCProjectVersion>16.0</VCProjectVersion>
|
||||
<Keyword>Win32Proj</Keyword>
|
||||
<ProjectGuid>{3ec47d19-2562-4303-82b9-6b1c93c5a37a}</ProjectGuid>
|
||||
<RootNamespace>HCchanelSpaceMeasuretest</RootNamespace>
|
||||
<WindowsTargetPlatformVersion>10.0</WindowsTargetPlatformVersion>
|
||||
<ProjectName>HC_channelSpaceMeasure_test</ProjectName>
|
||||
</PropertyGroup>
|
||||
<Import Project="$(VCTargetsPath)\Microsoft.Cpp.Default.props" />
|
||||
<PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Debug|Win32'" Label="Configuration">
|
||||
<ConfigurationType>Application</ConfigurationType>
|
||||
<UseDebugLibraries>true</UseDebugLibraries>
|
||||
<PlatformToolset>v142</PlatformToolset>
|
||||
<CharacterSet>Unicode</CharacterSet>
|
||||
</PropertyGroup>
|
||||
<PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Release|Win32'" Label="Configuration">
|
||||
<ConfigurationType>Application</ConfigurationType>
|
||||
<UseDebugLibraries>false</UseDebugLibraries>
|
||||
<PlatformToolset>v142</PlatformToolset>
|
||||
<WholeProgramOptimization>true</WholeProgramOptimization>
|
||||
<CharacterSet>Unicode</CharacterSet>
|
||||
</PropertyGroup>
|
||||
<PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Debug|x64'" Label="Configuration">
|
||||
<ConfigurationType>Application</ConfigurationType>
|
||||
<UseDebugLibraries>true</UseDebugLibraries>
|
||||
<PlatformToolset>v142</PlatformToolset>
|
||||
<CharacterSet>Unicode</CharacterSet>
|
||||
</PropertyGroup>
|
||||
<PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Release|x64'" Label="Configuration">
|
||||
<ConfigurationType>Application</ConfigurationType>
|
||||
<UseDebugLibraries>false</UseDebugLibraries>
|
||||
<PlatformToolset>v142</PlatformToolset>
|
||||
<WholeProgramOptimization>true</WholeProgramOptimization>
|
||||
<CharacterSet>Unicode</CharacterSet>
|
||||
</PropertyGroup>
|
||||
<Import Project="$(VCTargetsPath)\Microsoft.Cpp.props" />
|
||||
<ImportGroup Label="ExtensionSettings">
|
||||
</ImportGroup>
|
||||
<ImportGroup Label="Shared">
|
||||
</ImportGroup>
|
||||
<ImportGroup Label="PropertySheets" Condition="'$(Configuration)|$(Platform)'=='Debug|Win32'">
|
||||
<Import Project="$(UserRootDir)\Microsoft.Cpp.$(Platform).user.props" Condition="exists('$(UserRootDir)\Microsoft.Cpp.$(Platform).user.props')" Label="LocalAppDataPlatform" />
|
||||
</ImportGroup>
|
||||
<ImportGroup Label="PropertySheets" Condition="'$(Configuration)|$(Platform)'=='Release|Win32'">
|
||||
<Import Project="$(UserRootDir)\Microsoft.Cpp.$(Platform).user.props" Condition="exists('$(UserRootDir)\Microsoft.Cpp.$(Platform).user.props')" Label="LocalAppDataPlatform" />
|
||||
</ImportGroup>
|
||||
<ImportGroup Label="PropertySheets" Condition="'$(Configuration)|$(Platform)'=='Debug|x64'">
|
||||
<Import Project="$(UserRootDir)\Microsoft.Cpp.$(Platform).user.props" Condition="exists('$(UserRootDir)\Microsoft.Cpp.$(Platform).user.props')" Label="LocalAppDataPlatform" />
|
||||
</ImportGroup>
|
||||
<ImportGroup Label="PropertySheets" Condition="'$(Configuration)|$(Platform)'=='Release|x64'">
|
||||
<Import Project="$(UserRootDir)\Microsoft.Cpp.$(Platform).user.props" Condition="exists('$(UserRootDir)\Microsoft.Cpp.$(Platform).user.props')" Label="LocalAppDataPlatform" />
|
||||
</ImportGroup>
|
||||
<PropertyGroup Label="UserMacros" />
|
||||
<PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Debug|Win32'">
|
||||
<LinkIncremental>true</LinkIncremental>
|
||||
</PropertyGroup>
|
||||
<PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Release|Win32'">
|
||||
<LinkIncremental>false</LinkIncremental>
|
||||
</PropertyGroup>
|
||||
<PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Debug|x64'">
|
||||
<LinkIncremental>true</LinkIncremental>
|
||||
<OutDir>$(SolutionDir)build\$(Platform)\$(Configuration)\</OutDir>
|
||||
<IncludePath>..\..\thirdParty\VzNLSDK\Inc;..\sourceCode;..\sourceCode\inc;$(IncludePath)</IncludePath>
|
||||
</PropertyGroup>
|
||||
<PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Release|x64'">
|
||||
<LinkIncremental>false</LinkIncremental>
|
||||
<OutDir>$(SolutionDir)build\$(Platform)\$(Configuration)\</OutDir>
|
||||
<IncludePath>..\..\thirdParty\VzNLSDK\Inc;..\sourceCode;..\sourceCode\inc;$(IncludePath)</IncludePath>
|
||||
</PropertyGroup>
|
||||
<ItemDefinitionGroup Condition="'$(Configuration)|$(Platform)'=='Debug|Win32'">
|
||||
<ClCompile>
|
||||
<WarningLevel>Level3</WarningLevel>
|
||||
<SDLCheck>true</SDLCheck>
|
||||
<PreprocessorDefinitions>WIN32;_DEBUG;_CONSOLE;%(PreprocessorDefinitions)</PreprocessorDefinitions>
|
||||
<ConformanceMode>true</ConformanceMode>
|
||||
</ClCompile>
|
||||
<Link>
|
||||
<SubSystem>Console</SubSystem>
|
||||
<GenerateDebugInformation>true</GenerateDebugInformation>
|
||||
</Link>
|
||||
</ItemDefinitionGroup>
|
||||
<ItemDefinitionGroup Condition="'$(Configuration)|$(Platform)'=='Release|Win32'">
|
||||
<ClCompile>
|
||||
<WarningLevel>Level3</WarningLevel>
|
||||
<FunctionLevelLinking>true</FunctionLevelLinking>
|
||||
<IntrinsicFunctions>true</IntrinsicFunctions>
|
||||
<SDLCheck>true</SDLCheck>
|
||||
<PreprocessorDefinitions>WIN32;NDEBUG;_CONSOLE;%(PreprocessorDefinitions)</PreprocessorDefinitions>
|
||||
<ConformanceMode>true</ConformanceMode>
|
||||
</ClCompile>
|
||||
<Link>
|
||||
<SubSystem>Console</SubSystem>
|
||||
<EnableCOMDATFolding>true</EnableCOMDATFolding>
|
||||
<OptimizeReferences>true</OptimizeReferences>
|
||||
<GenerateDebugInformation>true</GenerateDebugInformation>
|
||||
</Link>
|
||||
</ItemDefinitionGroup>
|
||||
<ItemDefinitionGroup Condition="'$(Configuration)|$(Platform)'=='Debug|x64'">
|
||||
<ClCompile>
|
||||
<WarningLevel>Level3</WarningLevel>
|
||||
<SDLCheck>true</SDLCheck>
|
||||
<PreprocessorDefinitions>_CRT_SECURE_NO_WARNINGS;_DEBUG;_CONSOLE;%(PreprocessorDefinitions)</PreprocessorDefinitions>
|
||||
<ConformanceMode>true</ConformanceMode>
|
||||
<AdditionalIncludeDirectories>..\..\thirdParty\opencv320\build\include;</AdditionalIncludeDirectories>
|
||||
</ClCompile>
|
||||
<Link>
|
||||
<SubSystem>Console</SubSystem>
|
||||
<GenerateDebugInformation>true</GenerateDebugInformation>
|
||||
<AdditionalLibraryDirectories>..\..\thirdParty\opencv320\build\x64\vc14\lib;..\build\x64\Debug;%(AdditionalLibraryDirectories)</AdditionalLibraryDirectories>
|
||||
<AdditionalDependencies>opencv_world320d.lib;HC_channelSpaceMeasure.lib;%(AdditionalDependencies)</AdditionalDependencies>
|
||||
</Link>
|
||||
</ItemDefinitionGroup>
|
||||
<ItemDefinitionGroup Condition="'$(Configuration)|$(Platform)'=='Release|x64'">
|
||||
<ClCompile>
|
||||
<WarningLevel>Level3</WarningLevel>
|
||||
<FunctionLevelLinking>true</FunctionLevelLinking>
|
||||
<IntrinsicFunctions>true</IntrinsicFunctions>
|
||||
<SDLCheck>true</SDLCheck>
|
||||
<PreprocessorDefinitions>_CRT_SECURE_NO_WARNINGS;NDEBUG;_CONSOLE;%(PreprocessorDefinitions)</PreprocessorDefinitions>
|
||||
<ConformanceMode>true</ConformanceMode>
|
||||
<AdditionalIncludeDirectories>..\..\thirdParty\opencv320\build\include;</AdditionalIncludeDirectories>
|
||||
</ClCompile>
|
||||
<Link>
|
||||
<SubSystem>Console</SubSystem>
|
||||
<EnableCOMDATFolding>true</EnableCOMDATFolding>
|
||||
<OptimizeReferences>true</OptimizeReferences>
|
||||
<GenerateDebugInformation>true</GenerateDebugInformation>
|
||||
<AdditionalLibraryDirectories>..\..\thirdParty\opencv320\build\x64\vc14\lib;..\build\x64\Release;%(AdditionalLibraryDirectories)</AdditionalLibraryDirectories>
|
||||
<AdditionalDependencies>opencv_world320.lib;HC_channelSpaceMeasure.lib;%(AdditionalDependencies)</AdditionalDependencies>
|
||||
</Link>
|
||||
</ItemDefinitionGroup>
|
||||
<ItemGroup>
|
||||
<ClCompile Include="HC_chanelSpaceMeasure_test.cpp" />
|
||||
</ItemGroup>
|
||||
<Import Project="$(VCTargetsPath)\Microsoft.Cpp.targets" />
|
||||
<ImportGroup Label="ExtensionTargets">
|
||||
</ImportGroup>
|
||||
</Project>
|
||||
173
HC_channelSpaceMeausre/HC_channelSpaceMeausre.vcxproj
Normal file
173
HC_channelSpaceMeausre/HC_channelSpaceMeausre.vcxproj
Normal file
@ -0,0 +1,173 @@
|
||||
<?xml version="1.0" encoding="utf-8"?>
|
||||
<Project DefaultTargets="Build" xmlns="http://schemas.microsoft.com/developer/msbuild/2003">
|
||||
<ItemGroup Label="ProjectConfigurations">
|
||||
<ProjectConfiguration Include="Debug|Win32">
|
||||
<Configuration>Debug</Configuration>
|
||||
<Platform>Win32</Platform>
|
||||
</ProjectConfiguration>
|
||||
<ProjectConfiguration Include="Release|Win32">
|
||||
<Configuration>Release</Configuration>
|
||||
<Platform>Win32</Platform>
|
||||
</ProjectConfiguration>
|
||||
<ProjectConfiguration Include="Debug|x64">
|
||||
<Configuration>Debug</Configuration>
|
||||
<Platform>x64</Platform>
|
||||
</ProjectConfiguration>
|
||||
<ProjectConfiguration Include="Release|x64">
|
||||
<Configuration>Release</Configuration>
|
||||
<Platform>x64</Platform>
|
||||
</ProjectConfiguration>
|
||||
</ItemGroup>
|
||||
<ItemGroup>
|
||||
<ClCompile Include="..\sourceCode\channelSpaceMeasure.cpp" />
|
||||
</ItemGroup>
|
||||
<ItemGroup>
|
||||
<ClInclude Include="..\sourceCode\channelSpaceMeasure_Export.h" />
|
||||
</ItemGroup>
|
||||
<PropertyGroup Label="Globals">
|
||||
<VCProjectVersion>16.0</VCProjectVersion>
|
||||
<Keyword>Win32Proj</Keyword>
|
||||
<ProjectGuid>{52a65444-8505-4fd5-9501-e2d297e2eb2c}</ProjectGuid>
|
||||
<RootNamespace>HCchannelSpaceMeausre</RootNamespace>
|
||||
<WindowsTargetPlatformVersion>10.0</WindowsTargetPlatformVersion>
|
||||
<ProjectName>HC_channelSpaceMeasure</ProjectName>
|
||||
</PropertyGroup>
|
||||
<Import Project="$(VCTargetsPath)\Microsoft.Cpp.Default.props" />
|
||||
<PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Debug|Win32'" Label="Configuration">
|
||||
<ConfigurationType>DynamicLibrary</ConfigurationType>
|
||||
<UseDebugLibraries>true</UseDebugLibraries>
|
||||
<PlatformToolset>v142</PlatformToolset>
|
||||
<CharacterSet>Unicode</CharacterSet>
|
||||
</PropertyGroup>
|
||||
<PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Release|Win32'" Label="Configuration">
|
||||
<ConfigurationType>DynamicLibrary</ConfigurationType>
|
||||
<UseDebugLibraries>false</UseDebugLibraries>
|
||||
<PlatformToolset>v142</PlatformToolset>
|
||||
<WholeProgramOptimization>true</WholeProgramOptimization>
|
||||
<CharacterSet>Unicode</CharacterSet>
|
||||
</PropertyGroup>
|
||||
<PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Debug|x64'" Label="Configuration">
|
||||
<ConfigurationType>DynamicLibrary</ConfigurationType>
|
||||
<UseDebugLibraries>true</UseDebugLibraries>
|
||||
<PlatformToolset>v142</PlatformToolset>
|
||||
<CharacterSet>Unicode</CharacterSet>
|
||||
</PropertyGroup>
|
||||
<PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Release|x64'" Label="Configuration">
|
||||
<ConfigurationType>DynamicLibrary</ConfigurationType>
|
||||
<UseDebugLibraries>false</UseDebugLibraries>
|
||||
<PlatformToolset>v142</PlatformToolset>
|
||||
<WholeProgramOptimization>true</WholeProgramOptimization>
|
||||
<CharacterSet>Unicode</CharacterSet>
|
||||
</PropertyGroup>
|
||||
<Import Project="$(VCTargetsPath)\Microsoft.Cpp.props" />
|
||||
<ImportGroup Label="ExtensionSettings">
|
||||
</ImportGroup>
|
||||
<ImportGroup Label="Shared">
|
||||
</ImportGroup>
|
||||
<ImportGroup Label="PropertySheets" Condition="'$(Configuration)|$(Platform)'=='Debug|Win32'">
|
||||
<Import Project="$(UserRootDir)\Microsoft.Cpp.$(Platform).user.props" Condition="exists('$(UserRootDir)\Microsoft.Cpp.$(Platform).user.props')" Label="LocalAppDataPlatform" />
|
||||
</ImportGroup>
|
||||
<ImportGroup Label="PropertySheets" Condition="'$(Configuration)|$(Platform)'=='Release|Win32'">
|
||||
<Import Project="$(UserRootDir)\Microsoft.Cpp.$(Platform).user.props" Condition="exists('$(UserRootDir)\Microsoft.Cpp.$(Platform).user.props')" Label="LocalAppDataPlatform" />
|
||||
</ImportGroup>
|
||||
<ImportGroup Label="PropertySheets" Condition="'$(Configuration)|$(Platform)'=='Debug|x64'">
|
||||
<Import Project="$(UserRootDir)\Microsoft.Cpp.$(Platform).user.props" Condition="exists('$(UserRootDir)\Microsoft.Cpp.$(Platform).user.props')" Label="LocalAppDataPlatform" />
|
||||
</ImportGroup>
|
||||
<ImportGroup Label="PropertySheets" Condition="'$(Configuration)|$(Platform)'=='Release|x64'">
|
||||
<Import Project="$(UserRootDir)\Microsoft.Cpp.$(Platform).user.props" Condition="exists('$(UserRootDir)\Microsoft.Cpp.$(Platform).user.props')" Label="LocalAppDataPlatform" />
|
||||
</ImportGroup>
|
||||
<PropertyGroup Label="UserMacros" />
|
||||
<PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Debug|Win32'">
|
||||
<LinkIncremental>true</LinkIncremental>
|
||||
</PropertyGroup>
|
||||
<PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Release|Win32'">
|
||||
<LinkIncremental>false</LinkIncremental>
|
||||
</PropertyGroup>
|
||||
<PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Debug|x64'">
|
||||
<LinkIncremental>true</LinkIncremental>
|
||||
<OutDir>$(SolutionDir)build\$(Platform)\$(Configuration)\</OutDir>
|
||||
<IncludePath>..\..\thirdParty\VzNLSDK\Inc;..\..\thirdParty\opencv320\build\include;..\sourceCode;..\sourceCode\inc;$(IncludePath)</IncludePath>
|
||||
</PropertyGroup>
|
||||
<PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Release|x64'">
|
||||
<LinkIncremental>false</LinkIncremental>
|
||||
<OutDir>$(SolutionDir)build\$(Platform)\$(Configuration)\</OutDir>
|
||||
<IncludePath>..\..\thirdParty\VzNLSDK\Inc;..\..\thirdParty\opencv320\build\include;..\sourceCode;..\sourceCode\inc;$(IncludePath)</IncludePath>
|
||||
</PropertyGroup>
|
||||
<ItemDefinitionGroup Condition="'$(Configuration)|$(Platform)'=='Debug|Win32'">
|
||||
<ClCompile>
|
||||
<WarningLevel>Level3</WarningLevel>
|
||||
<SDLCheck>true</SDLCheck>
|
||||
<PreprocessorDefinitions>WIN32;_DEBUG;HCCHANNELSPACEMEAUSRE_EXPORTS;_WINDOWS;_USRDLL;%(PreprocessorDefinitions)</PreprocessorDefinitions>
|
||||
<ConformanceMode>true</ConformanceMode>
|
||||
<PrecompiledHeader>Use</PrecompiledHeader>
|
||||
<PrecompiledHeaderFile>pch.h</PrecompiledHeaderFile>
|
||||
</ClCompile>
|
||||
<Link>
|
||||
<SubSystem>Windows</SubSystem>
|
||||
<GenerateDebugInformation>true</GenerateDebugInformation>
|
||||
<EnableUAC>false</EnableUAC>
|
||||
</Link>
|
||||
</ItemDefinitionGroup>
|
||||
<ItemDefinitionGroup Condition="'$(Configuration)|$(Platform)'=='Release|Win32'">
|
||||
<ClCompile>
|
||||
<WarningLevel>Level3</WarningLevel>
|
||||
<FunctionLevelLinking>true</FunctionLevelLinking>
|
||||
<IntrinsicFunctions>true</IntrinsicFunctions>
|
||||
<SDLCheck>true</SDLCheck>
|
||||
<PreprocessorDefinitions>WIN32;NDEBUG;HCCHANNELSPACEMEAUSRE_EXPORTS;_WINDOWS;_USRDLL;%(PreprocessorDefinitions)</PreprocessorDefinitions>
|
||||
<ConformanceMode>true</ConformanceMode>
|
||||
<PrecompiledHeader>Use</PrecompiledHeader>
|
||||
<PrecompiledHeaderFile>pch.h</PrecompiledHeaderFile>
|
||||
</ClCompile>
|
||||
<Link>
|
||||
<SubSystem>Windows</SubSystem>
|
||||
<EnableCOMDATFolding>true</EnableCOMDATFolding>
|
||||
<OptimizeReferences>true</OptimizeReferences>
|
||||
<GenerateDebugInformation>true</GenerateDebugInformation>
|
||||
<EnableUAC>false</EnableUAC>
|
||||
</Link>
|
||||
</ItemDefinitionGroup>
|
||||
<ItemDefinitionGroup Condition="'$(Configuration)|$(Platform)'=='Debug|x64'">
|
||||
<ClCompile>
|
||||
<WarningLevel>Level3</WarningLevel>
|
||||
<SDLCheck>true</SDLCheck>
|
||||
<PreprocessorDefinitions>_CRT_SECURE_NO_WARNINGS;_DEBUG;HCCHANNELSPACEMEAUSRE_EXPORTS;_WINDOWS;_USRDLL;%(PreprocessorDefinitions)</PreprocessorDefinitions>
|
||||
<ConformanceMode>true</ConformanceMode>
|
||||
<PrecompiledHeader>NotUsing</PrecompiledHeader>
|
||||
<PrecompiledHeaderFile>pch.h</PrecompiledHeaderFile>
|
||||
<AdditionalIncludeDirectories>..\..\thirdParty\opencv320\build\include;%(AdditionalIncludeDirectories)</AdditionalIncludeDirectories>
|
||||
</ClCompile>
|
||||
<Link>
|
||||
<SubSystem>Windows</SubSystem>
|
||||
<GenerateDebugInformation>true</GenerateDebugInformation>
|
||||
<EnableUAC>false</EnableUAC>
|
||||
<AdditionalLibraryDirectories>..\..\thirdParty\opencv320\build\x64\vc14\lib;..\build\x64\Debug;%(AdditionalLibraryDirectories)</AdditionalLibraryDirectories>
|
||||
<AdditionalDependencies>opencv_world320d.lib;baseAlgorithm.lib;%(AdditionalDependencies)</AdditionalDependencies>
|
||||
</Link>
|
||||
</ItemDefinitionGroup>
|
||||
<ItemDefinitionGroup Condition="'$(Configuration)|$(Platform)'=='Release|x64'">
|
||||
<ClCompile>
|
||||
<WarningLevel>Level3</WarningLevel>
|
||||
<FunctionLevelLinking>true</FunctionLevelLinking>
|
||||
<IntrinsicFunctions>true</IntrinsicFunctions>
|
||||
<SDLCheck>true</SDLCheck>
|
||||
<PreprocessorDefinitions>_CRT_SECURE_NO_WARNINGS;NDEBUG;HCCHANNELSPACEMEAUSRE_EXPORTS;_WINDOWS;_USRDLL;%(PreprocessorDefinitions)</PreprocessorDefinitions>
|
||||
<ConformanceMode>true</ConformanceMode>
|
||||
<PrecompiledHeader>NotUsing</PrecompiledHeader>
|
||||
<PrecompiledHeaderFile>pch.h</PrecompiledHeaderFile>
|
||||
<AdditionalIncludeDirectories>..\..\thirdParty\opencv320\build\include;%(AdditionalIncludeDirectories)</AdditionalIncludeDirectories>
|
||||
</ClCompile>
|
||||
<Link>
|
||||
<SubSystem>Windows</SubSystem>
|
||||
<EnableCOMDATFolding>true</EnableCOMDATFolding>
|
||||
<OptimizeReferences>true</OptimizeReferences>
|
||||
<GenerateDebugInformation>true</GenerateDebugInformation>
|
||||
<EnableUAC>false</EnableUAC>
|
||||
<AdditionalLibraryDirectories>..\..\thirdParty\opencv320\build\x64\vc14\lib;..\build\x64\Release;%(AdditionalLibraryDirectories)</AdditionalLibraryDirectories>
|
||||
<AdditionalDependencies>opencv_world320.lib;baseAlgorithm.lib;%(AdditionalDependencies)</AdditionalDependencies>
|
||||
</Link>
|
||||
</ItemDefinitionGroup>
|
||||
<Import Project="$(VCTargetsPath)\Microsoft.Cpp.targets" />
|
||||
<ImportGroup Label="ExtensionTargets">
|
||||
</ImportGroup>
|
||||
</Project>
|
||||
@ -156,6 +156,10 @@ Project("{8BC9CEB8-8B4A-11D0-8D11-00A0C91BC942}") = "BQ_assemblyPosition", "BQ_a
|
||||
EndProjectSection
|
||||
EndProject
|
||||
Project("{8BC9CEB8-8B4A-11D0-8D11-00A0C91BC942}") = "BQ_assemblyPosition_test", "BQ_assemblyPosition_test\BQ_assemblyPosition_test.vcxproj", "{BC38D1E5-10CB-438B-AC72-6012303CE139}"
|
||||
ProjectSection(ProjectDependencies) = postProject
|
||||
{95DC3F1A-902A-490E-BD3B-B10463CF0EBD} = {95DC3F1A-902A-490E-BD3B-B10463CF0EBD}
|
||||
{0E62EEE4-ABB5-4364-A794-CCFFE8815426} = {0E62EEE4-ABB5-4364-A794-CCFFE8815426}
|
||||
EndProjectSection
|
||||
EndProject
|
||||
Project("{8BC9CEB8-8B4A-11D0-8D11-00A0C91BC942}") = "wheelArchHeigthMeasure", "wheelArchHeigthMeaure\wheelArchHeigthMeaure.vcxproj", "{CFE11556-106A-4216-BF62-FDA980528F7A}"
|
||||
ProjectSection(ProjectDependencies) = postProject
|
||||
@ -169,8 +173,15 @@ Project("{8BC9CEB8-8B4A-11D0-8D11-00A0C91BC942}") = "wheelArchHeigthMeasure_test
|
||||
EndProjectSection
|
||||
EndProject
|
||||
Project("{8BC9CEB8-8B4A-11D0-8D11-00A0C91BC942}") = "HC_channelSpaceMeasure", "HC_channelSpaceMeausre\HC_channelSpaceMeausre.vcxproj", "{52A65444-8505-4FD5-9501-E2D297E2EB2C}"
|
||||
ProjectSection(ProjectDependencies) = postProject
|
||||
{95DC3F1A-902A-490E-BD3B-B10463CF0EBD} = {95DC3F1A-902A-490E-BD3B-B10463CF0EBD}
|
||||
EndProjectSection
|
||||
EndProject
|
||||
Project("{8BC9CEB8-8B4A-11D0-8D11-00A0C91BC942}") = "HC_channelSpaceMeasure_test", "HC_chanelSpaceMeasure_test\HC_chanelSpaceMeasure_test.vcxproj", "{3EC47D19-2562-4303-82B9-6B1C93C5A37A}"
|
||||
ProjectSection(ProjectDependencies) = postProject
|
||||
{95DC3F1A-902A-490E-BD3B-B10463CF0EBD} = {95DC3F1A-902A-490E-BD3B-B10463CF0EBD}
|
||||
{52A65444-8505-4FD5-9501-E2D297E2EB2C} = {52A65444-8505-4FD5-9501-E2D297E2EB2C}
|
||||
EndProjectSection
|
||||
EndProject
|
||||
Global
|
||||
GlobalSection(SolutionConfigurationPlatforms) = preSolution
|
||||
|
||||
@ -299,185 +299,54 @@ void _outputScanDataFile_self(char* fileName, SVzNL3DLaserLine* scanData, int li
|
||||
sw.close();
|
||||
}
|
||||
|
||||
SVzNL3DLaserLine* vzReadLaserScanPointFromFile_XYZ(const char* fileName, int* scanLineNum, float* scanV,
|
||||
int* dataCalib, int* scanMaxStamp, int* canClockUnit)
|
||||
void vzReadLaserScanPointFromFile_XYZ_vector(const char* fileName, std::vector<std::vector< SVzNL3DPosition>>& scanData)
|
||||
{
|
||||
std::ifstream inputFile(fileName);
|
||||
std::string linedata;
|
||||
|
||||
if (inputFile.is_open() == false)
|
||||
return NULL;
|
||||
return;
|
||||
|
||||
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;
|
||||
std::vector< SVzNL3DPosition> a_line;
|
||||
int ptIdx = 0;
|
||||
SVzNL3DPosition* p3DPoint = NULL;
|
||||
if (dataFileVer == DATA_VER_NEW)
|
||||
while (getline(inputFile, linedata))
|
||||
{
|
||||
while (getline(inputFile, linedata))
|
||||
if (0 == strncmp("Line_", linedata.c_str(), 5))
|
||||
{
|
||||
if (0 == strncmp("ScanSpeed:", linedata.c_str(), 10))
|
||||
int ptSize = (int)a_line.size();
|
||||
if (ptSize > 0)
|
||||
{
|
||||
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;
|
||||
}
|
||||
scanData.push_back(a_line);
|
||||
}
|
||||
a_line.clear();
|
||||
ptIdx = 0;
|
||||
}
|
||||
|
||||
}
|
||||
else if (dataFileVer == DATA_VER_OLD)
|
||||
{
|
||||
while (getline(inputFile, linedata))
|
||||
else if (0 == strncmp("{", linedata.c_str(), 1))
|
||||
{
|
||||
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;
|
||||
}
|
||||
}
|
||||
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);
|
||||
SVzNL3DPosition a_pt;
|
||||
a_pt.pt3D.x = X;
|
||||
a_pt.pt3D.y = Y;
|
||||
a_pt.pt3D.z = Z;
|
||||
a_pt.nPointIdx = ptIdx;
|
||||
ptIdx++;
|
||||
a_line.push_back(a_pt);
|
||||
}
|
||||
}
|
||||
//last line
|
||||
int ptSize = (int)a_line.size();
|
||||
if (ptSize > 0)
|
||||
{
|
||||
scanData.push_back(a_line);
|
||||
a_line.clear();
|
||||
}
|
||||
|
||||
inputFile.close();
|
||||
return _scanLines;
|
||||
return;
|
||||
}
|
||||
|
||||
typedef struct
|
||||
@ -486,14 +355,16 @@ typedef struct
|
||||
int g;
|
||||
int b;
|
||||
}SG_color;
|
||||
void _outputScanDataFile_RGBD_obj(char* fileName, SVzNL3DLaserLine* scanData, int lineNum,
|
||||
float lineV, int maxTimeStamp, int clockPerSecond, SSG_boxCarDimension* dimen)
|
||||
void _outputScanDataFile_RGBD_obj(
|
||||
char* fileName,
|
||||
std::vector<std::vector< SVzNL3DPosition>>& scanData,
|
||||
float lineV, int maxTimeStamp, int clockPerSecond,
|
||||
std::vector<SWD_motorStatorPosition>& resultObjPositions)
|
||||
{
|
||||
int lineNum = (int)scanData.size();
|
||||
std::ofstream sw(fileName);
|
||||
int realLines = lineNum;
|
||||
if (dimen)
|
||||
realLines += 1;
|
||||
|
||||
realLines++;
|
||||
sw << "LineNum:" << realLines << std::endl;
|
||||
sw << "DataType: 0" << std::endl;
|
||||
sw << "ScanSpeed:" << lineV << std::endl;
|
||||
@ -517,97 +388,145 @@ void _outputScanDataFile_RGBD_obj(char* fileName, SVzNL3DLaserLine* scanData, in
|
||||
};
|
||||
int size = 1;
|
||||
int nTimeStamp = 0;
|
||||
double alpha = 0.8;
|
||||
for (int line = 0; line < lineNum; line++)
|
||||
{
|
||||
sw << "Line_" << line << "_" << scanData[line].nTimeStamp << "_" << scanData[line].nPositionCnt << std::endl;
|
||||
nTimeStamp = scanData[line].nTimeStamp;
|
||||
for (int i = 0; i < scanData[line].nPositionCnt; i++)
|
||||
int nPositionCnt = (int)scanData[line].size();
|
||||
sw << "Line_" << line << "_0_" << nPositionCnt << std::endl;
|
||||
for (int i = 0; i < nPositionCnt; i++)
|
||||
{
|
||||
SVzNL3DPosition* pt3D = &scanData[line].p3DPosition[i];
|
||||
if (pt3D->pt3D.z < 1e-4)
|
||||
continue;
|
||||
|
||||
int vType = pt3D->nPointIdx & 0xff;
|
||||
int hType = vType >> 4;
|
||||
int objId = (pt3D->nPointIdx >> 8) & 0xff;
|
||||
vType = vType & 0x0f;
|
||||
if (LINE_FEATURE_RIGHT_ANGLE_HR == vType)
|
||||
SVzNL3DPosition& pt3D = scanData[line][i];
|
||||
int type = pt3D.nPointIdx;
|
||||
if (1 == type)
|
||||
{
|
||||
rgb = { 255, 97, 0 };
|
||||
rgb = { 0,255,0 };
|
||||
rgb.r = (int)((double)rgb.r * alpha);
|
||||
rgb.g = (int)((double)rgb.g * alpha);
|
||||
rgb.b = (int)((double)rgb.b * alpha);
|
||||
size = 2;
|
||||
}
|
||||
else if (2 == type)
|
||||
{
|
||||
rgb = { 0,0,255 };
|
||||
rgb.r = (int)((double)rgb.r * alpha);
|
||||
rgb.g = (int)((double)rgb.g * alpha);
|
||||
rgb.b = (int)((double)rgb.b * alpha);
|
||||
size = 2;
|
||||
}
|
||||
else if (3 == type) //轮眉
|
||||
{
|
||||
rgb = { 255, 0, 0 };
|
||||
size = 3;
|
||||
}
|
||||
else if (LINE_FEATURE_RIGHT_ANGLE_HF == vType)
|
||||
else if (4 == type) //
|
||||
{
|
||||
rgb = { 255, 255, 0 };
|
||||
size = 3;
|
||||
}
|
||||
else if (LINE_FEATURE_RIGHT_ANGLE_RH == vType)
|
||||
else if (5 == type) //
|
||||
{
|
||||
rgb = { 255, 0, 255 };
|
||||
size = 3;
|
||||
}
|
||||
else if (LINE_FEATURE_RIGHT_ANGLE_FH == vType)
|
||||
{
|
||||
rgb = { 160, 82, 45 };
|
||||
size = 3;
|
||||
}
|
||||
else if (LINE_FEATURE_RIGHT_ANGLE_HR == hType)
|
||||
{
|
||||
rgb = { 0, 0, 255 };
|
||||
size = 3;
|
||||
}
|
||||
else if (LINE_FEATURE_RIGHT_ANGLE_HF == hType)
|
||||
{
|
||||
rgb = { 0, 255, 255 };
|
||||
size = 3;
|
||||
}
|
||||
else if (LINE_FEATURE_RIGHT_ANGLE_RH == hType)
|
||||
{
|
||||
rgb = { 0, 255, 0 };
|
||||
size = 3;
|
||||
}
|
||||
else if (LINE_FEATURE_RIGHT_ANGLE_FH == hType)
|
||||
{
|
||||
rgb = { 85, 107, 47 };
|
||||
rgb = { 255, 255, 0 };
|
||||
size = 3;
|
||||
}
|
||||
else
|
||||
{
|
||||
rgb = { 200, 200, 200 };
|
||||
rgb = { 100, 100, 100 };
|
||||
size = 1;
|
||||
}
|
||||
|
||||
float x = (float)pt3D->pt3D.x;
|
||||
float y = (float)pt3D->pt3D.y;
|
||||
float z = (float)pt3D->pt3D.z;
|
||||
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}-";
|
||||
sw << "{" << rgb.r << "," << rgb.g << "," << rgb.b << "," << size << " }" << std::endl;
|
||||
}
|
||||
}
|
||||
if (dimen)
|
||||
#if 0
|
||||
//if (objOps.size() > 0)
|
||||
{
|
||||
int ptNum = 4;
|
||||
int ptNum = 3;
|
||||
sw << "Line_" << lineNum << "_" << (nTimeStamp + 1000) << "_" << ptNum << std::endl;
|
||||
for (int i = 0; i < 4; i++)
|
||||
{
|
||||
rgb = { 255, 0, 0 };
|
||||
size = 25;
|
||||
float x = (float)dimen->endings[i].x;
|
||||
float y = (float)dimen->endings[i].y;
|
||||
float z = (float)dimen->endings[i].z;
|
||||
|
||||
sw << "{" << x << "," << y << "," << z << "}-";
|
||||
sw << "{0,0}-{0,0}-";
|
||||
sw << "{" << rgb.r << "," << rgb.g << "," << rgb.b << "," << size << " }" << std::endl;
|
||||
if (i == 0)
|
||||
{
|
||||
sw << "{" << x << "," << y << "," << z << "}-";
|
||||
sw << "{0,0}-{0,0}-";
|
||||
sw << "{" << rgb.r << "," << rgb.g << "," << rgb.b << "," << size << " }" << std::endl;
|
||||
}
|
||||
}
|
||||
rgb = { 255, 0, 0 };
|
||||
|
||||
size = 10;
|
||||
float x = (float)wheelArcHeight.wheelArchPos.x;
|
||||
float y = (float)wheelArcHeight.wheelArchPos.y;
|
||||
float z = (float)wheelArcHeight.wheelArchPos.z;
|
||||
sw << "{" << x << "," << y << "," << z << "}-";
|
||||
sw << "{0,0}-{0,0}-";
|
||||
sw << "{" << rgb.r << "," << rgb.g << "," << rgb.b << "," << size << " }" << std::endl;
|
||||
|
||||
x = (float)wheelArcHeight.wheelUpPos.x;
|
||||
y = (float)wheelArcHeight.wheelUpPos.y;
|
||||
z = (float)wheelArcHeight.wheelUpPos.z;
|
||||
sw << "{" << x << "," << y << "," << z << "}-";
|
||||
sw << "{0,0}-{0,0}-";
|
||||
sw << "{" << rgb.r << "," << rgb.g << "," << rgb.b << "," << size << " }" << std::endl;
|
||||
|
||||
x = (float)wheelArcHeight.wheelDownPos.x;
|
||||
y = (float)wheelArcHeight.wheelDownPos.y;
|
||||
z = (float)wheelArcHeight.wheelDownPos.z;
|
||||
sw << "{" << x << "," << y << "," << z << "}-";
|
||||
sw << "{0,0}-{0,0}-";
|
||||
sw << "{" << rgb.r << "," << rgb.g << "," << rgb.b << "," << size << " }" << std::endl;
|
||||
|
||||
x = (float)wheelArcHeight.wheelArchPos.x;
|
||||
y = (float)wheelArcHeight.wheelArchPos.y;
|
||||
z = (float)wheelArcHeight.wheelArchPos.z;
|
||||
sw << "{" << x << "," << y << "," << z << "}-";
|
||||
sw << "{0,0}-{0,0}-";
|
||||
sw << "{" << rgb.r << "," << rgb.g << "," << rgb.b << "," << size << " }" << std::endl;
|
||||
}
|
||||
//画出方向线
|
||||
rgb = { 255, 0, 0 };
|
||||
size = 3;
|
||||
int lineIdx = 0;
|
||||
sw << "Poly_" << lineIdx << "_2" << std::endl;
|
||||
sw << "{" << (float)wheelArcHeight.arcLine[0].x << "," << (float)wheelArcHeight.arcLine[0].y << "," << (float)wheelArcHeight.arcLine[0].z << "}-";
|
||||
sw << "{0,0}-{0,0}-";
|
||||
sw << "{" << (int)rgb.r << "," << (int)rgb.g << "," << (int)rgb.b << "," << size << "}" << std::endl;
|
||||
sw << "{" << (float)wheelArcHeight.arcLine[1].x << "," << (float)wheelArcHeight.arcLine[1].y << "," << (float)wheelArcHeight.arcLine[1].z << "}-";
|
||||
sw << "{0,0}-{0,0}-";
|
||||
sw << "{" << (int)rgb.r << "," << (int)rgb.g << "," << (int)rgb.b << "," << size << "}" << std::endl;
|
||||
|
||||
lineIdx++;
|
||||
sw << "Poly_" << lineIdx << "_2" << std::endl;
|
||||
sw << "{" << (float)wheelArcHeight.upLine[0].x << "," << (float)wheelArcHeight.upLine[0].y << "," << (float)wheelArcHeight.upLine[0].z << "}-";
|
||||
sw << "{0,0}-{0,0}-";
|
||||
sw << "{" << (int)rgb.r << "," << (int)rgb.g << "," << (int)rgb.b << "," << size << "}" << std::endl;
|
||||
sw << "{" << (float)wheelArcHeight.upLine[1].x << "," << (float)wheelArcHeight.upLine[1].y << "," << (float)wheelArcHeight.upLine[1].z << "}-";
|
||||
sw << "{0,0}-{0,0}-";
|
||||
sw << "{" << (int)rgb.r << "," << (int)rgb.g << "," << (int)rgb.b << "," << size << "}" << std::endl;
|
||||
|
||||
lineIdx++;
|
||||
sw << "Poly_" << lineIdx << "_2" << std::endl;
|
||||
sw << "{" << (float)wheelArcHeight.downLine[0].x << "," << (float)wheelArcHeight.downLine[0].y << "," << (float)wheelArcHeight.downLine[0].z << "}-";
|
||||
sw << "{0,0}-{0,0}-";
|
||||
sw << "{" << (int)rgb.r << "," << (int)rgb.g << "," << (int)rgb.b << "," << size << "}" << std::endl;
|
||||
sw << "{" << (float)wheelArcHeight.downLine[1].x << "," << (float)wheelArcHeight.downLine[1].y << "," << (float)wheelArcHeight.downLine[1].z << "}-";
|
||||
sw << "{0,0}-{0,0}-";
|
||||
sw << "{" << (int)rgb.r << "," << (int)rgb.g << "," << (int)rgb.b << "," << size << "}" << std::endl;
|
||||
|
||||
lineIdx++;
|
||||
sw << "Poly_" << lineIdx << "_2" << std::endl;
|
||||
sw << "{" << (float)wheelArcHeight.centerLine[0].x << "," << (float)wheelArcHeight.centerLine[0].y << "," << (float)wheelArcHeight.centerLine[0].z << "}-";
|
||||
sw << "{0,0}-{0,0}-";
|
||||
sw << "{" << (int)rgb.r << "," << (int)rgb.g << "," << (int)rgb.b << "," << size << "}" << std::endl;
|
||||
sw << "{" << (float)wheelArcHeight.centerLine[1].x << "," << (float)wheelArcHeight.centerLine[1].y << "," << (float)wheelArcHeight.centerLine[1].z << "}-";
|
||||
sw << "{0,0}-{0,0}-";
|
||||
sw << "{" << (int)rgb.r << "," << (int)rgb.g << "," << (int)rgb.b << "," << size << "}" << std::endl;
|
||||
|
||||
lineIdx++;
|
||||
sw << "Poly_" << lineIdx << "_2" << std::endl;
|
||||
sw << "{" << (float)wheelArcHeight.arcLine[0].x << "," << (float)wheelArcHeight.arcLine[0].y << "," << (float)wheelArcHeight.arcLine[0].z << "}-";
|
||||
sw << "{0,0}-{0,0}-";
|
||||
sw << "{" << (int)rgb.r << "," << (int)rgb.g << "," << (int)rgb.b << "," << size << "}" << std::endl;
|
||||
sw << "{" << (float)wheelArcHeight.arcLine[1].x << "," << (float)wheelArcHeight.arcLine[1].y << "," << (float)wheelArcHeight.arcLine[1].z << "}-";
|
||||
sw << "{0,0}-{0,0}-";
|
||||
sw << "{" << (int)rgb.r << "," << (int)rgb.g << "," << (int)rgb.b << "," << size << "}" << std::endl;
|
||||
#endif
|
||||
sw.close();
|
||||
}
|
||||
|
||||
@ -884,26 +803,65 @@ int main()
|
||||
|
||||
#if TEST_COMPUTE_GRASP_POS
|
||||
const char* dataPath[TEST_GROUP] = {
|
||||
"E:\\ShangGu\\电机定子\\", //0
|
||||
"F:/ShangGu/电机定子/数据1/", //0
|
||||
};
|
||||
|
||||
SVzNLRange fileIdx[TEST_GROUP] = {
|
||||
{1,1} };
|
||||
|
||||
double camPoseR[9] = {
|
||||
1.0, 0.0, 0.0,
|
||||
0.0, 1.0, 0.0,
|
||||
0.0, 0.0, 1.0 };
|
||||
|
||||
SWD_statorParam algoParam;
|
||||
algoParam.statorOuterD = 85.0;
|
||||
algoParam.statorInnerD = 50.0;
|
||||
|
||||
char _scan_file[256];
|
||||
|
||||
int endGroup = TEST_GROUP - 1;
|
||||
for (int grp = 0; grp <= endGroup; grp++)
|
||||
{
|
||||
SSG_planeCalibPara poseCalibPara;
|
||||
//初始化成单位阵
|
||||
poseCalibPara.planeCalib[0] = 1.0;
|
||||
poseCalibPara.planeCalib[1] = 0.0;
|
||||
poseCalibPara.planeCalib[2] = 0.0;
|
||||
poseCalibPara.planeCalib[3] = 0.0;
|
||||
poseCalibPara.planeCalib[4] = 1.0;
|
||||
poseCalibPara.planeCalib[5] = 0.0;
|
||||
poseCalibPara.planeCalib[6] = 0.0;
|
||||
poseCalibPara.planeCalib[7] = 0.0;
|
||||
poseCalibPara.planeCalib[8] = 1.0;
|
||||
poseCalibPara.planeHeight = 2600.0;
|
||||
for (int i = 0; i < 9; i++)
|
||||
poseCalibPara.invRMatrix[i] = poseCalibPara.planeCalib[i];
|
||||
char calibFile[250];
|
||||
#if 0
|
||||
if (grp == 0)
|
||||
{
|
||||
sprintf_s(calibFile, "F:\\ShangGu\\粒径数据\\曝光\\3D数据\\ground_calib_para.txt");
|
||||
poseCalibPara = _readCalibPara(calibFile);
|
||||
}
|
||||
#endif
|
||||
|
||||
SWD_statorParam statorParam;
|
||||
statorParam.statorOuterD = 85.0;
|
||||
statorParam.statorInnerD = 50.0;
|
||||
|
||||
SWD_statorPositonParam algoParam;
|
||||
memset(&algoParam, 0, sizeof(SWD_statorPositonParam));
|
||||
algoParam.cornerParam.cornerTh = 75; //45度角
|
||||
algoParam.cornerParam.scale = 5; // algoParam.bagParam.bagH / 8; // 15; // algoParam.bagParam.bagH / 8;
|
||||
algoParam.cornerParam.minEndingGap = 20; // algoParam.bagParam.bagW / 4;
|
||||
algoParam.cornerParam.minEndingGap_z = 20;
|
||||
algoParam.cornerParam.jumpCornerTh_1 = 20; //水平角度，小于此角度视为水平
|
||||
algoParam.cornerParam.jumpCornerTh_2 = 60;
|
||||
SSG_outlierFilterParam filterParam;
|
||||
algoParam.filterParam.continuityTh = 20.0; //噪声滤除。当相邻点的z跳变大于此门限时，检查是否为噪声。若长度小于outlierLen， 视为噪声
|
||||
algoParam.filterParam.outlierTh = 5;
|
||||
SSG_treeGrowParam growParam;
|
||||
algoParam.growParam.maxLineSkipNum = 10;
|
||||
algoParam.growParam.yDeviation_max = 10.0;
|
||||
algoParam.growParam.maxSkipDistance = 10.0;
|
||||
algoParam.growParam.zDeviation_max = 10.0;// algoParam.bagParam.bagH / 2; //袋子高度1/2
|
||||
algoParam.growParam.minLTypeTreeLen = 100; //mm
|
||||
algoParam.growParam.minVTypeTreeLen = 100; //mm
|
||||
|
||||
SWD_nextOpParam refPos;
|
||||
memset(&refPos, 0, sizeof(SWD_nextOpParam));
|
||||
refPos.cuttingZ = -1; //初始值，设为-1
|
||||
for (int fidx = fileIdx[grp].nMin; fidx <= fileIdx[grp].nMax; fidx++)
|
||||
{
|
||||
//fidx = 4;
|
||||
@ -912,31 +870,37 @@ int main()
|
||||
int dataCalib = 0;
|
||||
int maxTimeStamp = 0;
|
||||
int clockPerSecond = 0;
|
||||
|
||||
char _scan_file[256];
|
||||
sprintf_s(_scan_file, "%sLaserLine%d_grid.txt", dataPath[grp], fidx);
|
||||
SVzNL3DLaserLine* laser3DPoints = vzReadLaserScanPointFromFile_XYZ(_scan_file, &lineNum, &lineV, &dataCalib, &maxTimeStamp, &clockPerSecond);
|
||||
if (laser3DPoints == NULL)
|
||||
std::vector<std::vector< SVzNL3DPosition>> scanLines;
|
||||
vzReadLaserScanPointFromFile_XYZ_vector(_scan_file, scanLines);
|
||||
if (scanLines.size() == 0)
|
||||
continue;
|
||||
|
||||
long t1 = GetTickCount64();
|
||||
int errCode = 0;
|
||||
std::vector<SSG_motorStatorPosition> resultOpPositions;
|
||||
sg_motorStatorPosition(
|
||||
laser3DPoints,
|
||||
lineNum,
|
||||
std::vector<SWD_motorStatorPosition> resultObjPositions;
|
||||
SWD_statorOuterGrasper resultGraspPos;
|
||||
wd_motorStatorPosition(
|
||||
scanLines,
|
||||
statorParam,
|
||||
poseCalibPara,
|
||||
algoParam,
|
||||
&refPos, //上一次给出的参考位置，同时输出下一次的参考位置
|
||||
&errCode,
|
||||
resultOpPositions);
|
||||
resultObjPositions,
|
||||
resultGraspPos);
|
||||
long t2 = GetTickCount64();
|
||||
|
||||
char _dbg_file[256];
|
||||
#if 1
|
||||
sprintf_s(_dbg_file, "%sresult\\LaserLine%d_result.txt", dataPath[grp], fidx);
|
||||
_outputScanDataFile_RGBD_obj(_dbg_file, laser3DPoints, lineNum, lineV, maxTimeStamp, clockPerSecond, NULL);
|
||||
|
||||
_outputScanDataFile_RGBD_obj(_dbg_file,scanLines, 0,0,0, resultObjPositions);
|
||||
sprintf_s(_dbg_file, "%sresult\\LaserLine%d_result_img.png", dataPath[grp], fidx);
|
||||
cv::String imgName(_dbg_file);
|
||||
double rpy[3] = { -30, 15, 0 }; //{ 0,-45, 0 }; //
|
||||
_genXOYProjectionImage(imgName, laser3DPoints, lineNum, rpy);
|
||||
//_genXOYProjectionImage(imgName, laser3DPoints, lineNum, rpy);
|
||||
#endif
|
||||
printf("%s: %d(ms)!\n", _scan_file, (int)(t2 - t1));
|
||||
}
|
||||
|
||||
@ -12,13 +12,13 @@ const char* wd_BQAssemblyPositionVersion(void)
|
||||
return m_strVersion.c_str();
|
||||
}
|
||||
|
||||
//计算一个平面调平参数。
|
||||
//针对孔板计算一个平面调平参数：简化了算法，提取扫描线中没有孔的扫描线进行拟合
|
||||
//数据输入中可以有一个地平面和参考调平平面，以最高的平面进行调平
|
||||
//旋转矩阵为调平参数，即将平面法向调整为垂直向量的参数
|
||||
SSG_planeCalibPara sx_BQ_getBaseCalibPara(
|
||||
SSG_planeCalibPara sx_BQ_getHoleBaseCalibPara(
|
||||
std::vector< std::vector<SVzNL3DPosition>>& scanLines)
|
||||
{
|
||||
return sg_getPlaneCalibPara2(scanLines);
|
||||
return sg_getHolePlaneCalibPara(scanLines);
|
||||
}
|
||||
|
||||
//相机姿态调平，并去除地面
|
||||
@ -66,6 +66,37 @@ SSX_BQAssemblyInfo sx_BQ_computeAssemblyInfoFrom3D(
|
||||
return assemblyPose;
|
||||
}
|
||||
|
||||
std::vector<std::vector<int>> groundMask;
|
||||
for (int line = 0, line_max = (int)scanLines.size(); line < line_max; line++)
|
||||
{
|
||||
//调平，去除地面
|
||||
std::vector<SVzNL3DPosition>& a_line = scanLines[line];
|
||||
std::vector<int> a_lineMask;
|
||||
a_lineMask.resize(a_line.size());
|
||||
for (int i = 0; i < a_line.size(); i++)
|
||||
{
|
||||
if ((line == 1000) && (i == 1439))
|
||||
int kkk = 1;
|
||||
a_line[i].nPointIdx = i;
|
||||
a_lineMask[i] = 0;
|
||||
SVzNL3DPoint& a_pt = a_line[i].pt3D;
|
||||
if (a_pt.z < 1e-4)
|
||||
continue;
|
||||
double x = a_pt.x * groundCalibPara.planeCalib[0] + a_pt.y * groundCalibPara.planeCalib[1] + a_pt.z * groundCalibPara.planeCalib[2];
|
||||
double y = a_pt.x * groundCalibPara.planeCalib[3] + a_pt.y * groundCalibPara.planeCalib[4] + a_pt.z * groundCalibPara.planeCalib[5];
|
||||
double z = a_pt.x * groundCalibPara.planeCalib[6] + a_pt.y * groundCalibPara.planeCalib[7] + a_pt.z * groundCalibPara.planeCalib[8];
|
||||
if ((groundCalibPara.planeHeight > 0) && (z > (groundCalibPara.planeHeight-5))) //生成地面Mask
|
||||
{
|
||||
a_lineMask[i] = 1; // a_line[i].nPointIdx = -1; //mark
|
||||
}
|
||||
a_pt.x = x;
|
||||
a_pt.y = y;
|
||||
a_pt.z = z;
|
||||
a_line[i].pt3D = a_pt;
|
||||
}
|
||||
groundMask.push_back(a_lineMask);
|
||||
}
|
||||
|
||||
//将开始和结束的空白扫描线去除，获得扫描边界
|
||||
int validStartLine = -1;
|
||||
for (int i = 0; i < lineNum; i++)
|
||||
@ -98,10 +129,11 @@ SSX_BQAssemblyInfo sx_BQ_computeAssemblyInfoFrom3D(
|
||||
|
||||
//自适应各种旋转角度
|
||||
//垂直跳变特征提取
|
||||
double contourWin = 40.0; //地面附近的拐点视为端点
|
||||
std::vector<std::vector<SSG_basicFeature1D>> jumpFeatures_v_raw;
|
||||
for (int line = 0; line < lineNum; line++)
|
||||
{
|
||||
if (line == 250)
|
||||
if (line == 1000)
|
||||
int kkk = 1;
|
||||
|
||||
std::vector<SVzNL3DPosition>& lineData = scanLines[line];
|
||||
@ -113,11 +145,12 @@ SSX_BQAssemblyInfo sx_BQ_computeAssemblyInfoFrom3D(
|
||||
|
||||
std::vector<SSG_basicFeature1D> line_features;
|
||||
int dataSize = (int)lineData.size();
|
||||
sg_getLineCornerFeature_BQ(
|
||||
&lineData[0],
|
||||
dataSize,
|
||||
/// 提取激光线上的拐点特征，地面端点附近的拐点视为合格拐点
|
||||
sg_getLineContourCornerFeature_groundMask_BQ(
|
||||
lineData,
|
||||
groundMask[line],
|
||||
line,
|
||||
groundCalibPara.planeHeight,
|
||||
contourWin, //ground边缘范围
|
||||
cornerPara, //scale通常取bagH的1/4
|
||||
line_features);
|
||||
jumpFeatures_v_raw.push_back(line_features);
|
||||
@ -130,10 +163,19 @@ SSX_BQAssemblyInfo sx_BQ_computeAssemblyInfoFrom3D(
|
||||
}
|
||||
|
||||
//生成水平扫描
|
||||
std::vector<std::vector<SSG_basicFeature1D>> jumpFeatures_h_raw;
|
||||
std::vector<std::vector<SVzNL3DPosition>> hLines_raw;
|
||||
std::vector<std::vector<int>> groundMask_hLines;
|
||||
hLines_raw.resize(linePtNum);
|
||||
int lineNum_h_raw = (int)hLines_raw.size();
|
||||
#if 0
|
||||
hLines_raw.resize(linePtNum);
|
||||
groundMask_hLines.resize(linePtNum);
|
||||
for (int i = 0; i < linePtNum; i++)
|
||||
{
|
||||
hLines_raw[i].resize(lineNum);
|
||||
groundMask_hLines[i].resize(lineNum);
|
||||
}
|
||||
for (int line = 0; line < lineNum; line++)
|
||||
{
|
||||
for (int j = 0; j < linePtNum; j++)
|
||||
@ -142,14 +184,13 @@ SSX_BQAssemblyInfo sx_BQ_computeAssemblyInfoFrom3D(
|
||||
hLines_raw[j][line] = scanLines[line][j];
|
||||
hLines_raw[j][line].pt3D.x = scanLines[line][j].pt3D.y;
|
||||
hLines_raw[j][line].pt3D.y = scanLines[line][j].pt3D.x;
|
||||
groundMask_hLines[j][line] = groundMask[line][j];
|
||||
}
|
||||
}
|
||||
//水平arc特征提取
|
||||
std::vector<std::vector<SSG_basicFeature1D>> jumpFeatures_h_raw;
|
||||
int lineNum_h_raw = (int)hLines_raw.size();
|
||||
for (int line = 0; line < lineNum_h_raw; line++)
|
||||
{
|
||||
if (line == 416)
|
||||
if (line == 1906)
|
||||
int kkk = 1;
|
||||
std::vector<SVzNL3DPosition>& lineData = hLines_raw[line];
|
||||
//滤波，滤除异常点
|
||||
@ -158,55 +199,160 @@ SSX_BQAssemblyInfo sx_BQ_computeAssemblyInfoFrom3D(
|
||||
|
||||
std::vector<SSG_basicFeature1D> line_features;
|
||||
int dataSize = (int)lineData.size();
|
||||
sg_getLineCornerFeature_BQ(
|
||||
&hLines_raw[line][0],
|
||||
dataSize,
|
||||
|
||||
sg_getLineContourCornerFeature_groundMask_BQ(
|
||||
hLines_raw[line],
|
||||
groundMask_hLines[line],
|
||||
line,
|
||||
groundCalibPara.planeHeight,
|
||||
contourWin, //ground边缘范围
|
||||
cornerPara, //scale通常取bagH的1/4
|
||||
line_features);
|
||||
jumpFeatures_h_raw.push_back(line_features);
|
||||
}
|
||||
|
||||
#endif
|
||||
//特征生长，用于滤除噪点
|
||||
//垂直方向特征生长（激光线方向）
|
||||
std::vector<SSG_featureTree> v_trees;
|
||||
//使用聚类法代替水平和垂直特征生长
|
||||
|
||||
std::vector<std::vector<SSG_featureClusteringInfo>> featureInfoMask;
|
||||
std::vector<std::vector<SVzNL3DPoint>> feature3DInfo;
|
||||
featureInfoMask.resize(lineNum);
|
||||
feature3DInfo.resize(lineNum);
|
||||
for (int i = 0; i < lineNum; i++)
|
||||
{
|
||||
featureInfoMask[i].resize(linePtNum);
|
||||
feature3DInfo[i].resize(linePtNum);
|
||||
}
|
||||
//生成Mask
|
||||
double meanX = 0, meanY = 0, meanZ = 0;
|
||||
int objPtSize = 0;
|
||||
for (int line = 0; line < lineNum; line++)
|
||||
{
|
||||
bool isLastLine = false;
|
||||
if (line == lineNum - 1)
|
||||
isLastLine = true;
|
||||
std::vector<SSG_basicFeature1D>& a_lineJumpFeature = jumpFeatures_v_raw[line];
|
||||
if (a_lineJumpFeature.size() > 0)
|
||||
int kkk = 1;
|
||||
if (line == 202)
|
||||
int kkk = 1;
|
||||
sg_lineFeaturesGrowing(
|
||||
line,
|
||||
isLastLine,
|
||||
a_lineJumpFeature,
|
||||
v_trees,
|
||||
growParam);
|
||||
int featureNum = jumpFeatures_v_raw[line].size();
|
||||
for (int j = 0; j < featureNum; j++)
|
||||
{
|
||||
SVzNL2DPoint& a_pos = jumpFeatures_v_raw[line][j].jumpPos2D;
|
||||
SSG_featureClusteringInfo a_mask;
|
||||
memset(&a_mask, 0, sizeof(SSG_featureClusteringInfo));
|
||||
a_mask.featurType = 1;
|
||||
a_mask.lineIdx = a_pos.x;
|
||||
a_mask.ptIdx = a_pos.y;
|
||||
featureInfoMask[a_pos.x][a_pos.y] = a_mask;
|
||||
feature3DInfo[a_pos.x][a_pos.y] = scanLines[a_pos.x][a_pos.y].pt3D;
|
||||
meanX += scanLines[a_pos.x][a_pos.y].pt3D.x;
|
||||
meanY += scanLines[a_pos.x][a_pos.y].pt3D.y;
|
||||
meanZ += scanLines[a_pos.x][a_pos.y].pt3D.z;
|
||||
objPtSize++;
|
||||
}
|
||||
}
|
||||
|
||||
//水平方向特征生长（扫描运动方向）
|
||||
std::vector<SSG_featureTree> h_trees;
|
||||
#if 0
|
||||
for (int line = 0; line < lineNum_h_raw; line++)
|
||||
{
|
||||
if (line == 650)
|
||||
int kkk = 1;
|
||||
bool isLastLine = false;
|
||||
if (line == lineNum_h_raw - 1)
|
||||
isLastLine = true;
|
||||
std::vector<SSG_basicFeature1D>& a_lineJumpFeature = jumpFeatures_h_raw[line];
|
||||
sg_lineFeaturesGrowing(
|
||||
line,
|
||||
isLastLine,
|
||||
a_lineJumpFeature,
|
||||
h_trees,
|
||||
growParam);
|
||||
int featureNum = jumpFeatures_h_raw[line].size();
|
||||
for (int j = 0; j < featureNum; j++)
|
||||
{
|
||||
SVzNL2DPoint& a_pos = jumpFeatures_v_raw[line][j].jumpPos2D;
|
||||
if (featureInfoMask[a_pos.y][a_pos.x].featurType == 0)
|
||||
{
|
||||
SSG_featureClusteringInfo a_mask;
|
||||
memset(&a_mask, 0, sizeof(SSG_featureClusteringInfo));
|
||||
a_mask.featurType = 1;
|
||||
a_mask.lineIdx = a_pos.y;
|
||||
a_mask.ptIdx = a_pos.x;
|
||||
featureInfoMask[a_pos.y][a_pos.x] = a_mask;
|
||||
feature3DInfo[a_pos.y][a_pos.x] = scanLines[a_pos.y][a_pos.x].pt3D;
|
||||
}
|
||||
}
|
||||
}
|
||||
#endif
|
||||
|
||||
#if 0
|
||||
//聚类
|
||||
//采用迭代思想，回归思路进行高效聚类
|
||||
std::vector<std::vector< SVzNL2DPoint>> clusters; //只记录位置
|
||||
std::vector<SWD_clustersInfo> clustersInfo;
|
||||
int clusterID = 1;
|
||||
int clusterCheckWin = 30;
|
||||
for (int y = 0; y < linePtNum; y++)
|
||||
{
|
||||
for (int x = 0; x < lineNum; x++)
|
||||
{
|
||||
SSG_featureClusteringInfo& a_featureInfo = featureInfoMask[x][y];
|
||||
if ((0 == a_featureInfo.featurType) || (a_featureInfo.clusterID > 0)) //非特征或已经处理
|
||||
continue;
|
||||
|
||||
SVzNL3DPoint& a_feature3DValue = feature3DInfo[x][y];
|
||||
SVzNL3DRangeD a_clusterRoi;
|
||||
a_clusterRoi.xRange.min = a_feature3DValue.x;
|
||||
a_clusterRoi.xRange.max = a_feature3DValue.x;
|
||||
a_clusterRoi.yRange.min = a_feature3DValue.y;
|
||||
a_clusterRoi.yRange.max = a_feature3DValue.y;
|
||||
a_clusterRoi.zRange.min = a_feature3DValue.z;
|
||||
a_clusterRoi.zRange.max = a_feature3DValue.z;
|
||||
|
||||
SVzNL2DPoint a_seedPos = { x, y };
|
||||
std::vector< SVzNL2DPoint> a_cluster;
|
||||
a_cluster.push_back(a_seedPos);
|
||||
wd_gridPointClustering(
|
||||
featureInfoMask,//int，记录特征标记和clusterID，附加一个flag
|
||||
feature3DInfo,//double,记录坐标信息
|
||||
clusterCheckWin, //搜索窗口
|
||||
growParam,//聚类条件
|
||||
clusterID, //当前Cluster的ID
|
||||
a_cluster, //result
|
||||
a_clusterRoi
|
||||
);
|
||||
clusters.push_back(a_cluster);
|
||||
SWD_clustersInfo a_info;
|
||||
a_info.clusterIdx = clusterID;
|
||||
a_info.ptSize = (int)a_cluster.size();
|
||||
a_info.roi3D = a_clusterRoi;
|
||||
clustersInfo.push_back(a_info);
|
||||
clusterID++;
|
||||
}
|
||||
}
|
||||
//取最大的聚类为边界
|
||||
int clusterSize = clusters.size();
|
||||
if(clusterSize == 0)
|
||||
{
|
||||
*errCode = SX_ERR_ZERO_OBJ;
|
||||
return assemblyPose;
|
||||
}
|
||||
|
||||
int objPtSize = clusters[0].size();
|
||||
int objClusterID = 0;
|
||||
for (int i = 1; i < clusterSize; i++)
|
||||
{
|
||||
if (clusters[i].size() > objPtSize)
|
||||
{
|
||||
objPtSize = (int)clusters[i].size();
|
||||
objClusterID = i;
|
||||
}
|
||||
}
|
||||
if (objPtSize == 0)
|
||||
{
|
||||
*errCode = SX_ERR_ZERO_OBJ;
|
||||
return assemblyPose;
|
||||
}
|
||||
|
||||
double meanX = 0, meanY = 0, meanZ = 0;
|
||||
for (int i = 0; i < objPtSize; i++)
|
||||
{
|
||||
int objLineIdx = clusters[objClusterID][i].x;
|
||||
int objPtIdx = clusters[objClusterID][i].y;
|
||||
scanLines[objLineIdx][objPtIdx].nPointIdx = 1;
|
||||
meanX += scanLines[objLineIdx][objPtIdx].pt3D.x;
|
||||
meanY += scanLines[objLineIdx][objPtIdx].pt3D.y;
|
||||
meanZ += scanLines[objLineIdx][objPtIdx].pt3D.z;
|
||||
}
|
||||
#endif
|
||||
meanX = meanX / objPtSize;
|
||||
meanY = meanY / objPtSize;
|
||||
meanZ = meanZ / objPtSize;
|
||||
assemblyPose.O.x = meanX;
|
||||
assemblyPose.O.y = meanY;
|
||||
assemblyPose.O.z = meanZ;
|
||||
|
||||
#if 0
|
||||
std::vector<SWD_polarPt> polarPoints;
|
||||
for (int i = 0, i_max = (int)v_trees.size(); i < i_max; i++)
|
||||
{
|
||||
@ -407,20 +553,21 @@ SSX_BQAssemblyInfo sx_BQ_computeAssemblyInfoFrom3D(
|
||||
std::vector<SVzNL3DPoint> sidePts_2;
|
||||
std::vector<SVzNL3DPoint> sidePts_3;
|
||||
std::vector<SVzNL3DPoint> sidePts_4;
|
||||
#endif
|
||||
return assemblyPose;
|
||||
}
|
||||
|
||||
//根据Mark计算工件位置和姿态
|
||||
SSX_BQAssemblyInfo sx_BQ_computeAssemblyInfoFromMark(
|
||||
SSX_BQAssemblyInfo originPos,
|
||||
std::vector<cv::Point3d> originMarkPos,
|
||||
std::vector<cv::Point3d> currMarkPos,
|
||||
SSX_BQAssemblyInfo& originPos,
|
||||
std::vector<SVzNL3DPoint>& originMark,
|
||||
std::vector<SVzNL3DPoint>& currMark,
|
||||
int* errCode)
|
||||
{
|
||||
*errCode = 0;
|
||||
SSX_BQAssemblyInfo resultPos;
|
||||
memset(&resultPos, 0, sizeof(SSX_BQAssemblyInfo));
|
||||
if ((originMarkPos.size() < 3) || (currMarkPos.size() < 3))
|
||||
if ((originMark.size() < 3) || (currMark.size() < 3) || (originMark.size() != currMark.size()))
|
||||
{
|
||||
*errCode = SX_ERR_INVLID_MARK_NUM;
|
||||
return resultPos;
|
||||
@ -428,13 +575,42 @@ SSX_BQAssemblyInfo sx_BQ_computeAssemblyInfoFromMark(
|
||||
|
||||
cv::Mat R, T; //旋转平移
|
||||
cv::Point3d C_origin, C_curr; //质心
|
||||
|
||||
std::vector<cv::Point3d> originMarkPos;
|
||||
originMarkPos.resize(originMark.size());
|
||||
std::vector<cv::Point3d> currMarkPos;
|
||||
currMarkPos.resize(originMark.size());
|
||||
for (int i = 0; i < (int)originMark.size(); i++)
|
||||
{
|
||||
originMarkPos[i] = { originMark[i].x, originMark[i].y, originMark[i].z };
|
||||
currMarkPos[i] = { currMark[i].x, currMark[i].y, currMark[i].z};
|
||||
}
|
||||
caculateRT(originMarkPos, currMarkPos, R, T, C_origin, C_curr);
|
||||
std::vector<cv::Point3d> originMarkPos_RT;
|
||||
originMarkPos_RT.resize(originMarkPos.size());
|
||||
for (int i = 0; i < (int)originMark.size(); i++)
|
||||
{
|
||||
cv::Point3d RT_pt;
|
||||
pointRT(R, T, C_origin, C_curr, originMarkPos[i], RT_pt);
|
||||
originMarkPos_RT[i] = RT_pt;
|
||||
}
|
||||
|
||||
// 5. 推算Pn的坐标
|
||||
pointRT(R, T, C_origin, C_curr, originPos.O, resultPos.O);
|
||||
pointRT(R, T, C_origin, C_curr, originPos.X, resultPos.X);
|
||||
pointRT(R, T, C_origin, C_curr, originPos.Y, resultPos.Y);
|
||||
pointRT(R, T, C_origin, C_curr, originPos.Z, resultPos.Z);
|
||||
cv::Point3d O = { originPos.O.x, originPos.O.y, originPos.O.z};
|
||||
cv::Point3d X = { originPos.X.x, originPos.X.y, originPos.X.z };
|
||||
cv::Point3d Y = { originPos.Y.x, originPos.Y.y, originPos.Y.z };
|
||||
cv::Point3d Z = { originPos.Z.x, originPos.Z.y, originPos.Z.z };
|
||||
|
||||
cv::Point3d RO, RX, RY, RZ;
|
||||
pointRT(R, T, C_origin, C_curr, O, RO);
|
||||
pointRT(R, T, C_origin, C_curr, X, RX);
|
||||
pointRT(R, T, C_origin, C_curr, Y, RY);
|
||||
pointRT(R, T, C_origin, C_curr, Z, RZ);
|
||||
resultPos.O = { RO.x, RO.y, RO.z };
|
||||
resultPos.X = { RX.x, RX.y, RX.z };
|
||||
resultPos.Y = { RY.x, RY.y, RY.z };
|
||||
resultPos.Z = { RZ.x, RZ.y, RZ.z };
|
||||
|
||||
return resultPos;
|
||||
}
|
||||
|
||||
|
||||
@ -14,10 +14,10 @@ typedef struct
|
||||
typedef struct
|
||||
{
|
||||
int type; //组件种类
|
||||
cv::Point3d O; //工件中心点
|
||||
cv::Point3d X; //工件坐标系OX轴向量。
|
||||
cv::Point3d Y; //工件坐标系OY轴向量。
|
||||
cv::Point3d Z; //工件坐标系OZ轴向量。
|
||||
SVzNL3DPoint O; //工件中心点
|
||||
SVzNL3DPoint X; //工件坐标系OX轴向量。
|
||||
SVzNL3DPoint Y; //工件坐标系OY轴向量。
|
||||
SVzNL3DPoint Z; //工件坐标系OZ轴向量。
|
||||
}SSX_BQAssemblyInfo;
|
||||
|
||||
//读版本号
|
||||
@ -26,7 +26,7 @@ SG_APISHARED_EXPORT const char* wd_BQAssemblyPositionVersion(void);
|
||||
//计算一个平面调平参数。
|
||||
//数据输入中可以有一个地平面和参考调平平面，以最高的平面进行调平
|
||||
//旋转矩阵为调平参数，即将平面法向调整为垂直向量的参数
|
||||
SG_APISHARED_EXPORT SSG_planeCalibPara sx_BQ_getBaseCalibPara(
|
||||
SG_APISHARED_EXPORT SSG_planeCalibPara sx_BQ_getHoleBaseCalibPara(
|
||||
std::vector< std::vector<SVzNL3DPosition>>& scanLines);
|
||||
|
||||
//相机姿态调平，并去除地面
|
||||
@ -36,7 +36,7 @@ SG_APISHARED_EXPORT void sx_BQ_lineDataR(
|
||||
double groundH);
|
||||
|
||||
//提取工件角点及定位长度信息
|
||||
SG_APISHARED_EXPORT SSX_BQAssemblyInfo sx_BQ_getAssemblyInfo(
|
||||
SG_APISHARED_EXPORT SSX_BQAssemblyInfo sx_BQ_computeAssemblyInfoFrom3D(
|
||||
std::vector< std::vector<SVzNL3DPosition>>& scanLines,
|
||||
const SSG_cornerParam cornerPara,
|
||||
const SSG_outlierFilterParam filterParam,
|
||||
@ -45,5 +45,10 @@ SG_APISHARED_EXPORT SSX_BQAssemblyInfo sx_BQ_getAssemblyInfo(
|
||||
SSX_BQAssemblyPara assemblyParam,
|
||||
int* errCode);
|
||||
|
||||
|
||||
//根据Mark计算工件位置和姿态
|
||||
SG_APISHARED_EXPORT SSX_BQAssemblyInfo sx_BQ_computeAssemblyInfoFromMark(
|
||||
SSX_BQAssemblyInfo& originPos,
|
||||
std::vector<SVzNL3DPoint>& originMark,
|
||||
std::vector<SVzNL3DPoint>& currMark,
|
||||
int* errCode);
|
||||
|
||||
|
||||
@ -67,6 +67,14 @@ SG_APISHARED_EXPORT void sg_maskData_getLineCornerFeature(
|
||||
const SSG_cornerParam cornerPara, //scaleͨ³£È¡bagHµÄ1/4
|
||||
std::vector<SSG_basicFeature1D>& features);
|
||||
|
||||
//提取Gap特征。Gap特征：由两个负的Corner构成的区域
|
||||
SG_APISHARED_EXPORT void sg_getLineGapFeature(
|
||||
std::vector<SVzNL3DPosition>& lineData, //扫描线
|
||||
int lineIdx, //当前扫描线序号
|
||||
const SSG_cornerParam cornerPara,
|
||||
const SVzNLRangeD gapParam,
|
||||
std::vector<SSG_basicFeatureGap>& line_gaps);
|
||||
|
||||
SG_APISHARED_EXPORT void sg_getLineCornerFeature_BQ(
|
||||
SVzNL3DPosition* lineData,
|
||||
int dataSize,
|
||||
@ -75,6 +83,24 @@ SG_APISHARED_EXPORT void sg_getLineCornerFeature_BQ(
|
||||
const SSG_cornerParam cornerPara,
|
||||
std::vector<SSG_basicFeature1D>& line_features);
|
||||
|
||||
/// <summary>
|
||||
/// 提取激光线上的拐点特征，地面端点附近的拐点视为合格拐点
|
||||
/// 根据Seg进行corner提取
|
||||
/// nPointIdx被重新定义成Feature类型
|
||||
/// 算法流程：
|
||||
/// （1）逐点计算前向角和后向角
|
||||
/// （2）逐点计算拐角，顺时针为负，逆时针为正
|
||||
/// （3）搜索正拐角的极大值。
|
||||
/// （4）判断拐角是否为跳变
|
||||
/// </summary>
|
||||
SG_APISHARED_EXPORT void sg_getLineContourCornerFeature_groundMask_BQ(
|
||||
std::vector<SVzNL3DPosition>& lineData,
|
||||
std::vector<int> groundMask,
|
||||
int lineIdx,
|
||||
double contourWin, //ground边缘范围
|
||||
const SSG_cornerParam cornerPara, //scale通常取bagH的1/4
|
||||
std::vector<SSG_basicFeature1D>& line_features);
|
||||
|
||||
SG_APISHARED_EXPORT void wd_getLineDataIntervals(
|
||||
std::vector<SVzNL3DPosition>& lineData,
|
||||
const SSG_lineSegParam lineSegPara,
|
||||
@ -218,6 +244,14 @@ SG_APISHARED_EXPORT void sg_lineFeaturesGrowing(
|
||||
std::vector<SSG_featureTree>& trees,
|
||||
SSG_treeGrowParam growParam);
|
||||
|
||||
//gap特征生长
|
||||
SG_APISHARED_EXPORT void sg_lineGapsGrowing(
|
||||
int lineIdx,
|
||||
bool isLastLine,
|
||||
std::vector<SSG_basicFeatureGap>& features,
|
||||
std::vector<SSG_gapFeatureTree>& trees,
|
||||
SSG_treeGrowParam growParam);
|
||||
|
||||
//SG_APISHARED_EXPORT void sg_getTreeROI(SSG_featureTree* a_tree);
|
||||
|
||||
//¶Ôending½øÐÐÉú³¤
|
||||
|
||||
@ -265,6 +265,18 @@ typedef struct
|
||||
int angleChkScalePos; //仅用于加速angleCheck的速度
|
||||
}SSG_featureTree;
|
||||
|
||||
typedef struct
|
||||
{
|
||||
int treeState;
|
||||
int treeType;
|
||||
int sLineIdx;
|
||||
int eLineIdx;
|
||||
double tree_value;
|
||||
SSG_ROIRectD roi;
|
||||
std::vector< SSG_basicFeatureGap> treeNodes;
|
||||
int angleChkScalePos; //仅用于加速angleCheck的速度
|
||||
}SSG_gapFeatureTree;
|
||||
|
||||
typedef struct
|
||||
{
|
||||
int treeState;
|
||||
@ -531,4 +543,5 @@ typedef struct
|
||||
int clusterIdx;
|
||||
int ptSize;
|
||||
SVzNL3DRangeD roi3D;
|
||||
SVzNLRect roi2D;
|
||||
}SWD_clustersInfo;
|
||||
|
||||
@ -2057,6 +2057,161 @@ SSG_planeCalibPara sg_getPlaneCalibPara2(
|
||||
return planePara;
|
||||
}
|
||||
|
||||
//针对孔板计算一个平面调平参数：提取不经过孔的扫描线进行平面拟合
|
||||
//数据输入中可以有一个地平面和参考调平平面，以最高的平面进行调平
|
||||
//旋转矩阵为调平参数，即将平面法向调整为垂直向量的参数
|
||||
SSG_planeCalibPara sg_getHolePlaneCalibPara(
|
||||
std::vector< std::vector<SVzNL3DPosition>>& scanLines)
|
||||
{
|
||||
//设置初始结果
|
||||
double initCalib[9] = {
|
||||
1.0, 0.0, 0.0,
|
||||
0.0, 1.0, 0.0,
|
||||
0.0, 0.0, 1.0 };
|
||||
SSG_planeCalibPara planePara;
|
||||
for (int i = 0; i < 9; i++)
|
||||
planePara.planeCalib[i] = initCalib[i];
|
||||
planePara.planeHeight = -1.0;
|
||||
|
||||
SSG_lineSegParam segParam;
|
||||
segParam.segGapTh_y = 1.0;
|
||||
segParam.segGapTh_z = 1.0;
|
||||
int lineNum = (int)scanLines.size();
|
||||
std::vector<int> validLines;
|
||||
for (int line = 0; line < lineNum; line++)
|
||||
{
|
||||
//去除零点
|
||||
std::vector<SSG_RUN> segs;
|
||||
wd_getLineDataIntervals(scanLines[line], segParam, segs);
|
||||
if (segs.size() == 1)
|
||||
validLines.push_back(line);
|
||||
}
|
||||
|
||||
//取数据
|
||||
std::vector<cv::Point3f> Points3ds;
|
||||
for (int vline = 0; vline < (int)validLines.size(); vline++)
|
||||
{
|
||||
int line = validLines[vline];
|
||||
int nPositionCnt = (int)scanLines[line].size();
|
||||
for (int i = 0; i < nPositionCnt; i++)
|
||||
{
|
||||
SVzNL3DPosition* pt3D = &scanLines[line][i];
|
||||
if (pt3D->pt3D.z < 1e-4)
|
||||
continue;
|
||||
|
||||
cv::Point3f a_vldPt;
|
||||
a_vldPt.x = (float)pt3D->pt3D.x;
|
||||
a_vldPt.y = (float)pt3D->pt3D.y;
|
||||
a_vldPt.z = (float)pt3D->pt3D.z;
|
||||
Points3ds.push_back(a_vldPt);
|
||||
}
|
||||
}
|
||||
//平面拟合
|
||||
std::vector<double> planceFunc;
|
||||
vzCaculateLaserPlane(Points3ds, planceFunc);
|
||||
|
||||
#if 1 //两个向量的旋转旋转，使用四元数法，
|
||||
Vector3 a = Vector3(planceFunc[0], planceFunc[1], planceFunc[2]);
|
||||
Vector3 b = Vector3(0, 0, -1.0);
|
||||
Quaternion quanPara = rotationBetweenVectors(a, b);
|
||||
|
||||
RotationMatrix rMatrix;
|
||||
quaternionToMatrix(quanPara, rMatrix.data);
|
||||
//计算反向旋转矩阵
|
||||
Quaternion invQuanPara = rotationBetweenVectors(b, a);
|
||||
RotationMatrix invMatrix;
|
||||
quaternionToMatrix(invQuanPara, invMatrix.data);
|
||||
#else //根据平面的法向量计算欧拉角，进而计算旋转矩阵
|
||||
//参数计算
|
||||
SSG_EulerAngles eulerPra = planeNormalToEuler(planceFunc[0], planceFunc[1], planceFunc[2]);
|
||||
//反射进行校正
|
||||
eulerPra.roll = eulerPra.roll;
|
||||
eulerPra.pitch = eulerPra.pitch;
|
||||
eulerPra.yaw = eulerPra.yaw;
|
||||
RotationMatrix rMatrix = eulerToRotationMatrix(eulerPra.yaw, eulerPra.pitch, eulerPra.roll);
|
||||
#endif
|
||||
|
||||
planePara.planeCalib[0] = rMatrix.data[0][0];
|
||||
planePara.planeCalib[1] = rMatrix.data[0][1];
|
||||
planePara.planeCalib[2] = rMatrix.data[0][2];
|
||||
planePara.planeCalib[3] = rMatrix.data[1][0];
|
||||
planePara.planeCalib[4] = rMatrix.data[1][1];
|
||||
planePara.planeCalib[5] = rMatrix.data[1][2];
|
||||
planePara.planeCalib[6] = rMatrix.data[2][0];
|
||||
planePara.planeCalib[7] = rMatrix.data[2][1];
|
||||
planePara.planeCalib[8] = rMatrix.data[2][2];
|
||||
|
||||
planePara.invRMatrix[0] = invMatrix.data[0][0];
|
||||
planePara.invRMatrix[1] = invMatrix.data[0][1];
|
||||
planePara.invRMatrix[2] = invMatrix.data[0][2];
|
||||
planePara.invRMatrix[3] = invMatrix.data[1][0];
|
||||
planePara.invRMatrix[4] = invMatrix.data[1][1];
|
||||
planePara.invRMatrix[5] = invMatrix.data[1][2];
|
||||
planePara.invRMatrix[6] = invMatrix.data[2][0];
|
||||
planePara.invRMatrix[7] = invMatrix.data[2][1];
|
||||
planePara.invRMatrix[8] = invMatrix.data[2][2];
|
||||
|
||||
#if 0 //test: 两个矩阵的乘积必须是单位阵
|
||||
double testMatrix[3][3];
|
||||
for (int i = 0; i < 3; i++)
|
||||
{
|
||||
for (int j = 0; j < 3; j++)
|
||||
{
|
||||
testMatrix[i][j] = 0;
|
||||
for (int m = 0; m < 3; m++)
|
||||
testMatrix[i][j] += invMatrix.data[i][m] * rMatrix.data[m][j];
|
||||
}
|
||||
}
|
||||
#endif
|
||||
//数据进行转换
|
||||
SVzNLRangeD calibZRange = { 0, -1 };
|
||||
SVzNLRangeD topZRange = { 0, -1 };
|
||||
double sumMeanZ = 0;
|
||||
int sumSize = 0;
|
||||
for (int i = 0, i_max = (int)Points3ds.size(); i < i_max; i++)
|
||||
{
|
||||
//z
|
||||
if (topZRange.max < topZRange.min)
|
||||
{
|
||||
topZRange.min = Points3ds[i].z;
|
||||
topZRange.max = Points3ds[i].z;
|
||||
}
|
||||
else
|
||||
{
|
||||
if (topZRange.min > Points3ds[i].z)
|
||||
topZRange.min = Points3ds[i].z;
|
||||
if (topZRange.max < Points3ds[i].z)
|
||||
topZRange.max = Points3ds[i].z;
|
||||
}
|
||||
cv::Point3f a_calibPt;
|
||||
a_calibPt.x = (float)(Points3ds[i].x * planePara.planeCalib[0] + Points3ds[i].y * planePara.planeCalib[1] + Points3ds[i].z * planePara.planeCalib[2]);
|
||||
a_calibPt.y = (float)(Points3ds[i].x * planePara.planeCalib[3] + Points3ds[i].y * planePara.planeCalib[4] + Points3ds[i].z * planePara.planeCalib[5]);
|
||||
a_calibPt.z = (float)(Points3ds[i].x * planePara.planeCalib[6] + Points3ds[i].y * planePara.planeCalib[7] + Points3ds[i].z * planePara.planeCalib[8]);
|
||||
//z
|
||||
if (calibZRange.max < calibZRange.min)
|
||||
{
|
||||
calibZRange.min = a_calibPt.z;
|
||||
calibZRange.max = a_calibPt.z;
|
||||
sumMeanZ += a_calibPt.z;
|
||||
sumSize++;
|
||||
}
|
||||
else
|
||||
{
|
||||
if (calibZRange.min > a_calibPt.z)
|
||||
calibZRange.min = a_calibPt.z;
|
||||
if (calibZRange.max < a_calibPt.z)
|
||||
calibZRange.max = a_calibPt.z;
|
||||
sumMeanZ += a_calibPt.z;
|
||||
sumSize++;
|
||||
}
|
||||
}
|
||||
if (sumSize > 0)
|
||||
sumMeanZ = sumMeanZ / (double)sumSize;
|
||||
planePara.planeHeight = sumMeanZ; // calibZRange.min;
|
||||
|
||||
return planePara;
|
||||
}
|
||||
|
||||
//水平安装相机垂直扫描模式地面调平
|
||||
SSG_planeCalibPara sg_HCameraVScan_getGroundCalibPara(
|
||||
std::vector< std::vector<SVzNL3DPosition>>& scanLines)
|
||||
@ -2826,7 +2981,7 @@ void pointRT(const cv::Mat& R, const cv::Mat& T,
|
||||
Eigen::Vector3d result = _R * (vec_pt - vec_origin) + vec_rtOrigin;
|
||||
|
||||
rtPt.x = result(0);
|
||||
rtPt.x = result(1);
|
||||
rtPt.x = result(2);
|
||||
rtPt.y = result(1);
|
||||
rtPt.z = result(2);
|
||||
return;
|
||||
}
|
||||
|
||||
@ -732,6 +732,112 @@ void sg_lineFeaturesGrowing(
|
||||
}
|
||||
}
|
||||
|
||||
//将feature在trees上寻找合适的生长点进行生长。如果没有合适的生长点， 返回false
|
||||
//没有使用全匹配。一个feature一旦被匹配上，匹配就完成。没有使用最佳匹配。
|
||||
bool _gapGrowing(
|
||||
SSG_basicFeatureGap& a_gap,
|
||||
const int lineIdx,
|
||||
std::vector<SSG_gapFeatureTree>& trees,
|
||||
SSG_treeGrowParam growParam)
|
||||
{
|
||||
for (int i = 0, i_max = (int)trees.size(); i < i_max; i++)
|
||||
{
|
||||
SSG_gapFeatureTree& a_tree = trees[i];
|
||||
if (TREE_STATE_DEAD == a_tree.treeState)
|
||||
continue;
|
||||
//检查生长点
|
||||
SSG_basicFeatureGap last_node = a_tree.treeNodes.back();
|
||||
if (last_node.lineIdx == a_gap.lineIdx) //x为lineIdx，同一条扫描线上的不进行生长
|
||||
continue;
|
||||
|
||||
double last_cx = (last_node.gapPt_0.pt3D.x + last_node.gapPt_1.pt3D.x) / 2;
|
||||
double last_cy = (last_node.gapPt_0.pt3D.y + last_node.gapPt_1.pt3D.y) / 2;
|
||||
double last_cz = (last_node.gapPt_0.pt3D.z + last_node.gapPt_1.pt3D.z) / 2;
|
||||
//判断生长点
|
||||
double curr_cx = (a_gap.gapPt_0.pt3D.x + a_gap.gapPt_1.pt3D.x) / 2;
|
||||
double curr_cy = (a_gap.gapPt_0.pt3D.y + a_gap.gapPt_1.pt3D.y) / 2;
|
||||
double curr_cz = (a_gap.gapPt_0.pt3D.z + a_gap.gapPt_1.pt3D.z) / 2;
|
||||
double y_diff = abs(curr_cy - last_cy);
|
||||
double z_diff = abs(curr_cz - last_cz);
|
||||
int line_diff = abs(a_gap.lineIdx - last_node.lineIdx);
|
||||
double x_diff = abs(curr_cx - last_cx);
|
||||
if ((y_diff < growParam.yDeviation_max) && (z_diff < growParam.zDeviation_max) &&
|
||||
((line_diff < growParam.maxLineSkipNum) || (x_diff < growParam.maxSkipDistance)) )
|
||||
{
|
||||
a_tree.eLineIdx = lineIdx;
|
||||
a_tree.treeNodes.push_back(a_gap);
|
||||
return true;
|
||||
}
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
|
||||
//对于Gap生长树，如果是单调上升或单调下降，且最高点没有继续生长的去除（褶皱)
|
||||
bool _invalidateGapTrees(SSG_gapFeatureTree& a_tree, double minLTypeTreeLen, double minVTypeTreeLen)
|
||||
{
|
||||
SSG_basicFeatureGap& first_node = a_tree.treeNodes[0];
|
||||
double first_cx = (first_node.gapPt_0.pt3D.x + first_node.gapPt_1.pt3D.x) / 2;
|
||||
double first_cy = (first_node.gapPt_0.pt3D.y + first_node.gapPt_1.pt3D.y) / 2;
|
||||
SSG_basicFeatureGap& last_node = a_tree.treeNodes[a_tree.treeNodes.size() - 1];
|
||||
double last_cx = (last_node.gapPt_0.pt3D.x + last_node.gapPt_1.pt3D.x) / 2;
|
||||
double last_cy = (last_node.gapPt_0.pt3D.y + last_node.gapPt_1.pt3D.y) / 2;
|
||||
double len = sqrt(pow(first_cx - last_cx, 2) + pow(first_cy - last_cy, 2));
|
||||
if (len <= minLTypeTreeLen)
|
||||
return false;
|
||||
else
|
||||
return true;
|
||||
}
|
||||
|
||||
void sg_lineGapsGrowing(
|
||||
int lineIdx,
|
||||
bool isLastLine,
|
||||
std::vector<SSG_basicFeatureGap>& features,
|
||||
std::vector<SSG_gapFeatureTree>& trees,
|
||||
SSG_treeGrowParam growParam)
|
||||
{
|
||||
if (features.size() > 0)
|
||||
{
|
||||
for (int j = 0, j_max = (int)features.size(); j < j_max; j++)
|
||||
{
|
||||
SSG_basicFeatureGap& a_feature = features[j];
|
||||
if (a_feature.lineIdx == 207)
|
||||
int kkk = 1;
|
||||
bool isMatched = _gapGrowing(a_feature, lineIdx, trees, growParam);
|
||||
if (false == isMatched)
|
||||
{
|
||||
//新的生长树
|
||||
SSG_gapFeatureTree a_newTree;
|
||||
a_newTree.treeNodes.push_back(a_feature);
|
||||
a_newTree.treeState = TREE_STATE_ALIVE;
|
||||
a_newTree.treeType = 1;
|
||||
a_newTree.sLineIdx = lineIdx;
|
||||
a_newTree.eLineIdx = lineIdx;
|
||||
a_newTree.tree_value = 0;
|
||||
trees.push_back(a_newTree);
|
||||
}
|
||||
}
|
||||
}
|
||||
//检查停止生长的树,加速。
|
||||
//将生长节点为1的生长树移除
|
||||
int m_max = (int)trees.size();
|
||||
for (int m = m_max - 1; m >= 0; m--) //从后往前，这样删除不会影响循环
|
||||
{
|
||||
if (TREE_STATE_ALIVE == trees[m].treeState)
|
||||
{
|
||||
int line_diff = abs(lineIdx - trees[m].treeNodes.back().lineIdx);
|
||||
if (((growParam.maxLineSkipNum > 0) && (line_diff > growParam.maxLineSkipNum)) ||
|
||||
(true == isLastLine))
|
||||
{
|
||||
trees[m].treeState = TREE_STATE_DEAD;
|
||||
bool isValid = _invalidateGapTrees(trees[m], growParam.minLTypeTreeLen, growParam.minVTypeTreeLen);
|
||||
if (false == isValid)
|
||||
trees.erase(trees.begin() + m);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void sg_getEndingGrowingTrees(
|
||||
std::vector<SSG_2DValueI>& lineEndings,
|
||||
SVzNL3DLaserLine* laser3DPoints,
|
||||
|
||||
File diff suppressed because it is too large
Load Diff
371
sourceCode/channelSpaceMeasure.cpp
Normal file
371
sourceCode/channelSpaceMeasure.cpp
Normal file
@ -0,0 +1,371 @@
|
||||
#include <vector>
|
||||
#include "SG_baseDataType.h"
|
||||
#include "SG_baseAlgo_Export.h"
|
||||
#include "channelSpaceMeasure_Export.h"
|
||||
#include <opencv2/opencv.hpp>
|
||||
#include <limits>
|
||||
|
||||
//version 1.0.0 : base version release to customer
|
||||
std::string m_strVersion = "1.0.0";
|
||||
const char* wd_ChannelSPaceMeasureVersion(void)
|
||||
{
|
||||
return m_strVersion.c_str();
|
||||
}
|
||||
|
||||
|
||||
//逆时针旋转时 θ > 0 ；顺时针旋转时 θ < 0
|
||||
SVzNL3DPoint _rotate2D(SVzNL3DPoint pt, double sinTheta, double cosTheta)
|
||||
{
|
||||
SVzNL3DPoint rotatePt;
|
||||
rotatePt.x = pt.x * cosTheta - pt.y * sinTheta;
|
||||
rotatePt.y = pt.x * sinTheta + pt.y * cosTheta;
|
||||
rotatePt.z = pt.z;
|
||||
return rotatePt;
|
||||
}
|
||||
|
||||
void _XY_rotateLine(double angle, std::vector<SSG_basicFeatureGap>& line_src, std::vector<SSG_basicFeatureGap>& rotate_dst)
|
||||
{
|
||||
rotate_dst.resize(line_src.size());
|
||||
double sinTheta = sin(PI * angle / 180);
|
||||
double cosTheta = cos(PI * angle / 180);
|
||||
for (int i = 0, i_max = (int)line_src.size(); i < i_max; i++)
|
||||
{
|
||||
rotate_dst[i] = line_src[i];
|
||||
rotate_dst[i].gapPt_0.pt3D = _rotate2D(line_src[i].gapPt_0.pt3D, sinTheta, cosTheta);
|
||||
rotate_dst[i].gapPt_1.pt3D = _rotate2D(line_src[i].gapPt_1.pt3D, sinTheta, cosTheta);
|
||||
}
|
||||
return;
|
||||
}
|
||||
|
||||
double _computeChannelSpace(
|
||||
std::vector<SSG_basicFeatureGap>& line_1,
|
||||
std::vector<SSG_basicFeatureGap>& line_2)
|
||||
{
|
||||
double meanSpace = 0;
|
||||
int dataSize = (int)line_1.size();
|
||||
if (dataSize == 0)
|
||||
return 0;
|
||||
for (int i = 0; i < dataSize; i++)
|
||||
{
|
||||
double cy_1 = (line_1[i].gapPt_0.pt3D.y + line_1[i].gapPt_1.pt3D.y) / 2;
|
||||
double cy_2 = (line_2[i].gapPt_0.pt3D.y + line_2[i].gapPt_1.pt3D.y) / 2;
|
||||
meanSpace += abs(cy_2 - cy_1);
|
||||
}
|
||||
meanSpace = meanSpace / dataSize;
|
||||
return meanSpace;
|
||||
}
|
||||
|
||||
double _computeGapMeanWidth(std::vector<SSG_basicFeatureGap>& gap_data)
|
||||
{
|
||||
double meanWidth = 0;
|
||||
int dataSize = (int)gap_data.size();
|
||||
if (dataSize == 0)
|
||||
return 0;
|
||||
for (int i = 0; i < dataSize; i++)
|
||||
{
|
||||
double width = abs(gap_data[i].gapPt_0.pt3D.y - gap_data[i].gapPt_1.pt3D.y);
|
||||
meanWidth += width;
|
||||
}
|
||||
meanWidth = meanWidth / dataSize;
|
||||
return meanWidth;
|
||||
}
|
||||
|
||||
double _computeGapMeanDepth(std::vector<SSG_basicFeatureGap>& gap_data, std::vector< std::vector<SVzNL3DPosition>>& scanLines)
|
||||
{
|
||||
double meanDepth = 0;
|
||||
int dataSize = (int)gap_data.size();
|
||||
if (dataSize == 0)
|
||||
return 0;
|
||||
|
||||
for (int i = 0; i < dataSize; i++)
|
||||
{
|
||||
int ptIdx_1 = gap_data[i].gapPt_0.nPointIdx;
|
||||
int ptIdx_2 = gap_data[i].gapPt_1.nPointIdx;
|
||||
int line = gap_data[i].lineIdx;
|
||||
double max_z = 0;
|
||||
for (int j = ptIdx_1; j <= ptIdx_2; j++)
|
||||
{
|
||||
if (scanLines[line][j].pt3D.z > 1e-4)
|
||||
{
|
||||
if (max_z < scanLines[line][j].pt3D.z)
|
||||
max_z = scanLines[line][j].pt3D.z;
|
||||
}
|
||||
}
|
||||
double depth = max_z - (gap_data[i].gapPt_0.pt3D.z + gap_data[i].gapPt_1.pt3D.z) / 2;
|
||||
meanDepth += depth;
|
||||
}
|
||||
meanDepth = meanDepth / dataSize;
|
||||
return meanDepth;
|
||||
}
|
||||
|
||||
SSX_channelInfo sx_channelSpaceMeasure(
|
||||
std::vector< std::vector<SVzNL3DPosition>>& scanLines,
|
||||
bool isHorizonScan, //true:激光线平行槽道；false:激光线垂直槽道
|
||||
const SSG_cornerParam cornerPara,
|
||||
const SSG_outlierFilterParam filterParam,
|
||||
const SSG_treeGrowParam growParam,
|
||||
const SSX_channelParam channelParam,
|
||||
int* errCode)
|
||||
{
|
||||
*errCode = 0;
|
||||
SSX_channelInfo channelInfo;
|
||||
memset(&channelInfo, 0, sizeof(SSX_channelInfo));
|
||||
int lineNum = (int)scanLines.size();
|
||||
if (lineNum == 0)
|
||||
{
|
||||
*errCode = SG_ERR_3D_DATA_NULL;
|
||||
return channelInfo;
|
||||
}
|
||||
|
||||
int linePtNum = (int)scanLines[0].size();
|
||||
|
||||
//判断数据格式是否为grid。算法只能处理grid数据格式
|
||||
bool isGridData = true;
|
||||
for (int line = 0; line < lineNum; line++)
|
||||
{
|
||||
if (linePtNum != (int)scanLines[line].size())
|
||||
{
|
||||
isGridData = false;
|
||||
break;
|
||||
}
|
||||
}
|
||||
if (false == isGridData)//数据不是网格格式
|
||||
{
|
||||
*errCode = SG_ERR_NOT_GRID_FORMAT;
|
||||
return channelInfo;
|
||||
}
|
||||
|
||||
std::vector< std::vector<SVzNL3DPosition>> data_lines;
|
||||
if (false == isHorizonScan)
|
||||
{
|
||||
data_lines.resize(lineNum);
|
||||
for (int line = 0; line < lineNum; line++)
|
||||
{
|
||||
data_lines[line].insert(data_lines[line].end(), scanLines[line].begin(), scanLines[line].end());
|
||||
for (int j = 0, j_max = (int)data_lines[line].size(); j < j_max; j++)
|
||||
{
|
||||
data_lines[line][j].nPointIdx = j;
|
||||
scanLines[line][j].nPointIdx = 0; //转义复用
|
||||
}
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
data_lines.resize(linePtNum);
|
||||
for (int i = 0; i < linePtNum; i++)
|
||||
data_lines[i].resize(lineNum);
|
||||
for (int line = 0; line < lineNum; line++)
|
||||
{
|
||||
for (int j = 0; j < linePtNum; j++)
|
||||
{
|
||||
scanLines[line][j].nPointIdx = 0; //将原始数据的序列清0（会转义使用）
|
||||
data_lines[j][line] = scanLines[line][j];
|
||||
data_lines[j][line].pt3D.x = scanLines[line][j].pt3D.y;
|
||||
data_lines[j][line].pt3D.y = scanLines[line][j].pt3D.x;
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
lineNum = linePtNum;
|
||||
linePtNum = (int)data_lines[0].size();
|
||||
for (int line = 0; line < lineNum; line++)
|
||||
{
|
||||
for (int j = 0, j_max = (int)data_lines[line].size(); j < j_max; j++)
|
||||
data_lines[line][j].nPointIdx = j;
|
||||
}
|
||||
}
|
||||
|
||||
std::vector<std::vector<SSG_basicFeatureGap>> gapFeatures;
|
||||
for (int line = 0; line < lineNum; line++)
|
||||
{
|
||||
if (line == 44)
|
||||
int kkk = 1;
|
||||
std::vector<SVzNL3DPosition>& lineData = data_lines[line];
|
||||
|
||||
//滤波，滤除异常点
|
||||
sg_lineDataRemoveOutlier_changeOriginData(&lineData[0], linePtNum, filterParam);
|
||||
std::vector<SSG_basicFeatureGap> line_gaps;
|
||||
int dataSize = (int)lineData.size();
|
||||
//提取Gap特征
|
||||
sg_getLineGapFeature(
|
||||
lineData, //扫描线
|
||||
line, //当前扫描线序号
|
||||
cornerPara,
|
||||
channelParam.channelWidthRng,
|
||||
line_gaps);
|
||||
gapFeatures.push_back(line_gaps);
|
||||
}
|
||||
//特征生长
|
||||
std::vector<SSG_gapFeatureTree> growTrees;
|
||||
for (int line = 0; line < lineNum; line++)
|
||||
{
|
||||
bool isLastLine = false;
|
||||
if (line == lineNum - 1)
|
||||
isLastLine = true;
|
||||
std::vector<SSG_basicFeatureGap>& line_gapFeatures = gapFeatures[line];
|
||||
|
||||
sg_lineGapsGrowing(
|
||||
line,
|
||||
isLastLine,
|
||||
line_gapFeatures,
|
||||
growTrees,
|
||||
growParam);
|
||||
}
|
||||
if (growTrees.size() != 2)
|
||||
{
|
||||
*errCode = SG_ERR_NOT_GRID_FORMAT;
|
||||
return channelInfo;
|
||||
}
|
||||
|
||||
std::vector<SSG_basicFeatureGap>& nodes_1 = growTrees[0].treeNodes;
|
||||
std::vector<SSG_basicFeatureGap>& nodes_2 = growTrees[1].treeNodes;
|
||||
#if _OUTPUT_DEBUG_DATA
|
||||
for (int i = 0, i_max = (int)nodes_1.size(); i < i_max; i++)
|
||||
{
|
||||
int lineIdx, ptIdx;
|
||||
if (false == isHorizonScan)
|
||||
{
|
||||
lineIdx = nodes_1[i].lineIdx;
|
||||
ptIdx = nodes_1[i].gapPt_0.nPointIdx;
|
||||
scanLines[lineIdx][ptIdx].nPointIdx = 1;
|
||||
ptIdx = nodes_1[i].gapPt_1.nPointIdx;
|
||||
scanLines[lineIdx][ptIdx].nPointIdx = 2;
|
||||
}
|
||||
else
|
||||
{
|
||||
ptIdx = nodes_1[i].lineIdx;
|
||||
lineIdx = nodes_1[i].gapPt_0.nPointIdx;
|
||||
scanLines[lineIdx][ptIdx].nPointIdx = 1;
|
||||
lineIdx = nodes_1[i].gapPt_1.nPointIdx;
|
||||
scanLines[lineIdx][ptIdx].nPointIdx = 2;
|
||||
}
|
||||
}
|
||||
for (int i = 0, i_max = (int)nodes_2.size(); i < i_max; i++)
|
||||
{
|
||||
int lineIdx, ptIdx;
|
||||
if (false == isHorizonScan)
|
||||
{
|
||||
lineIdx = nodes_2[i].lineIdx;
|
||||
ptIdx = nodes_2[i].gapPt_0.nPointIdx;
|
||||
scanLines[lineIdx][ptIdx].nPointIdx = 1;
|
||||
ptIdx = nodes_2[i].gapPt_1.nPointIdx;
|
||||
scanLines[lineIdx][ptIdx].nPointIdx = 2;
|
||||
}
|
||||
else
|
||||
{
|
||||
ptIdx = nodes_2[i].lineIdx;
|
||||
lineIdx = nodes_2[i].gapPt_0.nPointIdx;
|
||||
scanLines[lineIdx][ptIdx].nPointIdx = 1;
|
||||
lineIdx = nodes_2[i].gapPt_1.nPointIdx;
|
||||
scanLines[lineIdx][ptIdx].nPointIdx = 2;
|
||||
}
|
||||
}
|
||||
#endif
|
||||
|
||||
//按扫描线配对
|
||||
std::vector<SSG_basicFeatureGap> line_1; //line_1和line_2为按扫描线对应的两个槽道上的Gap特征
|
||||
std::vector<SSG_basicFeatureGap> line_2;
|
||||
int i_idx = 0;
|
||||
int j_idx = 0;
|
||||
while (1)
|
||||
{
|
||||
int line_idx1 = nodes_1[i_idx].lineIdx;
|
||||
int line_idx2 = nodes_2[j_idx].lineIdx;
|
||||
if (line_idx1 == line_idx2)
|
||||
{
|
||||
line_1.push_back(nodes_1[i_idx]);
|
||||
line_2.push_back(nodes_2[j_idx]);
|
||||
i_idx++;
|
||||
j_idx++;
|
||||
}
|
||||
else
|
||||
{
|
||||
if (line_idx1 < line_idx2)
|
||||
i_idx++;
|
||||
else
|
||||
j_idx++;
|
||||
}
|
||||
if( (i_idx >= (int)nodes_1.size()) || (j_idx >= (int)nodes_2.size()))
|
||||
break;
|
||||
}
|
||||
|
||||
#if _OUTPUT_DEBUG_DATA
|
||||
for (int i = 0, i_max = (int)line_1.size(); i < i_max; i++)
|
||||
{
|
||||
int lineIdx, ptIdx;
|
||||
if (false == isHorizonScan)
|
||||
{
|
||||
lineIdx = line_1[i].lineIdx;
|
||||
ptIdx = line_1[i].gapPt_0.nPointIdx;
|
||||
scanLines[lineIdx][ptIdx].nPointIdx = 3;
|
||||
ptIdx = line_1[i].gapPt_1.nPointIdx;
|
||||
scanLines[lineIdx][ptIdx].nPointIdx = 4;
|
||||
}
|
||||
else
|
||||
{
|
||||
ptIdx = line_1[i].lineIdx;
|
||||
lineIdx = line_1[i].gapPt_0.nPointIdx;
|
||||
scanLines[lineIdx][ptIdx].nPointIdx = 3;
|
||||
lineIdx = line_1[i].gapPt_1.nPointIdx;
|
||||
scanLines[lineIdx][ptIdx].nPointIdx = 4;
|
||||
}
|
||||
}
|
||||
for (int i = 0, i_max = (int)line_2.size(); i < i_max; i++)
|
||||
{
|
||||
int lineIdx, ptIdx;
|
||||
if (false == isHorizonScan)
|
||||
{
|
||||
lineIdx = line_2[i].lineIdx;
|
||||
ptIdx = line_2[i].gapPt_0.nPointIdx;
|
||||
scanLines[lineIdx][ptIdx].nPointIdx = 3;
|
||||
ptIdx = line_2[i].gapPt_1.nPointIdx;
|
||||
scanLines[lineIdx][ptIdx].nPointIdx = 4;
|
||||
}
|
||||
else
|
||||
{
|
||||
ptIdx = line_2[i].lineIdx;
|
||||
lineIdx = line_2[i].gapPt_0.nPointIdx;
|
||||
scanLines[lineIdx][ptIdx].nPointIdx = 3;
|
||||
lineIdx = line_2[i].gapPt_1.nPointIdx;
|
||||
scanLines[lineIdx][ptIdx].nPointIdx = 4;
|
||||
}
|
||||
}
|
||||
#endif
|
||||
|
||||
//旋转，保证扫描线与Gap垂直
|
||||
double angleSearchWin = 30;
|
||||
double angleStepping = 0.1;
|
||||
|
||||
int loop = (int)(angleSearchWin / angleStepping);
|
||||
double bestSpace = 0;
|
||||
double rotateAngle = 0;
|
||||
for (int i = -loop; i <= loop; i++)
|
||||
{
|
||||
double angle = i * angleStepping;
|
||||
std::vector<SSG_basicFeatureGap> rotate_line_1;
|
||||
_XY_rotateLine(angle, line_1, rotate_line_1);
|
||||
std::vector<SSG_basicFeatureGap> rotate_line_2;
|
||||
_XY_rotateLine(angle, line_2, rotate_line_2);
|
||||
double meanSpace = _computeChannelSpace(rotate_line_1, rotate_line_2);
|
||||
if (bestSpace < meanSpace)
|
||||
{
|
||||
bestSpace = meanSpace;
|
||||
rotateAngle = angle;
|
||||
}
|
||||
}
|
||||
std::vector<SSG_basicFeatureGap> calib_line_1;
|
||||
_XY_rotateLine(rotateAngle, line_1, calib_line_1);
|
||||
std::vector<SSG_basicFeatureGap> calib_line_2;
|
||||
_XY_rotateLine(rotateAngle, line_2, calib_line_2);
|
||||
|
||||
channelInfo.channelSpace = _computeChannelSpace(calib_line_1, calib_line_2);
|
||||
channelInfo.channelWidth[0] = _computeGapMeanWidth(calib_line_1);
|
||||
channelInfo.channelWidth[1] = _computeGapMeanWidth(calib_line_2);
|
||||
channelInfo.channelDepth[0] = _computeGapMeanDepth(calib_line_1, data_lines);
|
||||
channelInfo.channelDepth[1] = _computeGapMeanDepth(calib_line_2, data_lines);
|
||||
return channelInfo;
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
32
sourceCode/channelSpaceMeasure_Export.h
Normal file
32
sourceCode/channelSpaceMeasure_Export.h
Normal file
@ -0,0 +1,32 @@
|
||||
#pragma once
|
||||
|
||||
#include "SG_algo_Export.h"
|
||||
#include <vector>
|
||||
|
||||
#define _OUTPUT_DEBUG_DATA 1
|
||||
|
||||
typedef struct
|
||||
{
|
||||
SVzNLRangeD channelWidthRng;
|
||||
SVzNLRangeD channleSpaceRng;
|
||||
}SSX_channelParam;
|
||||
|
||||
typedef struct
|
||||
{
|
||||
double channelSpace;
|
||||
double channelWidth[2];
|
||||
double channelDepth[2];
|
||||
}SSX_channelInfo;
|
||||
|
||||
//读版本号
|
||||
SG_APISHARED_EXPORT const char* wd_ChannelSPaceMeasureVersion(void);
|
||||
|
||||
//提取槽道宽度深度和间距
|
||||
SG_APISHARED_EXPORT SSX_channelInfo sx_channelSpaceMeasure(
|
||||
std::vector< std::vector<SVzNL3DPosition>>& scanLines,
|
||||
bool isHorizonScan, //true:激光线平行槽道；false:激光线垂直槽道
|
||||
const SSG_cornerParam cornerPara,
|
||||
const SSG_outlierFilterParam filterParam,
|
||||
const SSG_treeGrowParam growParam,
|
||||
const SSX_channelParam channelParam,
|
||||
int* errCode);
|
||||
@ -192,7 +192,7 @@ SSG_6DOF wd_getGasFillingPortPosition(
|
||||
SVzNL2DPoint a_seedPos = {x, y};
|
||||
std::vector< SVzNL2DPoint> a_cluster;
|
||||
a_cluster.push_back(a_seedPos);
|
||||
wd_pointClustering2D(
|
||||
wd_gridPointClustering(
|
||||
featureInfoMask,//int，记录特征标记和clusterID，附加一个flag
|
||||
feature3DInfo,//double,记录坐标信息
|
||||
clusterCheckWin, //搜索窗口
|
||||
|
||||
@ -33,6 +33,12 @@ typedef struct
|
||||
int neighborID[6];
|
||||
}SSG_hexagonNeighbour;
|
||||
|
||||
typedef struct
|
||||
{
|
||||
cv::RotatedRect fittingPara;
|
||||
SVzNL3DPoint objCenter;
|
||||
}SG_fittingInfo;
|
||||
|
||||
std::string m_strVersion = "1.0.0";
|
||||
const char* wd_particleSegVersion(void)
|
||||
{
|
||||
@ -353,11 +359,6 @@ bool compareByNeighbourNumber(const SSG_hexagonNeighbour& a, const SSG_hexagonNe
|
||||
{
|
||||
return a.linkNum < b.linkNum;
|
||||
}
|
||||
typedef struct
|
||||
{
|
||||
cv::RotatedRect fittingPara;
|
||||
SVzNL3DPoint objCenter;
|
||||
}SG_fittingInfo;
|
||||
|
||||
//圆周扫描定子环
|
||||
//以scanCenter为扫描中心，圆周扫描从scanR1到scanR2的圆形区域
|
||||
@ -602,7 +603,180 @@ bool computeOuterGraspPoint(
|
||||
return false;
|
||||
}
|
||||
|
||||
//切割
|
||||
void cloudPointsFilter_zRange(std::vector< std::vector<SVzNL3DPosition>>& scanLines, SVzNLRangeD zRange, std::vector< std::vector<SVzNL3DPosition>>& cutData)
|
||||
{
|
||||
int lineNum = (int)scanLines.size();
|
||||
cutData.resize(lineNum);
|
||||
for (int line = 0; line < lineNum; line++)
|
||||
{
|
||||
int ptNum = (int)scanLines[line].size();
|
||||
cutData[line].resize(ptNum);
|
||||
for (int i = 0; i < ptNum; i++)
|
||||
{
|
||||
SVzNL3DPosition a_pt = scanLines[line][i];
|
||||
if ((a_pt.pt3D.z < zRange.min) || (a_pt.pt3D.z > zRange.max))
|
||||
a_pt.pt3D.z = 0;
|
||||
cutData[line][i] = a_pt;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
//对无点区聚类
|
||||
void genCLusterMask(
|
||||
std::vector< std::vector<SVzNL3DPosition>>& clusterSrcData,
|
||||
bool maskType_null_vldPt, //true: nullPt(z=0); false:valid Point
|
||||
std::vector<std::vector<SSG_featureClusteringInfo>>& featureInfoMask,
|
||||
std::vector<std::vector<SVzNL3DPoint>>& feature3DInfo)
|
||||
{
|
||||
int lineNum = (int)clusterSrcData.size();
|
||||
if (lineNum == 0)
|
||||
return;
|
||||
int linePtNum = (int)clusterSrcData[0].size();
|
||||
|
||||
featureInfoMask.resize(lineNum);
|
||||
feature3DInfo.resize(lineNum);
|
||||
for (int i = 0; i < lineNum; i++)
|
||||
{
|
||||
featureInfoMask[i].resize(linePtNum);
|
||||
feature3DInfo[i].resize(linePtNum);
|
||||
}
|
||||
//生成Mask
|
||||
for (int line = 0; line < lineNum; line++)
|
||||
{
|
||||
std::vector<SVzNL3DPosition>& lineData = clusterSrcData[line];
|
||||
for (int ptIdx = 0; ptIdx < linePtNum; ptIdx++)
|
||||
{
|
||||
if( ((false == maskType_null_vldPt)&&(clusterSrcData[line][ptIdx].pt3D.z > 1e-4))||
|
||||
((true == maskType_null_vldPt) && (clusterSrcData[line][ptIdx].pt3D.z < 1e-4)))
|
||||
{
|
||||
SSG_featureClusteringInfo a_mask;
|
||||
memset(&a_mask, 0, sizeof(SSG_featureClusteringInfo));
|
||||
a_mask.featurType = 1;
|
||||
a_mask.lineIdx = line;
|
||||
a_mask.ptIdx = ptIdx;
|
||||
featureInfoMask[line][ptIdx] = a_mask;
|
||||
feature3DInfo[line][ptIdx] = clusterSrcData[line][ptIdx].pt3D;
|
||||
}
|
||||
}
|
||||
}
|
||||
return;
|
||||
}
|
||||
|
||||
//聚类:对无效点的聚类实际上是连通域处理
|
||||
void cloutPointsClustering_nullPt(
|
||||
std::vector< std::vector<SVzNL3DPosition>>& clusterSrcData,
|
||||
std::vector<std::vector<SSG_featureClusteringInfo>>& featureInfoMask,
|
||||
std::vector<std::vector<SVzNL3DPoint>>& feature3DInfo,
|
||||
std::vector<std::vector< SVzNL2DPoint>>& clusters, //只记录位置
|
||||
std::vector<SWD_clustersInfo>& clustersInfo)
|
||||
{
|
||||
int lineNum = (int)clusterSrcData.size();
|
||||
if (lineNum == 0)
|
||||
return;
|
||||
int linePtNum = (int)clusterSrcData[0].size();
|
||||
//采用迭代思想，回归思路进行高效聚类
|
||||
int clusterID = 1;
|
||||
int clusterCheckWin = 5;
|
||||
for (int y = 0; y < linePtNum; y++)
|
||||
{
|
||||
for (int x = 0; x < lineNum; x++)
|
||||
{
|
||||
SSG_featureClusteringInfo& a_featureInfo = featureInfoMask[x][y];
|
||||
if ((0 == a_featureInfo.featurType) || (a_featureInfo.clusterID > 0)) //非特征或已经处理
|
||||
continue;
|
||||
|
||||
SVzNL3DPoint& a_feature3DValue = feature3DInfo[x][y];
|
||||
SVzNL3DRangeD a_clusterRoi3D;
|
||||
memset(&a_clusterRoi3D, 0, sizeof(SVzNL3DRangeD));
|
||||
SVzNLRect clusterRoi2D;
|
||||
clusterRoi2D.left = x;
|
||||
clusterRoi2D.right = x;
|
||||
clusterRoi2D.top = y;
|
||||
clusterRoi2D.bottom = y;
|
||||
|
||||
SVzNL2DPoint a_seedPos = { x, y };
|
||||
std::vector< SVzNL2DPoint> a_cluster;
|
||||
a_cluster.push_back(a_seedPos);
|
||||
|
||||
//使用聚类方法完成8连通连通域分析
|
||||
wd_gridPointClustering_labelling(
|
||||
featureInfoMask,
|
||||
feature3DInfo,
|
||||
clusterID, //当前Cluster的ID
|
||||
a_cluster, //result
|
||||
clusterRoi2D //roi2D
|
||||
);
|
||||
clusters.push_back(a_cluster);
|
||||
SWD_clustersInfo a_info;
|
||||
a_info.clusterIdx = clusterID;
|
||||
a_info.ptSize = (int)a_cluster.size();
|
||||
a_info.roi2D = clusterRoi2D;
|
||||
a_info.roi3D = a_clusterRoi3D;
|
||||
clustersInfo.push_back(a_info);
|
||||
clusterID++;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
//聚类
|
||||
void cloutPointsClustering(
|
||||
std::vector< std::vector<SVzNL3DPosition>>& clusterSrcData,
|
||||
std::vector<std::vector<SSG_featureClusteringInfo>>& featureInfoMask,
|
||||
std::vector<std::vector<SVzNL3DPoint>>& feature3DInfo,
|
||||
SSG_treeGrowParam growParam,
|
||||
std::vector<std::vector< SVzNL2DPoint>>& clusters, //只记录位置
|
||||
std::vector<SWD_clustersInfo>& clustersInfo)
|
||||
{
|
||||
int lineNum = (int)clusterSrcData.size();
|
||||
if (lineNum == 0)
|
||||
return;
|
||||
int linePtNum = (int)clusterSrcData[0].size();
|
||||
//采用迭代思想，回归思路进行高效聚类
|
||||
int clusterID = 1;
|
||||
int clusterCheckWin = 5;
|
||||
for (int y = 0; y < linePtNum; y++)
|
||||
{
|
||||
for (int x = 0; x < lineNum; x++)
|
||||
{
|
||||
SSG_featureClusteringInfo& a_featureInfo = featureInfoMask[x][y];
|
||||
if ((0 == a_featureInfo.featurType) || (a_featureInfo.clusterID > 0)) //非特征或已经处理
|
||||
continue;
|
||||
|
||||
SVzNL3DPoint& a_feature3DValue = feature3DInfo[x][y];
|
||||
SVzNL3DRangeD a_clusterRoi;
|
||||
a_clusterRoi.xRange.min = a_feature3DValue.x;
|
||||
a_clusterRoi.xRange.max = a_feature3DValue.x;
|
||||
a_clusterRoi.yRange.min = a_feature3DValue.y;
|
||||
a_clusterRoi.yRange.max = a_feature3DValue.y;
|
||||
a_clusterRoi.zRange.min = a_feature3DValue.z;
|
||||
a_clusterRoi.zRange.max = a_feature3DValue.z;
|
||||
|
||||
SVzNL2DPoint a_seedPos = { x, y };
|
||||
std::vector< SVzNL2DPoint> a_cluster;
|
||||
a_cluster.push_back(a_seedPos);
|
||||
wd_gridPointClustering(
|
||||
featureInfoMask,//int，记录特征标记和clusterID，附加一个flag
|
||||
feature3DInfo,//double,记录坐标信息
|
||||
clusterCheckWin, //搜索窗口
|
||||
growParam,//聚类条件
|
||||
clusterID, //当前Cluster的ID
|
||||
a_cluster, //result
|
||||
a_clusterRoi
|
||||
);
|
||||
clusters.push_back(a_cluster);
|
||||
SWD_clustersInfo a_info;
|
||||
a_info.clusterIdx = clusterID;
|
||||
a_info.ptSize = (int)a_cluster.size();
|
||||
a_info.roi3D = a_clusterRoi;
|
||||
clustersInfo.push_back(a_info);
|
||||
clusterID++;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
//电机定子定位。
|
||||
//输出：（1）定子目标（2）隔板抓取点
|
||||
//算法逻辑：找到定子的高度->设置截取Z去掉底面->
|
||||
// 投影（注意此时边框也同时投影）->距离变换->提取定子目标->
|
||||
// 根据相邻和边框寻找最佳抓取目标和抓取点
|
||||
@ -614,8 +788,7 @@ void wd_motorStatorPosition(
|
||||
SWD_nextOpParam* refPos, //上一次给出的参考位置，同时输出下一次的参考位置
|
||||
int* errCode,
|
||||
std::vector<SWD_motorStatorPosition>& resultObjPositions,
|
||||
SWD_statorOuterGrasper& resultGraspPos
|
||||
)
|
||||
SWD_statorOuterGrasper& resultGraspPos)
|
||||
{
|
||||
int lineNum = (int)scanLines.size();
|
||||
if (lineNum == 0)
|
||||
@ -629,14 +802,22 @@ void wd_motorStatorPosition(
|
||||
return;
|
||||
|
||||
///开始数据处理
|
||||
//使用cutZ_level进行切割
|
||||
|
||||
|
||||
|
||||
double statorRingWidth = (statorParam.statorOuterD - statorParam.statorInnerD) / 2;
|
||||
//
|
||||
|
||||
|
||||
|
||||
if (refPos->cuttingZ < 0) //提取特征：定子上半部的环。根据提取的环判断定子高度
|
||||
{
|
||||
//垂直数据处理
|
||||
std::vector<std::vector<SWD_segFeature>> all_vLineArcs;
|
||||
for (int i = 0; i < lineNum; i++)
|
||||
{
|
||||
if (i == 202)
|
||||
if (i == 576)
|
||||
int k = 1;
|
||||
//提取定子环
|
||||
std::vector<SWD_segFeature> line_ringArcs;
|
||||
|
||||
@ -14,6 +14,8 @@ typedef struct
|
||||
double statorOuterD; //定子外直径
|
||||
double statorInnerD; //定子内孔直径
|
||||
double statorHeight;
|
||||
double cutZ_level0;
|
||||
double cutZ_level1;
|
||||
double gripperR;//夹爪半径
|
||||
}SWD_statorParam;
|
||||
|
||||
@ -46,13 +48,16 @@ SG_APISHARED_EXPORT void wd_lineDataR(
|
||||
double groundH);
|
||||
|
||||
///数据输入必须是grid格式，以进行水平方向和垂直方向的处理
|
||||
SG_APISHARED_EXPORT void wd_motorStatorPosition(
|
||||
//电机定子定位。
|
||||
//算法逻辑：找到定子的高度->设置截取Z去掉底面->
|
||||
// 投影（注意此时边框也同时投影）->距离变换->提取定子目标->
|
||||
// 根据相邻和边框寻找最佳抓取目标和抓取点
|
||||
SG_APISHARED_EXPORT void wd_motorStatorPosition(
|
||||
std::vector< std::vector<SVzNL3DPosition>>& scanLines,
|
||||
const SWD_statorParam positionParam,
|
||||
const SWD_statorParam statorParam,
|
||||
const SSG_planeCalibPara groundCalibPara,
|
||||
const SWD_statorPositonParam algoParam,
|
||||
SWD_nextOpParam* refPos, //上一次给出的参考位置，同时输出下一次的参考位置
|
||||
int* errCode,
|
||||
std::vector<SWD_motorStatorPosition>& resultObjPositions,
|
||||
SWD_statorOuterGrasper& resultGraspPos
|
||||
);
|
||||
SWD_statorOuterGrasper& resultGraspPos);
|
||||
@ -2636,8 +2636,8 @@ void _outputScanDataFile_removeZeros(char* fileName, SVzNL3DLaserLine * scanData
|
||||
}
|
||||
|
||||
#define TEST_CONVERT_TO_GRID 0
|
||||
#define TEST_COMPUTE_WHEEL_ARCH 1
|
||||
#define TEST_COMPUTE_CALIB_PARA 0
|
||||
#define TEST_COMPUTE_WHEEL_ARCH 0
|
||||
#define TEST_COMPUTE_CALIB_PARA 1
|
||||
|
||||
#define TEST_GROUP 1
|
||||
int main()
|
||||
@ -2683,7 +2683,7 @@ int main()
|
||||
|
||||
#if TEST_COMPUTE_CALIB_PARA
|
||||
char _calib_datafile[256];
|
||||
sprintf_s(_calib_datafile, "F:/ShangGu/项目/冠钦_轮眉高度测量/测试数据/LaserLine1_grid.txt");
|
||||
sprintf_s(_calib_datafile, "F:/ShangGu/项目/冠钦_轮眉高度测量/测试数据/ground_LaserData.txt");
|
||||
|
||||
std::vector< std::vector<SVzNL3DPosition>> scanLines;
|
||||
vzReadLaserScanPointFromFile_XYZ_vector(_calib_datafile, scanLines);
|
||||
@ -2707,7 +2707,7 @@ int main()
|
||||
sprintf_s(calibFile, "F:/ShangGu/项目/冠钦_轮眉高度测量/测试数据/ground_calib_para.txt");
|
||||
_outputCalibPara(calibFile, calibPara);
|
||||
char _out_file[256];
|
||||
sprintf_s(_out_file, "F:/ShangGu/项目/冠钦_轮眉高度测量/测试数据/LaserLine1_grid_calib.txt");
|
||||
sprintf_s(_out_file, "F:/ShangGu/项目/冠钦_轮眉高度测量/测试数据/LaserLine_grund_calib.txt");
|
||||
_outputScanDataFile_self(_out_file, scanLines, 0, 0, 0);
|
||||
printf("%s: calib done!\n", _calib_datafile);
|
||||
}
|
||||
@ -2809,7 +2809,7 @@ int main()
|
||||
growParam,
|
||||
poseCalibPara,
|
||||
&errCode);
|
||||
#endif
|
||||
|
||||
long t2 = GetTickCount64();
|
||||
|
||||
char _dbg_file[256];
|
||||
@ -2823,6 +2823,7 @@ int main()
|
||||
|
||||
}
|
||||
}
|
||||
#endif
|
||||
printf("all done!\n");
|
||||
}
|
||||
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user