jerryzeng/algoLib
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

修改朝向显示

Browse Source
This commit is contained in:
jerryzeng 2025-07-16 19:58:32 +08:00
parent 497b98e2c3
commit 724a5a15e6
1 changed files with 31 additions and 9 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

40
bagPositioning_test/bagPositioning_test.cpp
View File

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