rodAndBarDetection version 1.3.2 :
矩森棒材抓取修正一个错误码返回错误,测试了地面调平API
This commit is contained in:
parent
16fb40399a
commit
4580256e47
@ -60,6 +60,73 @@ float _computeVecAngle(const SVzNL3DPoint& a, const SVzNL3DPoint& b)
|
|||||||
return degree;
|
return degree;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
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();
|
||||||
|
}
|
||||||
|
|
||||||
|
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 wdReadLaserScanPointFromFile_XYZ_vector(const char* fileName, std::vector<std::vector< SVzNL3DPosition>>& scanData)
|
void wdReadLaserScanPointFromFile_XYZ_vector(const char* fileName, std::vector<std::vector< SVzNL3DPosition>>& scanData)
|
||||||
{
|
{
|
||||||
std::ifstream inputFile(fileName);
|
std::ifstream inputFile(fileName);
|
||||||
@ -194,49 +261,38 @@ void _outputScanDataFile(char* fileName, std::vector<std::vector< SVzNL3DPositio
|
|||||||
sw.close();
|
sw.close();
|
||||||
}
|
}
|
||||||
|
|
||||||
SSG_planeCalibPara _readCalibPara(char* fileName)
|
void _readRodParas(char* fileName, std::vector<SSX_rodParam>& rodParas)
|
||||||
{
|
{
|
||||||
//设置初始结果
|
|
||||||
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::ifstream inputFile(fileName);
|
||||||
std::string linedata;
|
std::string linedata;
|
||||||
|
|
||||||
if (inputFile.is_open() == false)
|
if (inputFile.is_open() == false)
|
||||||
return planePara;
|
return;
|
||||||
|
|
||||||
//调平矩阵
|
std::vector<int> indices;
|
||||||
std::getline(inputFile, linedata);
|
int maxIdx = 0;
|
||||||
sscanf_s(linedata.c_str(), "%lf, %lf, %lf", &planePara.planeCalib[0], &planePara.planeCalib[1], &planePara.planeCalib[2]);
|
std::vector< SSX_rodParam> paras;
|
||||||
std::getline(inputFile, linedata);
|
while (getline(inputFile, linedata))
|
||||||
sscanf_s(linedata.c_str(), "%lf, %lf, %lf", &planePara.planeCalib[3], &planePara.planeCalib[4], &planePara.planeCalib[5]);
|
{
|
||||||
std::getline(inputFile, linedata);
|
int idx;
|
||||||
sscanf_s(linedata.c_str(), "%lf, %lf, %lf", &planePara.planeCalib[6], &planePara.planeCalib[7], &planePara.planeCalib[8]);
|
float len, dia;
|
||||||
//地面高度
|
sscanf_s(linedata.c_str(), "%d_L_%f_D_%f", &idx, &len, &dia);
|
||||||
std::getline(inputFile, linedata);
|
SSX_rodParam a_para = { dia , len };
|
||||||
sscanf_s(linedata.c_str(), "%lf", &planePara.planeHeight);
|
indices.push_back(idx);
|
||||||
//反向旋转矩阵
|
paras.push_back(a_para);
|
||||||
std::getline(inputFile, linedata);
|
maxIdx = maxIdx < idx ? idx : maxIdx;
|
||||||
sscanf_s(linedata.c_str(), "%lf, %lf, %lf", &planePara.invRMatrix[0], &planePara.invRMatrix[1], &planePara.invRMatrix[2]);
|
}
|
||||||
std::getline(inputFile, linedata);
|
if (maxIdx == 0)
|
||||||
sscanf_s(linedata.c_str(), "%lf, %lf, %lf", &planePara.invRMatrix[3], &planePara.invRMatrix[4], &planePara.invRMatrix[5]);
|
return;
|
||||||
std::getline(inputFile, linedata);
|
rodParas.resize(maxIdx + 1);
|
||||||
sscanf_s(linedata.c_str(), "%lf, %lf, %lf", &planePara.invRMatrix[6], &planePara.invRMatrix[7], &planePara.invRMatrix[8]);
|
for (int i = 0; i < (int)indices.size(); i++)
|
||||||
|
{
|
||||||
inputFile.close();
|
int idx = indices[i];
|
||||||
return planePara;
|
rodParas[idx] = paras[i];
|
||||||
|
}
|
||||||
|
return;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
void _outputChanneltInfo(char* fileName, std::vector<SSX_rodPoseInfo>& screwInfo)
|
void _outputChanneltInfo(char* fileName, std::vector<SSX_rodPoseInfo>& screwInfo)
|
||||||
{
|
{
|
||||||
std::ofstream sw(fileName);
|
std::ofstream sw(fileName);
|
||||||
@ -1133,23 +1189,230 @@ void newLocatingPlateTest(void)
|
|||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
#define ROD_POSITION_TEST_GROUP 3
|
;
|
||||||
|
void _removeNullData(std::vector<std::vector< SVzNL3DPosition>>& scanLines, double _F, std::vector<std::vector< SVzNLPointXYZRGBA>>& scanData)
|
||||||
|
{
|
||||||
|
std::vector< SVzNLPointXYZRGBA> validPoints;
|
||||||
|
for (int line = 0; line < (int)scanLines.size(); line++)
|
||||||
|
{
|
||||||
|
for (int j = 0; j < (int)scanLines[line].size(); j++)
|
||||||
|
{
|
||||||
|
if (scanLines[line][j].pt3D.z > 1e-4)
|
||||||
|
{
|
||||||
|
SVzNLPointXYZRGBA a_pt;
|
||||||
|
double v = _F * scanLines[line][j].pt3D.y / scanLines[line][j].pt3D.z + 2000;
|
||||||
|
a_pt.nRGB = (int)(v + 0.5);
|
||||||
|
a_pt.x = scanLines[line][j].pt3D.x;
|
||||||
|
a_pt.y = scanLines[line][j].pt3D.y;
|
||||||
|
a_pt.z = scanLines[line][j].pt3D.z;
|
||||||
|
validPoints.push_back(a_pt);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
std::vector< SVzNLPointXYZRGBA> a_line;
|
||||||
|
SVzNLPointXYZRGBA pre_pt;
|
||||||
|
pre_pt.x = 0;
|
||||||
|
pre_pt.y = 0;
|
||||||
|
pre_pt.z = 0;
|
||||||
|
pre_pt.nRGB = -1;
|
||||||
|
for (int i = 0; i < validPoints.size(); i++)
|
||||||
|
{
|
||||||
|
if (validPoints[i].nRGB < (pre_pt.nRGB - 10))
|
||||||
|
{
|
||||||
|
if (a_line.size() > 0)
|
||||||
|
scanData.push_back(a_line);
|
||||||
|
a_line.clear();
|
||||||
|
}
|
||||||
|
a_line.push_back(validPoints[i]);
|
||||||
|
pre_pt = validPoints[i];
|
||||||
|
}
|
||||||
|
if(a_line.size() > 0)
|
||||||
|
scanData.push_back(a_line);
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
void _convertToGridData_XYZRGB(std::vector<std::vector< SVzNLPointXYZRGBA>>& scanData, double _F, std::vector<std::vector< SVzNL3DPosition>>& gridData)
|
||||||
|
{
|
||||||
|
int lineNum = (int)scanData.size();
|
||||||
|
int min_y = 100000000;
|
||||||
|
int max_y = -10000000;
|
||||||
|
int validStartLine = -1;
|
||||||
|
int validEndLine = -1;
|
||||||
|
for (int line = 0; line < lineNum; line++)
|
||||||
|
{
|
||||||
|
if (scanData[line].size() > 0)
|
||||||
|
{
|
||||||
|
if (validStartLine < 0)
|
||||||
|
{
|
||||||
|
validStartLine = line;
|
||||||
|
validEndLine = line;
|
||||||
|
}
|
||||||
|
else
|
||||||
|
validEndLine = line;
|
||||||
|
}
|
||||||
|
|
||||||
|
for (int i = 0; i < (int)scanData[line].size(); i++)
|
||||||
|
{
|
||||||
|
SVzNLPointXYZRGBA& a_pt = scanData[line][i];
|
||||||
|
if (a_pt.z > 1e-4)
|
||||||
|
{
|
||||||
|
double v = _F * a_pt.y / a_pt.z + 2000;
|
||||||
|
a_pt.nRGB = (int)(v + 0.5);
|
||||||
|
max_y = max_y < (int)a_pt.nRGB ? (int)a_pt.nRGB : max_y;
|
||||||
|
min_y = min_y > (int)a_pt.nRGB ? (int)a_pt.nRGB : min_y;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
if (min_y == 100000000)
|
||||||
|
return;
|
||||||
|
|
||||||
|
int vldLineNum = validEndLine - validStartLine + 1;
|
||||||
|
gridData.resize(vldLineNum);
|
||||||
|
int pt_counter = max_y - min_y + 1;
|
||||||
|
for (int i = 0; i < vldLineNum; i++)
|
||||||
|
{
|
||||||
|
gridData[i].resize(pt_counter);
|
||||||
|
for (int j = 0; j < pt_counter; j++)
|
||||||
|
{
|
||||||
|
gridData[i][j].nPointIdx = j;
|
||||||
|
gridData[i][j].pt3D.x = 0;
|
||||||
|
gridData[i][j].pt3D.y = 0;
|
||||||
|
gridData[i][j].pt3D.z = 0;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
for (int line = validStartLine; line <= validEndLine; line++)
|
||||||
|
{
|
||||||
|
int gridLine = line - validStartLine;
|
||||||
|
for (int i = 0; i < (int)scanData[line].size(); i++)
|
||||||
|
{
|
||||||
|
SVzNLPointXYZRGBA& a_pt = scanData[line][i];
|
||||||
|
if (a_pt.z > 1e-4)
|
||||||
|
{
|
||||||
|
int pt_id = a_pt.nRGB - min_y;
|
||||||
|
gridData[gridLine][pt_id].pt3D.x = a_pt.x;
|
||||||
|
gridData[gridLine][pt_id].pt3D.y = a_pt.y;
|
||||||
|
gridData[gridLine][pt_id].pt3D.z = a_pt.z;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
#define ROD_POSITION_TEST_GROUP 5
|
||||||
|
#define TEST_CONVERT_TO_GRID 0
|
||||||
|
#define TEST_COMPUTE_CALIB_PARA 0
|
||||||
|
#define TEST_COMPUTE_ROD_POSITION 1
|
||||||
void rodPositionTest(void)
|
void rodPositionTest(void)
|
||||||
{
|
{
|
||||||
|
#if TEST_CONVERT_TO_GRID
|
||||||
|
//将数据转换成栅格格式格式
|
||||||
|
char _scan_dir[256];
|
||||||
|
int lineNum = 0;
|
||||||
|
float lineV = 0.0f;
|
||||||
|
int dataCalib = 0;
|
||||||
|
int maxTimeStamp = 0;
|
||||||
|
int clockPerSecond = 0;
|
||||||
|
sprintf_s(_scan_dir, "F:/ShangGu/项目/冠钦项目/矩森棒材抓取/空料框及内剩一根棒材/");
|
||||||
|
char _scan_src_file[256];
|
||||||
|
double _F = 1219.15; //f
|
||||||
|
for (int i = 1; i <= 3; i++)
|
||||||
|
{
|
||||||
|
//read ply txt
|
||||||
|
sprintf_s(_scan_src_file, "%sLaserDataRAW_%d.txt", _scan_dir, i);
|
||||||
|
|
||||||
|
std::vector<std::vector< SVzNL3DPosition>> scanLines;
|
||||||
|
wdReadLaserScanPointFromFile_XYZ_vector(_scan_src_file, scanLines);
|
||||||
|
|
||||||
|
std::vector<std::vector< SVzNLPointXYZRGBA>> scanData;
|
||||||
|
_removeNullData(scanLines, _F, scanData);
|
||||||
|
|
||||||
|
std::vector<std::vector< SVzNL3DPosition>> gridData;
|
||||||
|
_convertToGridData_XYZRGB(scanData, _F, gridData);
|
||||||
|
char _out_file[256];
|
||||||
|
sprintf_s(_out_file, "%sLaserData_%d.txt", _scan_dir, i);
|
||||||
|
_outputScanDataFile(_out_file, gridData, 0.0f, 0, 0);
|
||||||
|
|
||||||
|
printf("%s: convert done!\n", _scan_src_file);
|
||||||
|
}
|
||||||
|
printf("all converted done!\n", _scan_src_file);
|
||||||
|
#endif
|
||||||
|
#if TEST_COMPUTE_CALIB_PARA
|
||||||
|
const char* calibDataPath = "F:/ShangGu/项目/冠钦项目/矩森棒材抓取/空料框及内剩一根棒材/调平/位置1/";
|
||||||
|
char _calib_datafile[256];
|
||||||
|
sprintf_s(_calib_datafile, "%sGroundData_1.txt", calibDataPath);
|
||||||
|
int lineNum = 0;
|
||||||
|
float lineV = 0.0f;
|
||||||
|
int dataCalib = 0;
|
||||||
|
int maxTimeStamp = 0;
|
||||||
|
int clockPerSecond = 0;
|
||||||
|
std::vector<std::vector< SVzNL3DPosition>> scanData;
|
||||||
|
wdReadLaserScanPointFromFile_XYZ_vector(_calib_datafile, scanData);
|
||||||
|
|
||||||
|
lineNum = (int)scanData.size();
|
||||||
|
if (scanData.size() > 0)
|
||||||
|
{
|
||||||
|
SSG_planeCalibPara calibPara = sx_rodPosition_getBaseCalibPara(scanData);
|
||||||
|
//结果进行验证
|
||||||
|
for (int i = 0; i < lineNum; i++)
|
||||||
|
{
|
||||||
|
if (i == 14)
|
||||||
|
int kkk = 1;
|
||||||
|
//行处理
|
||||||
|
//调平,去除地面
|
||||||
|
sx_rodPosition_lineDataR(scanData[i], calibPara.planeCalib, -1);
|
||||||
|
}
|
||||||
|
//
|
||||||
|
char calibFile[250];
|
||||||
|
sprintf_s(calibFile, "%sground_calib_para.txt", calibDataPath);
|
||||||
|
_outputCalibPara(calibFile, calibPara);
|
||||||
|
char _out_file[256];
|
||||||
|
sprintf_s(_out_file, "%sGroundData_1_calib_verify.txt", calibDataPath);
|
||||||
|
|
||||||
|
_outputScanDataFile(_out_file, scanData, lineV, maxTimeStamp, clockPerSecond);
|
||||||
|
#if 0
|
||||||
|
for (int fidx = fileIdx[cvtGrp].nMin; fidx <= fileIdx[cvtGrp].nMax; fidx++)
|
||||||
|
{
|
||||||
|
//fidx =4;
|
||||||
|
char _scan_file[256];
|
||||||
|
sprintf_s(_scan_file, "%sLaserData_%d.txt", dataPath[cvtGrp], fidx);
|
||||||
|
std::vector<std::vector< SVzNL3DPosition>> scanLines;
|
||||||
|
vzReadLaserScanPointFromFile_XYZ_vector(_scan_file, scanLines);
|
||||||
|
if (scanLines.size() == 0)
|
||||||
|
continue;
|
||||||
|
lineNum = (int)scanLines.size();
|
||||||
|
for (int i = 0; i < lineNum; i++)
|
||||||
|
{
|
||||||
|
//调平,去除地面
|
||||||
|
wd_lineDataR(scanLines[i], calibPara.planeCalib, -1);
|
||||||
|
}
|
||||||
|
sprintf_s(_scan_file, "%sLaserData_%d_calib_verify.txt", dataPath[cvtGrp], fidx);
|
||||||
|
int headNullLines = 0;
|
||||||
|
_outputScanDataFile_vector(_scan_file, scanLines, false, &headNullLines);
|
||||||
|
}
|
||||||
|
#endif
|
||||||
|
printf("%s: calib done!\n", _calib_datafile);
|
||||||
|
}
|
||||||
|
#endif
|
||||||
|
|
||||||
|
#if TEST_COMPUTE_ROD_POSITION
|
||||||
const char* dataPath[ROD_POSITION_TEST_GROUP] = {
|
const char* dataPath[ROD_POSITION_TEST_GROUP] = {
|
||||||
"F:/ShangGu/项目/冠钦项目/矩森棒材抓取/测试数据/", //0
|
"F:/ShangGu/项目/冠钦项目/矩森棒材抓取/测试数据/", //0
|
||||||
"F:/ShangGu/项目/冠钦项目/胶布圆棒抓取/模拟测试数据/", //1
|
"F:/ShangGu/项目/冠钦项目/矩森棒材抓取/空料框及内剩一根棒材/", //1
|
||||||
"F:/ShangGu/项目/冠钦项目/胶布圆棒抓取/模拟测试数据2/", //1
|
"F:/ShangGu/项目/冠钦项目/矩森棒材抓取/其它尺寸棒材点云/", //2
|
||||||
|
|
||||||
|
"F:/ShangGu/项目/冠钦项目/胶布圆棒抓取/模拟测试数据/", //3
|
||||||
|
"F:/ShangGu/项目/冠钦项目/胶布圆棒抓取/模拟测试数据2/", //4
|
||||||
};
|
};
|
||||||
|
|
||||||
SVzNLRange fileIdx[ROD_POSITION_TEST_GROUP] = {
|
SVzNLRange fileIdx[ROD_POSITION_TEST_GROUP] = {
|
||||||
{1,8}, {1,5},{1,11}
|
{1,8}, {1,3}, {1,31},
|
||||||
|
{1,5},{1,11}
|
||||||
};
|
};
|
||||||
|
|
||||||
const char* ver = wd_rodAndBarDetectionVersion();
|
const char* ver = wd_rodAndBarDetectionVersion();
|
||||||
printf("ver:%s\n", ver);
|
printf("ver:%s\n", ver);
|
||||||
|
|
||||||
for (int grp = 2; grp < ROD_POSITION_TEST_GROUP; grp++)
|
for (int grp = 0; grp <= 2; grp++)
|
||||||
{
|
{
|
||||||
SSG_planeCalibPara poseCalibPara;
|
SSG_planeCalibPara poseCalibPara;
|
||||||
//初始化成单位阵
|
//初始化成单位阵
|
||||||
@ -1165,15 +1428,21 @@ void rodPositionTest(void)
|
|||||||
poseCalibPara.planeHeight = -1.0;
|
poseCalibPara.planeHeight = -1.0;
|
||||||
for (int i = 0; i < 9; i++)
|
for (int i = 0; i < 9; i++)
|
||||||
poseCalibPara.invRMatrix[i] = poseCalibPara.planeCalib[i];
|
poseCalibPara.invRMatrix[i] = poseCalibPara.planeCalib[i];
|
||||||
//char calibFile[250];
|
|
||||||
//sprintf_s(calibFile, "%sground_calib_para.txt", dataPath[grp]);
|
char calibFile[250];
|
||||||
//poseCalibPara = _readCalibPara(calibFile);
|
sprintf_s(calibFile, "%sground_calib_para.txt", dataPath[grp]);
|
||||||
|
poseCalibPara = _readCalibPara(calibFile);
|
||||||
|
|
||||||
|
sprintf_s(calibFile, "%sparameter.txt", dataPath[grp]);
|
||||||
|
std::vector<SSX_rodParam> rodParas;
|
||||||
|
if (grp <= 2)
|
||||||
|
_readRodParas(calibFile, rodParas);
|
||||||
|
|
||||||
for (int fidx = fileIdx[grp].nMin; fidx <= fileIdx[grp].nMax; fidx++)
|
for (int fidx = fileIdx[grp].nMin; fidx <= fileIdx[grp].nMax; fidx++)
|
||||||
{
|
{
|
||||||
//fidx =1;
|
//fidx =5;
|
||||||
char _scan_file[256];
|
char _scan_file[256];
|
||||||
if( (1 == grp) ||(2 == grp))
|
if( (3 == grp) ||(4 == grp))
|
||||||
sprintf_s(_scan_file, "%s%d_LaserData_Hi229156.txt", dataPath[grp], fidx);
|
sprintf_s(_scan_file, "%s%d_LaserData_Hi229156.txt", dataPath[grp], fidx);
|
||||||
else
|
else
|
||||||
sprintf_s(_scan_file, "%sLaserData_%d.txt", dataPath[grp], fidx);
|
sprintf_s(_scan_file, "%sLaserData_%d.txt", dataPath[grp], fidx);
|
||||||
@ -1188,10 +1457,9 @@ void rodPositionTest(void)
|
|||||||
long t1 = (long)GetTickCount64();//统计时间
|
long t1 = (long)GetTickCount64();//统计时间
|
||||||
|
|
||||||
SSX_rodParam rodParam;
|
SSX_rodParam rodParam;
|
||||||
if (0 == grp)
|
if (grp <= 2)
|
||||||
{
|
{
|
||||||
rodParam.diameter = 52.0; //圆棒直径
|
rodParam = rodParas[fidx];
|
||||||
rodParam.len = 290;
|
|
||||||
}
|
}
|
||||||
else
|
else
|
||||||
{
|
{
|
||||||
@ -1200,7 +1468,7 @@ void rodPositionTest(void)
|
|||||||
}
|
}
|
||||||
|
|
||||||
SSG_cornerParam cornerParam;
|
SSG_cornerParam cornerParam;
|
||||||
cornerParam.cornerTh = 60; //45度角
|
cornerParam.cornerTh = 45; //45度角
|
||||||
cornerParam.scale = rodParam.diameter / 4; // algoParam.bagParam.bagH / 8; // 15; // algoParam.bagParam.bagH / 8;
|
cornerParam.scale = rodParam.diameter / 4; // algoParam.bagParam.bagH / 8; // 15; // algoParam.bagParam.bagH / 8;
|
||||||
cornerParam.minEndingGap = 20; // algoParam.bagParam.bagW / 4;
|
cornerParam.minEndingGap = 20; // algoParam.bagParam.bagW / 4;
|
||||||
cornerParam.minEndingGap_z = 5.0;
|
cornerParam.minEndingGap_z = 5.0;
|
||||||
@ -1216,8 +1484,8 @@ void rodPositionTest(void)
|
|||||||
growParam.yDeviation_max = 10.0;
|
growParam.yDeviation_max = 10.0;
|
||||||
growParam.maxSkipDistance = 10.0;
|
growParam.maxSkipDistance = 10.0;
|
||||||
growParam.zDeviation_max = 10.0;//
|
growParam.zDeviation_max = 10.0;//
|
||||||
growParam.minLTypeTreeLen = 100; //mm, 螺杆长度
|
growParam.minLTypeTreeLen = rodParam.len * 0.75; //mm, 螺杆长度
|
||||||
growParam.minVTypeTreeLen = 100; //mm
|
growParam.minVTypeTreeLen = rodParam.len * 0.75; //mm
|
||||||
|
|
||||||
bool isHorizonScan = true; //true:激光线平行槽道;false:激光线垂直槽道
|
bool isHorizonScan = true; //true:激光线平行槽道;false:激光线垂直槽道
|
||||||
int errCode = 0;
|
int errCode = 0;
|
||||||
@ -1233,7 +1501,7 @@ void rodPositionTest(void)
|
|||||||
&errCode);
|
&errCode);
|
||||||
|
|
||||||
long t2 = (long)GetTickCount64();
|
long t2 = (long)GetTickCount64();
|
||||||
printf("%s: %d(ms)!\n", _scan_file, (int)(t2 - t1));
|
printf("%s: %d(ms),errCode=%d\n", _scan_file, (int)(t2 - t1),errCode);
|
||||||
//输出测试结果
|
//输出测试结果
|
||||||
sprintf_s(_scan_file, "%sresult\\%d_result.txt", dataPath[grp], fidx);
|
sprintf_s(_scan_file, "%sresult\\%d_result.txt", dataPath[grp], fidx);
|
||||||
_outputRGBDScan_RGBD_rodInfo(_scan_file, scanLines, rodInfo);
|
_outputRGBDScan_RGBD_rodInfo(_scan_file, scanLines, rodInfo);
|
||||||
@ -1241,6 +1509,7 @@ void rodPositionTest(void)
|
|||||||
_outputRodtInfo(_scan_file, rodInfo);
|
_outputRodtInfo(_scan_file, rodInfo);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
#endif
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
@ -1357,8 +1626,8 @@ int main()
|
|||||||
{
|
{
|
||||||
//ESG_testMode testMode = keSG_测试_配天螺杆定位;
|
//ESG_testMode testMode = keSG_测试_配天螺杆定位;
|
||||||
//ESG_testMode testMode = keSG_测试_配天定位盘定位;
|
//ESG_testMode testMode = keSG_测试_配天定位盘定位;
|
||||||
ESG_testMode testMode = keSG_测试_配天新定位盘定位;
|
//ESG_testMode testMode = keSG_测试_配天新定位盘定位;
|
||||||
//ESG_testMode testMode = keSG_测试_棒材抓取;
|
ESG_testMode testMode = keSG_测试_棒材抓取;
|
||||||
//ESG_testMode testMode = keSG_测试_筑裕钢筋焊缝定位;
|
//ESG_testMode testMode = keSG_测试_筑裕钢筋焊缝定位;
|
||||||
|
|
||||||
if(keSG_测试_配天螺杆定位 == testMode)
|
if(keSG_测试_配天螺杆定位 == testMode)
|
||||||
|
|||||||
@ -4597,7 +4597,7 @@ void wd_getRodArcFeature_segmentPeakCornerMethod(
|
|||||||
std::vector<SWD_rodArcFeature>& line_rodArcs //环
|
std::vector<SWD_rodArcFeature>& line_rodArcs //环
|
||||||
)
|
)
|
||||||
{
|
{
|
||||||
double arcPerPointCornerMinValue = 5; //arc上每个点的转角最小值
|
double arcPerPointCornerMinValue = 5.0; //arc上每个点的转角最小值
|
||||||
double arcTotalCornerMinValue = 30; //整个Arc的转角最小值
|
double arcTotalCornerMinValue = 30; //整个Arc的转角最小值
|
||||||
|
|
||||||
std::vector<SSG_RUN> segs;
|
std::vector<SSG_RUN> segs;
|
||||||
|
|||||||
@ -22,7 +22,8 @@
|
|||||||
//version 1.2.11 : 根据现场数据调整特征生长参数
|
//version 1.2.11 : 根据现场数据调整特征生长参数
|
||||||
//version 1.3.0 : 新的定位盘中心测量功能
|
//version 1.3.0 : 新的定位盘中心测量功能
|
||||||
//version 1.3.1 : 定位盘中心测量功能添加了噪声过滤
|
//version 1.3.1 : 定位盘中心测量功能添加了噪声过滤
|
||||||
std::string m_strVersion = "1.3. ";
|
//version 1.3.2 : 矩森棒材抓取修正一个错误码返回错误,测试了地面调平API
|
||||||
|
std::string m_strVersion = "1.3.2";
|
||||||
const char* wd_rodAndBarDetectionVersion(void)
|
const char* wd_rodAndBarDetectionVersion(void)
|
||||||
{
|
{
|
||||||
return m_strVersion.c_str();
|
return m_strVersion.c_str();
|
||||||
@ -245,7 +246,7 @@ void rodAarcFeatueDetection(
|
|||||||
int linePtNum = (int)scanLines[0].size();
|
int linePtNum = (int)scanLines[0].size();
|
||||||
for (int line = 0; line < lineNum; line++)
|
for (int line = 0; line < lineNum; line++)
|
||||||
{
|
{
|
||||||
if (line == 1304)
|
if (line == 413)
|
||||||
int kkk = 1;
|
int kkk = 1;
|
||||||
std::vector<SVzNL3DPosition>& lineData = scanLines[line];
|
std::vector<SVzNL3DPosition>& lineData = scanLines[line];
|
||||||
|
|
||||||
@ -2676,7 +2677,7 @@ void sx_rodPositioning(
|
|||||||
|
|
||||||
if ((rodArcTrees_v.size() == 0) && (rodArcTrees_h.size() == 0))
|
if ((rodArcTrees_v.size() == 0) && (rodArcTrees_h.size() == 0))
|
||||||
{
|
{
|
||||||
*errCode = SG_ERR_NOT_GRID_FORMAT;
|
*errCode = SX_ERR_ZERO_OBJECTS;
|
||||||
return;
|
return;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|||||||
@ -65,7 +65,7 @@ SG_APISHARED_EXPORT const char* wd_rodAndBarDetectionVersion(void);
|
|||||||
SG_APISHARED_EXPORT SSG_planeCalibPara sx_rodPosition_getBaseCalibPara(
|
SG_APISHARED_EXPORT SSG_planeCalibPara sx_rodPosition_getBaseCalibPara(
|
||||||
std::vector< std::vector<SVzNL3DPosition>>& scanLines);
|
std::vector< std::vector<SVzNL3DPosition>>& scanLines);
|
||||||
|
|
||||||
#if 0
|
#if 1
|
||||||
//相机姿态调平,并去除地面
|
//相机姿态调平,并去除地面
|
||||||
SG_APISHARED_EXPORT void sx_rodPosition_lineDataR(
|
SG_APISHARED_EXPORT void sx_rodPosition_lineDataR(
|
||||||
std::vector< SVzNL3DPosition>& a_line,
|
std::vector< SVzNL3DPosition>& a_line,
|
||||||
|
|||||||
Loading…
x
Reference in New Issue
Block a user