diff --git a/rodAndBarDetection_test/rodAndBarDetection_test.cpp b/rodAndBarDetection_test/rodAndBarDetection_test.cpp index a067a68..0313203 100644 --- a/rodAndBarDetection_test/rodAndBarDetection_test.cpp +++ b/rodAndBarDetection_test/rodAndBarDetection_test.cpp @@ -228,9 +228,11 @@ void _outputPlatePiseInfo(char* fileName, SSX_pointPoseInfo& centerInfo) { std::ofstream sw(fileName); char dataStr[250]; - sprintf_s(dataStr, 250, "定位盘: center_( %g, %g, %g ), dir_( %g, %g, %g )", + sprintf_s(dataStr, 250, "定位盘: center_( %g, %g, %g ), normalDir_( %g, %g, %g ), xDir_( %g, %g, %g ), yDir_( %g, %g, %g )", centerInfo.center.x, centerInfo.center.y, centerInfo.center.z, - centerInfo.normalDir.x, centerInfo.normalDir.y, centerInfo.normalDir.z); + centerInfo.normalDir.x, centerInfo.normalDir.y, centerInfo.normalDir.z, + centerInfo.xDir.x, centerInfo.xDir.y, centerInfo.xDir.z, + centerInfo.yDir.x, centerInfo.yDir.y, centerInfo.yDir.z); sw << dataStr << std::endl; sw.close(); } @@ -260,7 +262,7 @@ void _outputWeldSeamInfo(char* fileName, std::vector& weldSeam int objNum = (int)weldSeamInfo.size(); for (int i = 0; i < objNum; i++) { - sprintf_s(dataStr, 250, "螺杆_%d: center_( %g, %g, %g ), dir_normal_( %g, %g, %g )", + sprintf_s(dataStr, 250, "螺杆_%d: center_( %g, %g, %g ), dir_normal_( %g, %g, %g ), x_dir_(", i + 1, weldSeamInfo[i].center.x, weldSeamInfo[i].center.y, weldSeamInfo[i].center.z, weldSeamInfo[i].normalDir.x, weldSeamInfo[i].normalDir.y, weldSeamInfo[i].normalDir.z); sw << dataStr << std::endl; @@ -496,6 +498,14 @@ void _outputRGBDScan_RGBD_centerPose( SVzNL3DPoint pt1 = { poseInfo.center.x + len * poseInfo.normalDir.x, poseInfo.center.y + len * poseInfo.normalDir.y, poseInfo.center.z + len * poseInfo.normalDir.z }; + + SVzNL3DPoint basePt = poseInfo.center; + SVzNL3DPoint pt2 = { poseInfo.center.x + len * poseInfo.xDir.x, + poseInfo.center.y + len * poseInfo.xDir.y, + poseInfo.center.z + len * poseInfo.xDir.z }; + SVzNL3DPoint pt3 = { poseInfo.center.x + len * poseInfo.yDir.x, + poseInfo.center.y + len * poseInfo.yDir.y, + poseInfo.center.z + len * poseInfo.yDir.z }; //显示法向量 sw << "Poly_" << lineIdx << "_2" << std::endl; sw << "{" << (float)pt0.x << "," << (float)pt0.y << "," << (float)pt0.z << "}-"; @@ -505,6 +515,26 @@ void _outputRGBDScan_RGBD_centerPose( sw << "{0,0}-{0,0}-"; sw << "{" << (int)rgb.r << "," << (int)rgb.g << "," << (int)rgb.b << "," << size << "}" << std::endl; lineIdx++; + + rgb = { 0, 250, 0 }; + sw << "Poly_" << lineIdx << "_2" << std::endl; + sw << "{" << (float)basePt.x << "," << (float)basePt.y << "," << (float)basePt.z << "}-"; + sw << "{0,0}-{0,0}-"; + sw << "{" << (int)rgb.r << "," << (int)rgb.g << "," << (int)rgb.b << "," << size << "}" << std::endl; + sw << "{" << pt2.x << "," << pt2.y << "," << pt2.z << "}-"; + sw << "{0,0}-{0,0}-"; + sw << "{" << (int)rgb.r << "," << (int)rgb.g << "," << (int)rgb.b << "," << size << "}" << std::endl; + lineIdx++; + + rgb = { 0, 0, 250 }; + sw << "Poly_" << lineIdx << "_2" << std::endl; + sw << "{" << (float)basePt.x << "," << (float)basePt.y << "," << (float)basePt.z << "}-"; + sw << "{0,0}-{0,0}-"; + sw << "{" << (int)rgb.r << "," << (int)rgb.g << "," << (int)rgb.b << "," << size << "}" << std::endl; + sw << "{" << pt3.x << "," << pt3.y << "," << pt3.z << "}-"; + sw << "{0,0}-{0,0}-"; + sw << "{" << (int)rgb.r << "," << (int)rgb.g << "," << (int)rgb.b << "," << size << "}" << std::endl; + lineIdx++; } } sw.close(); diff --git a/sourceCode/rodAndBarDetection.cpp b/sourceCode/rodAndBarDetection.cpp index adb7c48..b925adf 100644 --- a/sourceCode/rodAndBarDetection.cpp +++ b/sourceCode/rodAndBarDetection.cpp @@ -9,7 +9,8 @@ //version 1.1.0 : ˵ƽͰĶλ //version 1.1.1 : ʼͻİ汾 //version 1.2.0 : ݸ˲˶λIJ -std::string m_strVersion = "1.2.0"; +//version 1.2.1 : ˶λ̬ +std::string m_strVersion = "1.2.1"; const char* wd_rodAndBarDetectionVersion(void) { return m_strVersion.c_str(); @@ -832,6 +833,8 @@ SSX_pointPoseInfo sx_getLocationPlatePose( double center_z = (ptTop.z + ptBtm.z + ptLeft.z + ptRight.z) / 4; resultPose.center = { center_x, center_y, center_z }; resultPose.normalDir = { 0, 0, -1.0 }; + resultPose.yDir = { 0, -1.0, 0 }; + resultPose.xDir = { 1.0, 0, 0 }; //תȥ for (int i = 0; i < lineNum; i++) { @@ -847,6 +850,17 @@ SSX_pointPoseInfo sx_getLocationPlatePose( y = resultPose.normalDir.x * poseR.invRMatrix[3] + resultPose.normalDir.y * poseR.invRMatrix[4] + resultPose.normalDir.z * poseR.invRMatrix[5]; z = resultPose.normalDir.x * poseR.invRMatrix[6] + resultPose.normalDir.y * poseR.invRMatrix[7] + resultPose.normalDir.z * poseR.invRMatrix[8]; resultPose.normalDir = { x, y, z }; + + x = resultPose.xDir.x * poseR.invRMatrix[0] + resultPose.xDir.y * poseR.invRMatrix[1] + resultPose.xDir.z * poseR.invRMatrix[2]; + y = resultPose.xDir.x * poseR.invRMatrix[3] + resultPose.xDir.y * poseR.invRMatrix[4] + resultPose.xDir.z * poseR.invRMatrix[5]; + z = resultPose.xDir.x * poseR.invRMatrix[6] + resultPose.xDir.y * poseR.invRMatrix[7] + resultPose.xDir.z * poseR.invRMatrix[8]; + resultPose.xDir = { x, y, z }; + + x = resultPose.yDir.x * poseR.invRMatrix[0] + resultPose.yDir.y * poseR.invRMatrix[1] + resultPose.yDir.z * poseR.invRMatrix[2]; + y = resultPose.yDir.x * poseR.invRMatrix[3] + resultPose.yDir.y * poseR.invRMatrix[4] + resultPose.yDir.z * poseR.invRMatrix[5]; + z = resultPose.yDir.x * poseR.invRMatrix[6] + resultPose.yDir.y * poseR.invRMatrix[7] + resultPose.yDir.z * poseR.invRMatrix[8]; + resultPose.yDir = { x, y, z }; + return resultPose; } diff --git a/sourceCode/rodAndBarDetection_Export.h b/sourceCode/rodAndBarDetection_Export.h index a3cadb6..75b2fe8 100644 --- a/sourceCode/rodAndBarDetection_Export.h +++ b/sourceCode/rodAndBarDetection_Export.h @@ -14,6 +14,8 @@ typedef struct typedef struct { SVzNL3DPoint center; //ݸ˶˲ĵ + SVzNL3DPoint xDir; + SVzNL3DPoint yDir; SVzNL3DPoint normalDir; // }SSX_pointPoseInfo; // diff --git a/sourceCode/wheelArchHeigthMeasure.cpp b/sourceCode/wheelArchHeigthMeasure.cpp index 296c0fc..b651dee 100644 --- a/sourceCode/wheelArchHeigthMeasure.cpp +++ b/sourceCode/wheelArchHeigthMeasure.cpp @@ -13,7 +13,8 @@ //version 1.3.2 : ԲƸĽ㷨ǿ³ //version 1.3.3 : üȡ˸Ľ˿ܵȡ //version 1.3.4 : üĸ߶ȼ㷽Y߶Ȳ޸Ϊ -std::string m_strVersion = "1.3.4"; +//version 1.3.5 : Ľüȡ㷽 +std::string m_strVersion = "1.3.5"; const char* wd_wheelArchHeigthMeasureVersion(void) { return m_strVersion.c_str(); @@ -519,6 +520,40 @@ void _genEdgeContinuousPos(SSG_endingTree&edgeTree, std::vector& e } } +SVzNL2DPoint _getRealArcPos(SVzNL2DPoint seedPos, double a, double b, double c, std::vector& lineData, double chkRng_Y) +{ + SVzNL3DPosition seedPt = lineData[seedPos.y]; + + double minDist = -1; + int idx = -1; + for (int i = seedPos.y; i >= 0; i--) + { + if ( lineData[i].pt3D.z < 1e-4) + continue; + if (lineData[i].nPointIdx != 2) + break; + + double yLen = abs(lineData[i].pt3D.y - seedPt.pt3D.y); + if (yLen > chkRng_Y) + break; + + double dist = abs(lineData[i].pt3D.y * a + lineData[i].pt3D.z * b + c); + if (minDist < 0) + { + minDist = dist; + idx = i; + } + else if(minDist > dist) + { + minDist = dist; + idx = i; + } + } + if (idx >= 0) + seedPos.y = idx; + return seedPos; +} + //ü߶Ȳ WD_wheelArchInfo wd_wheelArchHeigthMeasure( std::vector< std::vector>& scanLines, @@ -937,6 +972,29 @@ WD_wheelArchInfo wd_wheelArchHeigthMeasure( *errCode = SX_ERR_NO_WHEEL_ARC; return result; } + + //ü㣺ʹ45ߣȡеΪü + double chkRng_Y = 50.0; +#if 0 + double vLine_a = downWheelPt.z - upWheelPt.z; + double vLine_b = upWheelPt.y - downWheelPt.y; + double vLine_c = downWheelPt.y * upWheelPt.z - upWheelPt.y * downWheelPt.z; + //ת45 + // תֱߵϵѧƵ + double r_a = vLine_a + vLine_b; + double r_b = vLine_b - vLine_a; + double r_c = -r_a * upWheelPt.y - r_b * upWheelPt.z; +#else + //ת30 + double vAngle = atan2(downWheelPt.z - upWheelPt.z, downWheelPt.y - upWheelPt.y); + vAngle += (60.0 / 180.0) * PI; + double r_a = tan(vAngle); + double r_b = -1.0; + double r_c = upWheelPt.z - r_a * upWheelPt.y; +#endif + arcPos = _getRealArcPos(arcPos, r_a, r_b, r_c, scanLines[arcPos.x], chkRng_Y); + + //XYƽϵ double outLineLen = 200; SVzNL3DPoint arcPt = scanLines[arcPos.x][arcPos.y].pt3D; diff --git a/wheelArchHeigthMeasure_test/wheelArchHeigthMeasure_test.cpp b/wheelArchHeigthMeasure_test/wheelArchHeigthMeasure_test.cpp index 90b627e..acf38fc 100644 --- a/wheelArchHeigthMeasure_test/wheelArchHeigthMeasure_test.cpp +++ b/wheelArchHeigthMeasure_test/wheelArchHeigthMeasure_test.cpp @@ -973,7 +973,7 @@ void _outputScanDataFile_RGBD_obj(char* fileName, std::vector 0) { - int ptNum = 3; + int ptNum = 4; sw << "Line_" << lineNum << "_" << (nTimeStamp + 1000) << "_" << ptNum << std::endl; rgb = { 255, 0, 0 }; @@ -1000,6 +1000,13 @@ void _outputScanDataFile_RGBD_obj(char* fileName, std::vector