From 724a5a15e66f5009e4a3d61fa6e9a43bba5cbb81 Mon Sep 17 00:00:00 2001 From: jerryzeng Date: Wed, 16 Jul 2025 19:58:32 +0800 Subject: [PATCH] =?UTF-8?q?=E4=BF=AE=E6=94=B9=E6=9C=9D=E5=90=91=E6=98=BE?= =?UTF-8?q?=E7=A4=BA?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- bagPositioning_test/bagPositioning_test.cpp | 40 ++++++++++++++++----- 1 file changed, 31 insertions(+), 9 deletions(-) diff --git a/bagPositioning_test/bagPositioning_test.cpp b/bagPositioning_test/bagPositioning_test.cpp index 5c77ba2..d785b19 100644 --- a/bagPositioning_test/bagPositioning_test.cpp +++ b/bagPositioning_test/bagPositioning_test.cpp @@ -1660,20 +1660,42 @@ void _XOYprojection_RGBD( const double deg2rad = PI / 180.0; const double yaw = objOps[i].centerPos.z_yaw * deg2rad; double cy = cos(yaw); double sy = sin(yaw); - double x1 = objOps[i].centerPos.x + R * cy; double y1 = objOps[i].centerPos.y - R * sy; - double x2 = objOps[i].centerPos.x - R * cy; double y2 = objOps[i].centerPos.y + R * sy; - int px1 = (int)((x1 - x_range.min) / x_scale + x_skip); - int py1 = (int)((y1 - y_range.min) / y_scale + y_skip); - int px2 = (int)((x2 - x_range.min) / x_scale + x_skip); - int py2 = (int)((y2 - y_range.min) / y_scale + y_skip); + double arrowLen = R/3; + double arrowAngle = 30; + double ca = cos(arrowAngle * deg2rad); + double sa = sin(arrowAngle * deg2rad); + SVzNL2DPointD endingPt[4]; + endingPt[0] = { R, 0 }; + endingPt[1] = { -R, 0 }; + endingPt[2] = { R - arrowLen * ca, -arrowLen * sa }; + endingPt[3] = { R - arrowLen * ca, arrowLen * sa }; + for (int m = 0; m < 4; m++) + { + double tmp_x = endingPt[m].x * cy - endingPt[m].y * sy; + double tmp_y = -endingPt[m].x * sy - endingPt[m].y * cy; + endingPt[m].x = tmp_x + objOps[i].centerPos.x; + endingPt[m].y = tmp_y + objOps[i].centerPos.y; + } + int px1 = (int)((endingPt[0].x - x_range.min) / x_scale + x_skip); + int py1 = (int)((endingPt[0].y - y_range.min) / y_scale + y_skip); + int px2 = (int)((endingPt[1].x - x_range.min) / x_scale + x_skip); + int py2 = (int)((endingPt[1].y - y_range.min) / y_scale + y_skip); cv::line(img, cv::Point(px1, py1), cv::Point(px2, py2), cv::Scalar(rgb[2], rgb[1], rgb[0]), 2); + int px3 = (int)((endingPt[2].x - x_range.min) / x_scale + x_skip); + int py3 = (int)((endingPt[2].y - y_range.min) / y_scale + y_skip); + int px4 = (int)((endingPt[3].x - x_range.min) / x_scale + x_skip); + int py4 = (int)((endingPt[3].y - y_range.min) / y_scale + y_skip); if (objOps[i].orienFlag == 1) { - cv::circle(img, cv::Point(px1, py1), 5, cv::Scalar(0, 255, 0), -1); + cv::line(img, cv::Point(px1, py1), cv::Point(px3, py3), cv::Scalar(0, 255, 0), 2); + cv::line(img, cv::Point(px1, py1), cv::Point(px4, py4), cv::Scalar(0, 255, 0), 2); + //cv::circle(img, cv::Point(px1, py1), 5, cv::Scalar(0, 255, 0), -1); } else if (objOps[i].orienFlag == 2) { - cv::circle(img, cv::Point(px1, py1), 5, cv::Scalar(0, 0, 255), -1); + cv::line(img, cv::Point(px1, py1), cv::Point(px3, py3), cv::Scalar(0, 0, 255), 2); + cv::line(img, cv::Point(px1, py1), cv::Point(px4, py4), cv::Scalar(0, 0, 255), 2); + //cv::circle(img, cv::Point(px1, py1), 5, cv::Scalar(0, 0, 255), -1); } } } @@ -2682,7 +2704,7 @@ int main() } for (int fidx = fileIdx[grp].nMin; fidx <= fileIdx[grp].nMax; fidx++) { - fidx = 10; + //fidx = 10; if (grp < TEST_TOP_VIEW_GROUP) //正面抓取 { int lineNum = 0;