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; //