motorStatorPosition version 1.0.0
定子线圈抓取，初始版本
This commit is contained in:
parent
8ac9065383
commit
c1af27cf46
@ -245,15 +245,16 @@ void _outputRGBDScan_RGBD(
|
||||
|
||||
#define TEST_GET_ROI_DATA 0
|
||||
#define TEST_COMPUTE_SPACE 1
|
||||
#define TEST_GROUP 1
|
||||
#define TEST_GROUP 2
|
||||
int main()
|
||||
{
|
||||
const char* dataPath[TEST_GROUP] = {
|
||||
"F:/ShangGu/项目/水木宏创/槽道间距测量/模拟数据/", //0
|
||||
"F:/ShangGu/项目/水木宏创/槽道间距测量/现场数据/", //0
|
||||
};
|
||||
|
||||
SVzNLRange fileIdx[TEST_GROUP] = {
|
||||
{1,9},
|
||||
{1,9},{1,4}
|
||||
};
|
||||
|
||||
const char* ver = wd_ChannelSPaceMeasureVersion();
|
||||
@ -285,7 +286,7 @@ int main()
|
||||
#endif
|
||||
|
||||
#if TEST_COMPUTE_SPACE
|
||||
for (int grp = 0; grp < TEST_GROUP; grp++)
|
||||
for (int grp = 1; grp < TEST_GROUP; grp++)
|
||||
{
|
||||
for (int fidx = fileIdx[grp].nMin; fidx <= fileIdx[grp].nMax; fidx++)
|
||||
{
|
||||
@ -316,15 +317,15 @@ int main()
|
||||
|
||||
SSG_treeGrowParam growParam;
|
||||
growParam.maxLineSkipNum = 10;
|
||||
growParam.yDeviation_max = 20.0;
|
||||
growParam.yDeviation_max = 40.0;
|
||||
growParam.maxSkipDistance = 20.0;
|
||||
growParam.zDeviation_max = 50.0;//
|
||||
growParam.minLTypeTreeLen = 100; //mm
|
||||
growParam.minVTypeTreeLen = 100; //mm
|
||||
growParam.zDeviation_max = 100.0;//
|
||||
growParam.minLTypeTreeLen = 500; //mm
|
||||
growParam.minVTypeTreeLen = 500; //mm
|
||||
|
||||
SSX_channelParam channelParam;
|
||||
channelParam.channleSpaceRng = { 300, 800 };
|
||||
channelParam.channelWidthRng = { 5,30 };
|
||||
channelParam.channelWidthRng = { 5,50 };
|
||||
|
||||
bool isHorizonScan = true; //true:激光线平行槽道；false:激光线垂直槽道
|
||||
int errCode = 0;
|
||||
|
||||
@ -563,7 +563,7 @@ int main()
|
||||
SVzNLRange fileIdx[TEST_GROUP] = {
|
||||
{1,13},
|
||||
{2,5},
|
||||
{1,10}
|
||||
{18,18}
|
||||
};
|
||||
|
||||
const char* ver = wd_bagThreadPositioningVersion();
|
||||
@ -571,33 +571,33 @@ int main()
|
||||
|
||||
#if CONVERT_TO_GRID
|
||||
int convertGrp = 2;
|
||||
for (int fidx = 1; fidx <= 10; fidx++)
|
||||
for (int fidx = 17; fidx <= 17; fidx++)
|
||||
{
|
||||
char _scan_file[256];
|
||||
sprintf_s(_scan_file, "%s%dheightline_mm.txt", dataPath[convertGrp], fidx);
|
||||
|
||||
std::vector<std::vector< SVzNL3DPosition>> scanData;
|
||||
double lineStep = 0.032;
|
||||
double baseZ = 350.0;
|
||||
double lineStep = 0.033;
|
||||
double baseZ = 300.0;
|
||||
vzReadLaserScanPointFromFile_encodePlyTxt(_scan_file, lineStep, baseZ, scanData);
|
||||
bool isGridData = checkGridFormat(scanData);
|
||||
int gridFormat = (true == isGridData) ? 1 : 0;
|
||||
printf("%s: 栅格格式=%d\n", _scan_file, gridFormat);
|
||||
removeNullLines(scanData);
|
||||
std::vector<std::vector< SVzNL3DPosition>> sampleData;
|
||||
downSampleGridData(scanData, 3, sampleData);
|
||||
downSampleGridData(scanData, 2, sampleData);
|
||||
//将数据恢复为按扫描线存储格式
|
||||
sprintf_s(_scan_file, "%s袋子_grid_%d.txt", dataPath[convertGrp], fidx);
|
||||
_outputScanDataFile(_scan_file, sampleData,0, 0, 0);
|
||||
printf("%s: done.\n", _scan_file);
|
||||
}
|
||||
|
||||
#if 0
|
||||
char _scan_file[256];
|
||||
sprintf_s(_scan_file, "%sgroundData.txt", dataPath[convertGrp]);
|
||||
|
||||
std::vector<std::vector< SVzNL3DPosition>> scanData;
|
||||
double lineStep = 0.032;
|
||||
double baseZ = 350.0;
|
||||
double baseZ = 300.0;
|
||||
vzReadLaserScanPointFromFile_encodePlyTxt(_scan_file, lineStep, baseZ, scanData);
|
||||
bool isGridData = checkGridFormat(scanData);
|
||||
int gridFormat = (true == isGridData) ? 1 : 0;
|
||||
@ -609,6 +609,7 @@ int main()
|
||||
sprintf_s(_scan_file, "%sgroundData_grid.txt", dataPath[convertGrp]);
|
||||
_outputScanDataFile(_scan_file, sampleData, 0, 0, 0);
|
||||
printf("%s: done.\n", _scan_file);
|
||||
#endif
|
||||
|
||||
#endif
|
||||
|
||||
@ -678,8 +679,8 @@ int main()
|
||||
}
|
||||
|
||||
sprintf_s(_scan_file, "%s袋子_grid_%d.txt", dataPath[grp], fidx);
|
||||
std::vector<std::vector< SVzNL3DPosition>> scanLines;
|
||||
wdReadLaserScanPointFromFile_XYZ_vector(_scan_file, scanLines);
|
||||
std::vector<std::vector< SVzNL3DPosition>> scanLines_src;
|
||||
wdReadLaserScanPointFromFile_XYZ_vector(_scan_file, scanLines_src);
|
||||
|
||||
long t1 = (long)GetTickCount64();//统计时间
|
||||
|
||||
@ -741,6 +742,12 @@ int main()
|
||||
scanInfo.mark_distance = 39.0; //两个Mark的距离
|
||||
}
|
||||
|
||||
std::vector<std::vector< SVzNL3DPosition>> scanLines;;
|
||||
if ((grp == 2) && (fidx == 18))
|
||||
downSampleGridData(scanLines_src, 3, scanLines);
|
||||
else
|
||||
scanLines = scanLines_src;
|
||||
|
||||
int errCode = 0;
|
||||
std::vector<SSX_bagThreadInfo> bagThreadInfo;
|
||||
std::vector<SSX_bagThreadInfo> bagThreadInfo_relative; //相对于Mark的坐标
|
||||
@ -761,10 +768,19 @@ int main()
|
||||
long t2 = (long)GetTickCount64();
|
||||
printf("%s: %d(ms)!\n", _scan_file, (int)(t2 - t1));
|
||||
//输出测试结果
|
||||
if (output_markCenter.size() == 2)
|
||||
{
|
||||
SVzNL3DPoint markDir = { output_markCenter[1].x - output_markCenter[0].x,
|
||||
output_markCenter[1].y - output_markCenter[0].y,
|
||||
output_markCenter[1].z - output_markCenter[0].z };
|
||||
double distance = sqrt(pow(markDir.x, 2) + pow(markDir.y, 2) + pow(markDir.z, 2));
|
||||
printf(" mark1: ( %g, %g, %g ), distance = %g\n", markDir.x, markDir.y, markDir.z, distance);
|
||||
}
|
||||
|
||||
sprintf_s(_scan_file, "%sresult\\%d_result.txt", dataPath[grp], fidx);
|
||||
_outputRGBDScan_RGBD(_scan_file, scanLines, bagThreadInfo, output_markCenter);
|
||||
sprintf_s(_scan_file, "%sresult\\%d_screw_info.txt", dataPath[grp], fidx);
|
||||
_outputChanneltInfo(_scan_file, bagThreadInfo);
|
||||
_outputChanneltInfo(_scan_file, bagThreadInfo_relative);
|
||||
}
|
||||
}
|
||||
#endif
|
||||
|
||||
@ -303,8 +303,8 @@ void _outputRGBDScan_fillingPort_RGBD(
|
||||
sw.close();
|
||||
}
|
||||
|
||||
#define TEST_CONVERT_TO_GRID 1
|
||||
#define TEST_COMPUTE_POSE 0
|
||||
#define TEST_CONVERT_TO_GRID 0
|
||||
#define TEST_COMPUTE_POSE 1
|
||||
#define TEST_GROUP 2
|
||||
int main()
|
||||
{
|
||||
@ -314,7 +314,7 @@ int main()
|
||||
};
|
||||
|
||||
SVzNLRange fileIdx[TEST_GROUP] = {
|
||||
{2,30},
|
||||
{31,31},
|
||||
};
|
||||
|
||||
const char* ver = wd_gasFillingPortPositionVersion();
|
||||
@ -333,7 +333,7 @@ int main()
|
||||
#endif
|
||||
|
||||
#if TEST_COMPUTE_POSE
|
||||
for (int grp = 0; grp < TEST_GROUP; grp++)
|
||||
for (int grp = 0; grp <=0; grp++)
|
||||
{
|
||||
for (int fidx = fileIdx[grp].nMin; fidx <= fileIdx[grp].nMax; fidx++)
|
||||
{
|
||||
@ -355,7 +355,7 @@ int main()
|
||||
gasFillingPortPara.outerD = 52.0;
|
||||
|
||||
SSG_lineSegParam lineSegPara;
|
||||
lineSegPara.maxDist = 1.0;
|
||||
lineSegPara.distScale = 1.0;
|
||||
lineSegPara.segGapTh_y = 5.0; //y方向间隔大于5mm认为是分段
|
||||
lineSegPara.segGapTh_z = 10.0; //z方向间隔大于10mm认为是分段
|
||||
|
||||
|
||||
@ -273,6 +273,59 @@ SVzNL3DLaserLine* _convertToGridData_XYZRGB(SVzNLXYZRGBDLaserLine* laser3DPoints
|
||||
return gridData;
|
||||
}
|
||||
|
||||
void vzReadLaserScanPointFromFile_feihu(
|
||||
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> linePoints;
|
||||
linePoints.clear();
|
||||
int startIdx = -1;
|
||||
int ptIdx = 0;
|
||||
double pre_y = DBL_MAX;
|
||||
while (std::getline(inputFile, linedata))
|
||||
{
|
||||
if (linedata.empty())
|
||||
continue;
|
||||
|
||||
if (0 == strncmp("{", linedata.c_str(), 1))
|
||||
{
|
||||
float X, Y, Z;
|
||||
int imageY = 0;
|
||||
float leftX, leftY;
|
||||
float rightX, rightY;
|
||||
int r = -1, g = -1, b = -1;
|
||||
sscanf_s(linedata.c_str(), "{%f,%f,%f,%f,%f,%f }-{%f,%f}-{%f,%f}", &X, &Y, &Z, &r, &g, &b, &leftX, &leftY, &rightX, &rightY);
|
||||
|
||||
SVzNL3DPosition a_pt;
|
||||
a_pt.nPointIdx = ptIdx;
|
||||
ptIdx++;
|
||||
a_pt.pt3D.x = X;
|
||||
a_pt.pt3D.y = Y;
|
||||
a_pt.pt3D.z = Z;
|
||||
|
||||
if ((Y+5.0) < pre_y)
|
||||
{
|
||||
//新行
|
||||
if (linePoints.size() > 0)
|
||||
scanData.push_back(linePoints);
|
||||
linePoints.clear();
|
||||
ptIdx = 0;
|
||||
}
|
||||
linePoints.push_back(a_pt);
|
||||
pre_y = Y;
|
||||
}
|
||||
}
|
||||
if (linePoints.size() > 0)
|
||||
scanData.push_back(linePoints);
|
||||
return;
|
||||
}
|
||||
|
||||
void _outputScanDataFile_self(char* fileName, SVzNL3DLaserLine* scanData, int lineNum,
|
||||
float lineV, int maxTimeStamp, int clockPerSecond)
|
||||
{
|
||||
@ -359,7 +412,7 @@ void _outputScanDataFile_RGBD_obj(
|
||||
char* fileName,
|
||||
std::vector<std::vector< SVzNL3DPosition>>& scanData,
|
||||
float lineV, int maxTimeStamp, int clockPerSecond,
|
||||
std::vector<SWD_motorStatorPosition>& resultObjPositions)
|
||||
std::vector<SWD_statorInnerGrasper>& resultObjPositions)
|
||||
{
|
||||
int lineNum = (int)scanData.size();
|
||||
std::ofstream sw(fileName);
|
||||
@ -396,43 +449,8 @@ void _outputScanDataFile_RGBD_obj(
|
||||
for (int i = 0; i < nPositionCnt; i++)
|
||||
{
|
||||
SVzNL3DPosition& pt3D = scanData[line][i];
|
||||
int type = pt3D.nPointIdx;
|
||||
if (1 == type)
|
||||
{
|
||||
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 (4 == type) //
|
||||
{
|
||||
rgb = { 255, 255, 0 };
|
||||
size = 3;
|
||||
}
|
||||
else if (5 == type) //
|
||||
{
|
||||
rgb = { 255, 255, 0 };
|
||||
size = 3;
|
||||
}
|
||||
else
|
||||
{
|
||||
rgb = { 100, 100, 100 };
|
||||
size = 1;
|
||||
}
|
||||
rgb = { 250, 250, 250 };
|
||||
size = 1;
|
||||
|
||||
float x = (float)pt3D.pt3D.x;
|
||||
float y = (float)pt3D.pt3D.y;
|
||||
@ -442,90 +460,61 @@ void _outputScanDataFile_RGBD_obj(
|
||||
sw << "{" << rgb.r << "," << rgb.g << "," << rgb.b << "," << size << " }" << std::endl;
|
||||
}
|
||||
}
|
||||
#if 0
|
||||
//if (objOps.size() > 0)
|
||||
#if 1
|
||||
std::vector<SVzNL3DPoint> centerPoints;
|
||||
std::vector<SVzNL3DPoint> dirPoints;
|
||||
int ptNum = (int)resultObjPositions.size();
|
||||
if (ptNum > 0)
|
||||
{
|
||||
int ptNum = 3;
|
||||
sw << "Line_" << lineNum << "_" << (nTimeStamp + 1000) << "_" << ptNum << std::endl;
|
||||
|
||||
rgb = { 255, 0, 0 };
|
||||
for (int i = 0; i < ptNum; i++)
|
||||
{
|
||||
SVzNL3DPoint a_center = { resultObjPositions[i].opCenter.x, resultObjPositions[i].opCenter.y, resultObjPositions[i].opCenter.z };
|
||||
SVzNL3DPoint a_dir = { resultObjPositions[i].opCenter.x, resultObjPositions[i].opCenter.y, resultObjPositions[i].opCenter.z + 50 };
|
||||
centerPoints.push_back(a_center);
|
||||
dirPoints.push_back(a_dir);
|
||||
}
|
||||
|
||||
sw << "Line_" << lineNum << "_" << (nTimeStamp + 1000) << "_" << (ptNum+1) << std::endl;
|
||||
size = 10;
|
||||
float x = (float)wheelArcHeight.wheelArchPos.x;
|
||||
float y = (float)wheelArcHeight.wheelArchPos.y;
|
||||
float z = (float)wheelArcHeight.wheelArchPos.z;
|
||||
sw << "{" << x << "," << y << "," << z << "}-";
|
||||
for (int i = 0; i < ptNum; i++)
|
||||
{
|
||||
if(i == 0) rgb = { 255, 0, 0 };
|
||||
else rgb = { 255, 255, 0 };
|
||||
sw << "{" << centerPoints[i].x << "," << centerPoints[i].y << "," << centerPoints[i].z << "}-";
|
||||
sw << "{0,0}-{0,0}-";
|
||||
sw << "{" << rgb.r << "," << rgb.g << "," << rgb.b << "," << size << " }" << std::endl;
|
||||
}
|
||||
rgb = { 255, 0, 0 };
|
||||
sw << "{" << centerPoints[0].x << "," << centerPoints[0].y << "," << centerPoints[0].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 << "}-";
|
||||
//画出方向线
|
||||
size = 3;
|
||||
int lineIdx = 0;
|
||||
for (int i = 0; i < ptNum; i++)
|
||||
{
|
||||
if (i == 0) rgb = { 255, 0, 0 };
|
||||
else rgb = { 255, 255, 0 };
|
||||
sw << "Poly_" << lineIdx << "_2" << std::endl;
|
||||
sw << "{" << centerPoints[i].x << "," << centerPoints[i].y << "," << centerPoints[i].z << "}-";
|
||||
sw << "{0,0}-{0,0}-";
|
||||
sw << "{" << (int)rgb.r << "," << (int)rgb.g << "," << (int)rgb.b << "," << size << "}" << std::endl;
|
||||
sw << "{" << dirPoints[i].x << "," << dirPoints[i].y << "," << dirPoints[i].z << "}-";
|
||||
sw << "{0,0}-{0,0}-";
|
||||
sw << "{" << (int)rgb.r << "," << (int)rgb.g << "," << (int)rgb.b << "," << size << "}" << std::endl;
|
||||
lineIdx++;
|
||||
}
|
||||
|
||||
rgb = { 255, 0, 0 };
|
||||
sw << "Poly_" << lineIdx << "_2" << std::endl;
|
||||
sw << "{" << centerPoints[0].x << "," << centerPoints[0].y << "," << centerPoints[0].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 << "{" << (int)rgb.r << "," << (int)rgb.g << "," << (int)rgb.b << "," << size << "}" << std::endl;
|
||||
sw << "{" << dirPoints[0].x << "," << dirPoints[0].y << "," << dirPoints[0].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;
|
||||
sw << "{" << (int)rgb.r << "," << (int)rgb.g << "," << (int)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();
|
||||
}
|
||||
@ -767,6 +756,13 @@ void _genXOYProjectionImage(cv::String& fileName, SVzNL3DLaserLine* scanData, in
|
||||
#define TEST_GROUP 1
|
||||
int main()
|
||||
{
|
||||
const char* dataPath[TEST_GROUP] = {
|
||||
"F:/ShangGu/电机定子/数据1/", //0
|
||||
};
|
||||
|
||||
SVzNLRange fileIdx[TEST_GROUP] = {
|
||||
{4,18} };
|
||||
|
||||
#if CONVERT_TO_GRID
|
||||
//将数据转换成栅格格式格式
|
||||
char _scan_dir[256];
|
||||
@ -775,12 +771,12 @@ int main()
|
||||
int dataCalib = 0;
|
||||
int maxTimeStamp = 0;
|
||||
int clockPerSecond = 0;
|
||||
sprintf_s(_scan_dir, "E:\\上古\\电机定子\\");
|
||||
int cvt_grp = 0;
|
||||
char _scan_file[256];
|
||||
double _F = 1300; //f
|
||||
for (int i = 1; i <= 1; i++)
|
||||
double _F = 1299.956; //f
|
||||
for (int i = 4; i <= 18; i++)
|
||||
{
|
||||
sprintf_s(_scan_file, "%sLaserLine%d.txt", _scan_dir, i);
|
||||
sprintf_s(_scan_file, "%sLaserData%d.txt", dataPath[cvt_grp], i);
|
||||
SVzNLXYZRGBDLaserLine* laser3DPoints_RGBD = vzReadLaserScanPointFromFile_XYZRGB(_scan_file, &lineNum, &lineV, &dataCalib, &maxTimeStamp, &clockPerSecond);
|
||||
if (laser3DPoints_RGBD == NULL)
|
||||
continue;
|
||||
@ -794,7 +790,7 @@ int main()
|
||||
SVzNL3DLaserLine* gridData = _convertToGridData_XYZRGB(laser3DPoints_RGBD, lineNum, _F, camPoseR, &vldLineNum);
|
||||
|
||||
char _out_file[256];
|
||||
sprintf_s(_out_file, "%sLaserLine%d_grid.txt", _scan_dir, i);
|
||||
sprintf_s(_out_file, "%sLaserLine%d_grid.txt", dataPath[cvt_grp], i);
|
||||
_outputScanDataFile_self(_out_file, gridData, vldLineNum,
|
||||
lineV, maxTimeStamp, clockPerSecond);
|
||||
printf("%s: convert done!\n", _scan_file);
|
||||
@ -802,12 +798,6 @@ int main()
|
||||
#endif
|
||||
|
||||
#if TEST_COMPUTE_GRASP_POS
|
||||
const char* dataPath[TEST_GROUP] = {
|
||||
"F:/ShangGu/电机定子/数据1/", //0
|
||||
};
|
||||
|
||||
SVzNLRange fileIdx[TEST_GROUP] = {
|
||||
{1,1} };
|
||||
|
||||
|
||||
int endGroup = TEST_GROUP - 1;
|
||||
@ -824,14 +814,14 @@ int main()
|
||||
poseCalibPara.planeCalib[6] = 0.0;
|
||||
poseCalibPara.planeCalib[7] = 0.0;
|
||||
poseCalibPara.planeCalib[8] = 1.0;
|
||||
poseCalibPara.planeHeight = 2600.0;
|
||||
poseCalibPara.planeHeight = 645.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");
|
||||
sprintf_s(calibFile, "%sground_calib_para.txt", dataPath[grp]);
|
||||
poseCalibPara = _readCalibPara(calibFile);
|
||||
}
|
||||
#endif
|
||||
@ -839,6 +829,10 @@ int main()
|
||||
SWD_statorParam statorParam;
|
||||
statorParam.statorOuterD = 85.0;
|
||||
statorParam.statorInnerD = 50.0;
|
||||
statorParam.statorHeight = 100.0; //定子高度
|
||||
statorParam.plateThickness = 8.0; //隔板厚度
|
||||
statorParam.plateW = 500.0;//隔板长
|
||||
statorParam.plateH = 300.0;//隔板宽
|
||||
|
||||
SWD_statorPositonParam algoParam;
|
||||
memset(&algoParam, 0, sizeof(SWD_statorPositonParam));
|
||||
@ -859,11 +853,10 @@ int main()
|
||||
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++)
|
||||
{
|
||||
SWD_statorGriperState stateMachine;
|
||||
memset(&stateMachine, 0, sizeof(SWD_statorGriperState));
|
||||
//fidx = 4;
|
||||
int lineNum = 0;
|
||||
float lineV = 0.0f;
|
||||
@ -880,23 +873,21 @@ int main()
|
||||
|
||||
long t1 = GetTickCount64();
|
||||
int errCode = 0;
|
||||
std::vector<SWD_motorStatorPosition> resultObjPositions;
|
||||
SWD_statorOuterGrasper resultGraspPos;
|
||||
std::vector<SWD_statorInnerGrasper> resultObjPositions;
|
||||
wd_motorStatorPosition(
|
||||
scanLines,
|
||||
statorParam,
|
||||
poseCalibPara,
|
||||
algoParam,
|
||||
&refPos, //上一次给出的参考位置，同时输出下一次的参考位置
|
||||
&stateMachine, //操作状态机。指示当前状态
|
||||
&errCode,
|
||||
resultObjPositions,
|
||||
resultGraspPos);
|
||||
resultObjPositions);
|
||||
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,scanLines, 0,0,0, resultObjPositions);
|
||||
_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 }; //
|
||||
|
||||
@ -441,9 +441,9 @@ typedef struct
|
||||
|
||||
typedef struct
|
||||
{
|
||||
SVzNL3DPoint opCenter; //定子中心位置
|
||||
double objR;
|
||||
}SWD_motorStatorPosition;
|
||||
int objID;
|
||||
SSG_6DOF opCenter; //定子中心位置
|
||||
}SWD_statorInnerGrasper;
|
||||
|
||||
typedef struct
|
||||
{
|
||||
|
||||
@ -2864,7 +2864,7 @@ void pointCloud2DQuantization(
|
||||
#endif
|
||||
{
|
||||
SVzNL2DPoint v2i = { line, i };
|
||||
backIndexing[px][px] = v2i;
|
||||
backIndexing[px][py] = v2i;
|
||||
quantiData[px][py].z = pt3D->pt3D.z;
|
||||
//´¹Ö±²åÖµ
|
||||
if (prePt)
|
||||
@ -2905,11 +2905,11 @@ void pointCloud2DQuantization(
|
||||
int pixWin = (int)(inerPolateDistTh / scale);
|
||||
int cols = (int)quantiData[0].size();
|
||||
int rows = (int)quantiData.size();
|
||||
for (int y = 0; y < rows; y++) //和激光扫描方向一致
|
||||
for (int y = 0; y < cols; y++) //和激光扫描方向一致
|
||||
{
|
||||
int pre_x = -1;
|
||||
double pre_value = -1;
|
||||
for (int x = 0; x < cols; x++)
|
||||
for (int x = 0; x < rows; x++)
|
||||
{
|
||||
double value = quantiData[x][y].z;
|
||||
if (value > 1e-4)
|
||||
|
||||
@ -295,6 +295,9 @@ void wd_gridPointClustering_labelling_2(
|
||||
int i = 0;
|
||||
int lineNum = (int)srcData.size();
|
||||
int linePtNum = (int)srcData[0].size();
|
||||
|
||||
SVzNL2DPoint& inputSeed = a_cluster[0];
|
||||
labelMask[inputSeed.x][inputSeed.y].flag = 1; //第一个种子
|
||||
while (1)
|
||||
{
|
||||
if (i >= a_cluster.size())
|
||||
|
||||
@ -25,6 +25,7 @@
|
||||
#define SX_ERR_INVLID_CUTTING_Z -2101
|
||||
#define SX_ERR_ZERO_OBJ_TOPLAYER -2102
|
||||
#define SX_ERR_ZERO_OBJ_BTMLAYER -2103
|
||||
#define SX_ERR_GET_INVALID_PALTE -2104 //¸ô°åÌáÈ¡´íÎó
|
||||
|
||||
//뀔관
|
||||
#define SX_BAG_TRAY_EMPTY -2201
|
||||
|
||||
@ -235,7 +235,7 @@ void wd_bagThreadPositioning(
|
||||
}
|
||||
lineInterval = lineInterval / lineIntervalNum;
|
||||
|
||||
SVzNLRangeD jumpHeight = { scanInfo.mark_height-2.0, scanInfo.mark_height+2.0};
|
||||
SVzNLRangeD jumpHeight = { scanInfo.mark_height-3.0, scanInfo.mark_height+3.0};
|
||||
double markRotateAngle = 0;
|
||||
SVzNL3DPoint markCenter = { 0,0,0 };
|
||||
std::vector< SVzNL2DPoint> debug_markOrigin;
|
||||
@ -424,8 +424,8 @@ void wd_bagThreadPositioning(
|
||||
{
|
||||
double w = markClustersInfo[i].roi3D.xRange.max - markClustersInfo[i].roi3D.xRange.min;
|
||||
double h = markClustersInfo[i].roi3D.yRange.max - markClustersInfo[i].roi3D.yRange.min;
|
||||
if ((w >= scanInfo.mark_diameter - 1.0) && (w <= scanInfo.mark_diameter + 1.0) &&
|
||||
(h >= scanInfo.mark_diameter - 1.0) && (h <= scanInfo.mark_diameter + 1.0))
|
||||
if ((w >= scanInfo.mark_diameter - 1.5) && (w <= scanInfo.mark_diameter + 1.5) &&
|
||||
(h >= scanInfo.mark_diameter - 1.5) && (h <= scanInfo.mark_diameter + 1.5))
|
||||
{
|
||||
validMarks.push_back(i);
|
||||
}
|
||||
|
||||
File diff suppressed because it is too large
Load Diff
@ -13,10 +13,10 @@ typedef struct
|
||||
{
|
||||
double statorOuterD; //定子外直径
|
||||
double statorInnerD; //定子内孔直径
|
||||
double statorHeight;
|
||||
double cutZ_level0;
|
||||
double cutZ_level1;
|
||||
double gripperR;//夹爪半径
|
||||
double statorHeight; //定子高度
|
||||
double plateThickness; //隔板厚度
|
||||
double plateW;//隔板长
|
||||
double plateH;//隔板宽
|
||||
}SWD_statorParam;
|
||||
|
||||
typedef struct
|
||||
@ -26,11 +26,19 @@ typedef struct
|
||||
SSG_treeGrowParam growParam;
|
||||
}SWD_statorPositonParam;
|
||||
|
||||
typedef enum
|
||||
{
|
||||
keWD_OPERATE_Uknown = 0,
|
||||
keSG_OPERATE_TOP_LAYER, //抓取上层
|
||||
keSG_OPERATE_BTM_LAYER, //抓取下层
|
||||
keSG_OPERATE_PLATE, //抓取隔板
|
||||
} ESG_OpeationState;
|
||||
|
||||
typedef struct
|
||||
{
|
||||
SVzNL3DPoint refPos;
|
||||
double cuttingZ; //z截断值，用于分割定子上半部和地面
|
||||
}SWD_nextOpParam;
|
||||
ESG_OpeationState opState; //指示前一目标参考位置是否有效
|
||||
}SWD_statorGriperState;
|
||||
|
||||
//读版本号
|
||||
SG_APISHARED_EXPORT const char* wd_particleSegVersion(void);
|
||||
@ -53,11 +61,10 @@ SG_APISHARED_EXPORT void wd_lineDataR(
|
||||
// 投影（注意此时边框也同时投影）->距离变换->提取定子目标->
|
||||
// 根据相邻和边框寻找最佳抓取目标和抓取点
|
||||
SG_APISHARED_EXPORT void wd_motorStatorPosition(
|
||||
std::vector< std::vector<SVzNL3DPosition>>& scanLines,
|
||||
std::vector< std::vector<SVzNL3DPosition>>& scanLinesInput,
|
||||
const SWD_statorParam statorParam,
|
||||
const SSG_planeCalibPara groundCalibPara,
|
||||
const SWD_statorPositonParam algoParam,
|
||||
SWD_nextOpParam* refPos, //上一次给出的参考位置，同时输出下一次的参考位置
|
||||
SWD_statorGriperState* opState, //操作状态机。指示当前状态
|
||||
int* errCode,
|
||||
std::vector<SWD_motorStatorPosition>& resultObjPositions,
|
||||
SWD_statorOuterGrasper& resultGraspPos);
|
||||
std::vector<SWD_statorInnerGrasper>& resultObjPositions);
|
||||
@ -461,25 +461,27 @@ int main()
|
||||
pts_eye.resize(6);
|
||||
std::vector<cv::Point3d> pts_robot;
|
||||
pts_robot.resize(6);
|
||||
pts_eye[0] = { 187.22, -99.43, 1797.70 }; //, R = 0.6413, P = 0.0302, Y = -91.1494
|
||||
pts_eye[1] = { 186.50, -140.52, 1797.69 }; // R = 0.6413, P = 0.0302, Y = -88.8130
|
||||
pts_eye[2] = { 256.73, -93.55, 1797.83 }; //, R = 0.6413, P = 0.0302, Y = -89.3432
|
||||
pts_eye[3] = { 236.69, -48.82, 1797.77 }; // R = 0.6413, P = 0.0302, Y = -89.5285
|
||||
pts_eye[4] = { 173.62, 13.92, 1797.30 };// R = 0.6413, P = 0.0302, Y = -89.6450
|
||||
pts_eye[5] = { 232.11, -153.38, 1797.88 };// R = 0.6413, P = 0.0302, Y = -82.0278
|
||||
pts_eye[0] = { 463.48, 347.63, 1301.43 }; //, R = 0.6413, P = 0.0302, Y = -91.1494
|
||||
pts_eye[1] = { 254.15, 259.55, 1303.12 }; // R = 0.6413, P = 0.0302, Y = -88.8130
|
||||
pts_eye[2] = { 141.16, 284.02, 1302.27 }; //, R = 0.6413, P = 0.0302, Y = -89.3432
|
||||
pts_eye[3] = { 90.66, 5.80, 1303.27 }; // R = 0.6413, P = 0.0302, Y = -89.5285
|
||||
pts_eye[4] = { 252.47, -10.45, 1304.33 };// R = 0.6413, P = 0.0302, Y = -89.6450
|
||||
pts_eye[5] = { 403.53, -13.99, 1304.24 };// R = 0.6413, P = 0.0302, Y = -82.0278
|
||||
//pts_eye[6] = { 242.70, -74.51, 1796.83 };
|
||||
|
||||
//A:绕z, B：绕y， C:绕x
|
||||
pts_robot[0] = { 55.830, -984.472, 97.687 }; // A:0.018 B : -0.638 C : -0.292
|
||||
pts_robot[1] = { 95.469, -985.345, 97.782 }; // A:-1.032 B : -0.354 C : 1.001
|
||||
pts_robot[2] = { 47.908, -1053.709, 97.802 }; // A:-1.032 B : -0.356 C : 0.998
|
||||
pts_robot[3] = { 3.646, -1033.152, 96.494 }; // A:-1.028 B : -0.367 C : 0.442
|
||||
pts_robot[4] = { -58.843, -969.793, 95.297 }; // A:-1.029 B : -0.367 C : 0.442
|
||||
pts_robot[5] = { 108.031, -1029.803, 98.333 }; // A:-9.208 B : -0.367 C : 0.442
|
||||
pts_robot[0] = { -406.649, -1246.187, 588.611 }; // A:0.018 B : -0.638 C : -0.292
|
||||
pts_robot[1] = { -316.649, -1036.290, 587.619 }; // A:-1.032 B : -0.354 C : 1.001
|
||||
pts_robot[2] = { -340.590, -922.510, 586.731 }; // A:-1.032 B : -0.356 C : 0.998
|
||||
pts_robot[3] = { -61.892,-874.564,589.841 }; // A:-1.028 B : -0.367 C : 0.442
|
||||
pts_robot[4] = { -46.658,-1036.959,589.820 }; // A:-1.029 B : -0.367 C : 0.442
|
||||
pts_robot[5] = { -43.92, -1188.370,591.006 }; // A:-9.208 B : -0.367 C : 0.442
|
||||
//pts_robot[6] = { 29.298, -1033.69, 97.909 };
|
||||
|
||||
//使用前6组数据
|
||||
std::vector<cv::Point3d> test_pts_eye;
|
||||
std::vector<cv::Point3d> test_pts_robot;
|
||||
for (int i = 0; i < 5; i++)
|
||||
for (int i = 0; i < 6; i++)
|
||||
{
|
||||
test_pts_eye.push_back(pts_eye[i]);
|
||||
test_pts_robot.push_back(pts_robot[i]);
|
||||
@ -501,21 +503,30 @@ int main()
|
||||
|
||||
//验算6轴姿态
|
||||
std::vector<cv::Point3d> verify_pts_eye;
|
||||
verify_pts_eye.insert(verify_pts_eye.end(), pts_eye.begin(), pts_eye.end());
|
||||
cv::Point3d a_center = { 232.997, -173.533, 1795.9 };
|
||||
verify_pts_eye.push_back(a_center);
|
||||
verify_pts_eye.insert(verify_pts_eye.end(), pts_eye.begin(), pts_eye.begin()+6);
|
||||
//cv::Point3d a_center = { 232.997, -173.533, 1795.9 };
|
||||
//verify_pts_eye.push_back(a_center);
|
||||
//a_center = { 225.57, -68.33, 1796.77 };
|
||||
//verify_pts_eye.push_back(a_center);
|
||||
//a_center = { 120.45, -45.97, 1796.25 };
|
||||
//verify_pts_eye.push_back(a_center);
|
||||
//a_center = { 242.70, -74.51, 1796.83 };
|
||||
//verify_pts_eye.push_back(a_center);
|
||||
std::vector<std::vector< cv::Point3d>> pose_eye;
|
||||
pose_eye.resize(7);
|
||||
for (int i = 0; i < 7; i++)
|
||||
pose_eye.resize(6);
|
||||
for (int i = 0; i < 6; i++)
|
||||
pose_eye[i].resize(3);
|
||||
pose_eye[0][0] = { -0.020, -1.000, -0.011 }; pose_eye[0][1] = { 1.000, -0.020, -0.001 }; pose_eye[0][2] = { 0.001, -0.011, 1.000 };
|
||||
pose_eye[1][0] = { 0.021,-1.000,-0.011 }; pose_eye[1][1] = { 1.000,0.021,-0.000 }; pose_eye[1][2] = { 0.001,-0.011,1.000 };
|
||||
pose_eye[2][0] = { 0.011,-1.000,-0.011 }; pose_eye[2][1] = { 1.000,0.011,-0.000 }; pose_eye[2][2] = { 0.001,-0.011,1.000 };
|
||||
pose_eye[3][0] = { 0.008,-1.000,-0.011 }; pose_eye[3][1] = { 1.000,0.008,-0.000 }; pose_eye[3][2] = { 0.001,-0.011,1.000 };
|
||||
pose_eye[4][0] = { 0.006,-1.000,-0.011 }; pose_eye[4][1] = { 1.000,0.006,-0.000 }; pose_eye[4][2] = { 0.001,-0.011,1.000 };
|
||||
pose_eye[5][0] = { 0.139,-0.990,-0.011 }; pose_eye[5][1] = { 0.990,0.139,0.001 }; pose_eye[5][2] = { 0.001,-0.011,1.000 };
|
||||
pose_eye[6][0] = { 0.136746, -0.990563, -0.00926168 }; pose_eye[6][1] = { 0.990606, 0.136747, 0.000517805 }; pose_eye[6][2] = { 0.000753588, -0.00924548, 0.999957 };
|
||||
for (int i = 0; i < 7; i++)
|
||||
pose_eye[0][0] = { -0.046,-0.999,-0.009 }; pose_eye[0][1] = { 0.999,-0.046,0.004 }; pose_eye[0][2] = { -0.005,-0.009,1.000 };
|
||||
pose_eye[1][0] = { -0.049,-0.999,-0.009 }; pose_eye[1][1] = { 0.999,-0.050,0.004 }; pose_eye[1][2] = { -0.005,-0.009,1.000 };
|
||||
pose_eye[2][0] = { -0.049,-0.999,-0.009 }; pose_eye[2][1] = { 0.999,-0.049,0.004 }; pose_eye[2][2] = { -0.005,-0.009,1.000 };
|
||||
pose_eye[3][0] = { -0.059,-0.998,-0.009 }; pose_eye[3][1] = { 0.998,-0.059,0.004 }; pose_eye[3][2] = { -0.005,-0.009,1.000 };
|
||||
pose_eye[4][0] = { -0.070,-0.997,-0.009 }; pose_eye[4][1] = { 0.998,-0.070,0.004 }; pose_eye[4][2] = { -0.005,-0.009,1.000 };
|
||||
pose_eye[5][0] = { -0.068,-0.998,-0.009 }; pose_eye[5][1] = { 0.998,-0.068,0.004 }; pose_eye[5][2] = { -0.005,-0.009,1.000 };
|
||||
//pose_eye[6][0] = { 0.136746, -0.990563, -0.00926168 }; pose_eye[6][1] = { 0.990606, 0.136747, 0.000517805 }; pose_eye[6][2] = { 0.000753588, -0.00924548, 0.999957 };
|
||||
//pose_eye[7][0] = { 0.058, 0.998, 0.011 }; pose_eye[7][1] = { 0.999, -0.046, 0.003 }; pose_eye[7][2] = { 0.003, 0.011, -1.000 };
|
||||
//pose_eye[8][0] = { 0.017,1.000,0.011 }; pose_eye[8][1] = { 1.000,-0.008,0.003 }; pose_eye[8][2] = { 0.003,0.011,-1.000 };
|
||||
//pose_eye[9][0] = { -0.316, 0.949, 0.009 }; pose_eye[9][1] = { 0.951, 0.310, 0.007 }; pose_eye[9][2] = { 0.003, 0.011, -1.000 };
|
||||
for (int i = 0; i < 6; i++)
|
||||
{
|
||||
cv::Point3d rtPt;
|
||||
pointRT_2(R, T, verify_pts_eye[i], rtPt); //RT前后的点
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user