完善rpy计算

This commit is contained in:
2024-06-03 03:20:35 +08:00
parent 25ab3ca8f6
commit b5db01588a
5 changed files with 471 additions and 14 deletions
@@ -1,7 +1,415 @@
#include <back/threadPoolManager.h>
#include <halconAlgo/Correction.h>
#include <iostream>
struct Points
{
double OriginX, OriginY, OriginZ;
double xX, xY, xZ;
double yX, yY, yZ;
double zX, zY, zZ;
};
struct CorrectionPoints
{
std::vector<double> Origin;//X Y Z
std::vector<double> Result_X;//X Y Z
std::vector<double> Result_Y;//X Y Z
std::vector<double> Result_Z;//X Y Z
double AngleZX = -1;
double AngleYZ = -1;
double AngleXY = -1;
double ofx1, ofx2, ofx3;
double ofy1, ofy2, ofy3;
double ofz1, ofz2, ofz3;
};
using namespace cv;
void calculateRotationMatrix(const Points& points, double rotationMatrix[3][3]) {
rotationMatrix[0][0] = points.xX;
rotationMatrix[0][1] = points.yX;
rotationMatrix[0][2] = points.zX;
rotationMatrix[1][0] = points.xY;
rotationMatrix[1][1] = points.yY;
rotationMatrix[1][2] = points.zY;
rotationMatrix[2][0] = points.xZ;
rotationMatrix[2][1] = points.yZ;
rotationMatrix[2][2] = points.zZ;
}
// 从旋转矩阵中提取RPY值
void extractRPY(const double rotationMatrix[3][3], double& roll, double& pitch, double& yaw) {
// 计算pitch
pitch = atan2(-rotationMatrix[2][0], sqrt(rotationMatrix[2][1] * rotationMatrix[2][1] + rotationMatrix[2][2] * rotationMatrix[2][2])) * (180.0 / M_PI);
// 计算yaw
yaw = atan2(rotationMatrix[1][0], rotationMatrix[0][0]) * (180.0 / M_PI);
// 计算roll
roll = atan2(rotationMatrix[2][1], rotationMatrix[2][2]) * (180.0 / M_PI);
}
// 计算旋转角度
Point3d calculateRotationAngles(const Points& point1, const Points& point2)
{
// 构建两个旋转矩阵
cv::Mat R1 = (cv::Mat_<double>(3, 3) <<
point1.xX, point1.yX, point1.zX,
point1.xY, point1.yY, point1.zY,
point1.xZ, point1.yZ, point1.zZ);
cv::Mat R2 = (cv::Mat_<double>(3, 3) <<
point2.xX, point2.yX, point2.zX,
point2.xY, point2.yY, point2.zY,
point2.xZ, point2.yZ, point2.zZ);
// 计算从旋转矩阵R1到R2的旋转矩阵
cv::Mat rotation = R2 * R1.t(); // 转置矩阵R1
// 使用OpenCV的Rodrigues函数将旋转矩阵转换为旋转向量
cv::Mat rvec;
cv::Rodrigues(rotation, rvec);
// 将旋转向量转换为欧拉角
cv::Mat euler;
cv::Mat_<double> euler_angles(3, 1);
cv::Rodrigues(rvec, euler);
euler_angles = euler * (180.0 / CV_PI); // 转换为度数
return Point3d(euler_angles.at<double>(0), euler_angles.at<double>(1), euler_angles.at<double>(2));
}
std::vector<cv::Point3d> getoffsetPos(CorrectionPoints& originPoints, CorrectionPoints& newPoints)
{
//CorrectionPoints returnPoints;
//returnPoints.Origin.push_back(originPoints.Origin[0] - newPoints.Origin[0]);
//returnPoints.Origin.push_back(originPoints.Origin[1] - newPoints.Origin[1]);
//returnPoints.Origin.push_back(originPoints.Origin[2] - newPoints.Origin[2]);
//returnPoints.Result_X.push_back(originPoints.Result_X[0] - newPoints.Result_X[0]);
//returnPoints.Result_X.push_back(originPoints.Result_X[1] - newPoints.Result_X[1]);
//returnPoints.Result_X.push_back(originPoints.Result_X[2] - newPoints.Result_X[2]);
//returnPoints.Result_Y.push_back(originPoints.Result_Y[0] - newPoints.Result_Y[0]);
//returnPoints.Result_Y.push_back(originPoints.Result_Y[1] - newPoints.Result_Y[1]);
//returnPoints.Result_Y.push_back(originPoints.Result_Y[2] - newPoints.Result_Y[2]);
//returnPoints.Result_Z.push_back(originPoints.Result_Z[0] - newPoints.Result_Z[0]);
//returnPoints.Result_Z.push_back(originPoints.Result_Z[1] - newPoints.Result_Z[1]);
//returnPoints.Result_Z.push_back(originPoints.Result_Z[2] - newPoints.Result_Z[2]);
//returnPoints.AngleZX = originPoints.AngleZX - newPoints.AngleZX;
//returnPoints.AngleYZ = originPoints.AngleYZ - newPoints.AngleYZ;
//returnPoints.AngleXY = originPoints.AngleXY - newPoints.AngleXY;
//// 第一次校准的数据
cv::Point3d origin1(0.0, 0.0, 0.0); // 原点坐标
cv::Point3d dir1_1(originPoints.ofx1, originPoints.ofx2, originPoints.ofx3); // 第一个方向的点
cv::Point3d dir1_2(originPoints.ofy1, originPoints.ofy2, originPoints.ofy3); // 第二个方向的点
cv::Point3d dir1_3(originPoints.ofz1, originPoints.ofz2, originPoints.ofz3); // 第三个方向的点
// 第二次校准的数据
cv::Point3d origin2(0.0, 0.0, 0.0); // 新的原点坐标
cv::Point3d dir2_1(newPoints.ofx1, newPoints.ofx2, newPoints.ofx3); // 第一个方向的点
cv::Point3d dir2_2(newPoints.ofy1, newPoints.ofy2, newPoints.ofy3); // 第二个方向的点
cv::Point3d dir2_3(newPoints.ofz1, newPoints.ofz2, newPoints.ofz3); // 第三个方向的点
// 创建矩阵来保存方向向量
cv::Mat R1 = (cv::Mat_<double>(3, 3) << dir1_1.x, dir1_2.x, dir1_3.x,
dir1_1.y, dir1_2.y, dir1_3.y,
dir1_1.z, dir1_2.z, dir1_3.z);
cv::Mat R2 = (cv::Mat_<double>(3, 3) << dir2_1.x, dir2_2.x, dir2_3.x,
dir2_1.y, dir2_2.y, dir2_3.y,
dir2_1.z, dir2_2.z, dir2_3.z);
// 计算旋转矩阵
cv::Mat R = R2 * R1.inv();
// 计算平移向量
cv::Mat T = (cv::Mat_<double>(3, 1) << origin2.x - origin1.x, origin2.y - origin1.y, origin2.z - origin1.z);
// 构建4x4变换矩阵
cv::Mat transformationMatrix = cv::Mat::eye(4, 4, CV_64F);
R.copyTo(transformationMatrix(cv::Rect(0, 0, 3, 3)));
T.copyTo(transformationMatrix(cv::Rect(3, 0, 1, 3)));
std::vector<cv::Point3d> basePoints = { cv::Point3d(0, 0, 0),
cv::Point3d(100, 0, 0),
cv::Point3d(0, 100, 0),
cv::Point3d(0, 0, 100) };
std::vector<cv::Point3d> transformedPoints;
for (const auto& point : basePoints) {
cv::Mat pointMat = (cv::Mat_<double>(4, 1) << point.x, point.y, point.z, 1.0);
cv::Mat transformedPointMat = transformationMatrix * pointMat;
cv::Point3d transformedPoint;
transformedPoint.x = transformedPointMat.at<double>(0, 0) / transformedPointMat.at<double>(3, 0);
transformedPoint.y = transformedPointMat.at<double>(1, 0) / transformedPointMat.at<double>(3, 0);
transformedPoint.z = transformedPointMat.at<double>(2, 0) / transformedPointMat.at<double>(3, 0);
transformedPoints.push_back(transformedPoint);
}
return transformedPoints;
}
bool HalconCorrection(correctionStruct* correctionData, CorrectionPoints& result)
{
#pragma region XOY_DEFINE
HTuple hv_in_X_X0Y;
for (auto rangeData : correctionData->in_X_X0Y)
hv_in_X_X0Y.Append(rangeData);
HTuple hv_in_Y_X0Y;
for (auto rangeData : correctionData->in_Y_X0Y)
hv_in_Y_X0Y.Append(rangeData);
HTuple hv_in_Z_X0Y;
for (auto rangeData : correctionData->in_Z_X0Y)
hv_in_Z_X0Y.Append(rangeData);
HTuple hv_in_Z_X0Y_Value;
for (auto rangeData : correctionData->in_Z_X0Y_Value)
hv_in_Z_X0Y_Value.Append(rangeData);
#pragma endregion
#pragma region XOZ_DEFINE
HTuple hv_in_X_X0Z;
for (auto rangeData : correctionData->in_X_X0Z)
hv_in_X_X0Z.Append(rangeData);
HTuple hv_in_Y_X0Z;
for (auto rangeData : correctionData->in_Y_X0Z)
hv_in_Y_X0Z.Append(rangeData);
HTuple hv_in_Z_X0Z;
for (auto rangeData : correctionData->in_Z_X0Z)
hv_in_Z_X0Z.Append(rangeData);
HTuple hv_in_Y_X0Z_Value;
for (auto rangeData : correctionData->in_Y_X0Z_Value)
hv_in_Y_X0Z_Value.Append(rangeData);
#pragma endregion
#pragma region YOZ_DEFINE
HTuple hv_in_X_Y0Z;
for (auto rangeData : correctionData->in_X_Y0Z)
hv_in_X_Y0Z.Append(rangeData);
HTuple hv_in_Y_Y0Z;
for (auto rangeData : correctionData->in_Y_Y0Z)
hv_in_Y_Y0Z.Append(rangeData);
HTuple hv_in_Z_Y0Z;
for (auto rangeData : correctionData->in_Z_Y0Z)
hv_in_Z_Y0Z.Append(rangeData);
HTuple hv_in_X_Y0Z_Value;
for (auto rangeData : correctionData->in_X_Y0Z_Value)
hv_in_X_Y0Z_Value.Append(rangeData);
#pragma endregion
HTuple hv_out_Origin;
HTuple hv_out_Result_X;
HTuple hv_out_Result_Y;
HTuple hv_out_Result_Z;
HTuple hv_out_AngleZX;
HTuple hv_out_AngleYZ;
HTuple hv_out_AngleXY;
HTuple hv_out_Flag;
halcon_Correction(
hv_in_X_X0Y, hv_in_Y_X0Y, hv_in_Z_X0Y, hv_in_Z_X0Y_Value,
hv_in_X_X0Z, hv_in_Y_X0Z, hv_in_Z_X0Z, hv_in_Y_X0Z_Value,
hv_in_X_Y0Z, hv_in_Y_Y0Z, hv_in_Z_Y0Z, hv_in_X_Y0Z_Value,
&hv_out_Origin, &hv_out_Result_X, &hv_out_Result_Y, &hv_out_Result_Z,
&hv_out_AngleZX, &hv_out_AngleYZ, &hv_out_AngleXY, &hv_out_Flag);
int ok_Flag = 0;
if (hv_out_Flag.Length() == 1)
if (hv_out_Flag[0].D() == 1)
{
if (hv_out_Origin.Length() == 3)
{
result.Origin = { hv_out_Origin[0].D(),hv_out_Origin[1].D(), hv_out_Origin[2].D() };
ok_Flag++;
}
if (hv_out_Result_X.Length() == 3)
{
result.Result_X = { hv_out_Result_X[0].D(),hv_out_Result_X[1].D(), hv_out_Result_X[2].D() };
ok_Flag++;
}
if (hv_out_Result_Y.Length() == 3)
{
result.Result_Y = { hv_out_Result_Y[0].D(),hv_out_Result_Y[1].D(), hv_out_Result_Y[2].D() };
ok_Flag++;
}
if (hv_out_Result_Z.Length() == 3)
{
result.Result_Z = { hv_out_Result_Z[0].D(),hv_out_Result_Z[1].D(), hv_out_Result_Z[2].D() };
ok_Flag++;
}
if (hv_out_AngleZX.Length() == 1)
{
result.AngleZX = hv_out_AngleZX[0].D();
ok_Flag++;
}
if (hv_out_AngleYZ.Length() == 1)
{
result.AngleYZ = hv_out_AngleYZ[0].D();
ok_Flag++;
}
if (hv_out_AngleXY.Length() == 1)
{
result.AngleXY = hv_out_AngleXY[0].D();
ok_Flag++;
}
}
else
{
std::cout << "error";
}
if (ok_Flag != 7)
{
//qDebug()
return false;
}
result.ofx1 = result.Result_X[0] - result.Origin[0];
result.ofx2 = result.Result_X[1] - result.Origin[1];
result.ofx3 = result.Result_X[2] - result.Origin[2];
result.ofy1 = result.Result_Y[0] - result.Origin[0];
result.ofy2 = result.Result_Y[1] - result.Origin[1];
result.ofy3 = result.Result_Y[2] - result.Origin[2];
result.ofz1 = result.Result_Z[0] - result.Origin[0];
result.ofz2 = result.Result_Z[1] - result.Origin[1];
result.ofz3 = result.Result_Z[2] - result.Origin[2];
return true;
}
void test()
{
correctionStruct* correctionData1 = new correctionStruct();
correctionData1->in_X_X0Y = { 536.38,666.22,369.58 };
correctionData1->in_Y_X0Y = { -1383.24,-1228.4,-1228.4 };
correctionData1->in_Z_X0Y = { -131.81,-130.16,-130.15 };
correctionData1->in_Z_X0Y_Value = { -75.9252,-76.9252,-77.9252 };
correctionData1->in_X_X0Z = { 615.24,191.45,197.34 };
correctionData1->in_Y_X0Z = { -372.66,-372.85,-372.89 };
correctionData1->in_Z_X0Z = { -925.76,-933.44,-1001.14 };
correctionData1->in_Y_X0Z_Value = { -74.1117,-77.9532,-79.273 };
correctionData1->in_X_Y0Z = { -213.84,-213.9,-213.85 };
correctionData1->in_Y_Y0Z = { -1007.81,-1072.33,-999.51 };
correctionData1->in_Z_Y0Z = { -601.31,-601.34,-693.32 };
correctionData1->in_X_Y0Z_Value = { 80.9952,80.223,80.8002 };
correctionStruct* correctionData2 = new correctionStruct();
correctionData2->in_X_X0Y = { 536.38,666.22,369.58 };
correctionData2->in_Y_X0Y = { -1383.24,-1228.4,-1228.4 };
correctionData2->in_Z_X0Y = { -131.81,-130.16,-130.15 };
correctionData2->in_Z_X0Y_Value = { -75.9252,-76.9252,-77.9252 };
correctionData2->in_X_X0Z = { 615.24,191.45,197.34 };
correctionData2->in_Y_X0Z = { -372.66,-372.85,-372.89 };
correctionData2->in_Z_X0Z = { -925.76,-933.44,-1001.14 };
correctionData2->in_Y_X0Z_Value = { -74.1117,-77.9532,-79.273 };
correctionData2->in_X_Y0Z = { -213.84,-213.9,-213.85 };
correctionData2->in_Y_Y0Z = { -1007.81,-1072.33,-999.51 };
correctionData2->in_Z_Y0Z = { -601.31,-601.34,-693.32 };
correctionData2->in_X_Y0Z_Value = { 80.9952,80.223,80.8002 };
CorrectionPoints oldPoints;
HalconCorrection(correctionData1, oldPoints);
CorrectionPoints newPoints;
HalconCorrection(correctionData2, newPoints);
Points point1;
point1.OriginX = 0;
point1.OriginY = 0;
point1.OriginZ = 0;
/*point1.xX = oldPoints.ofx1;
point1.xY = oldPoints.ofx2;
point1.xZ = oldPoints.ofx3;
point1.yX = oldPoints.ofy1;
point1.yY = oldPoints.ofy2;
point1.yZ = oldPoints.ofy3;
point1.zX = oldPoints.ofz1;
point1.zY = oldPoints.ofz2;
point1.zZ = oldPoints.ofz3;*/
point1.xX = 0;
point1.xY = 100;
point1.xZ = 0;
point1.yX = 0;
point1.yY = 0;
point1.yZ = 100;
point1.zX = 100;
point1.zY = 0;
point1.zZ = 0;
Points point2;
point2.OriginX = 0;
point2.OriginY = 0;
point2.OriginZ = 0;
point2.xX = newPoints.ofx1;
point2.xY = newPoints.ofx2;
point2.xZ = newPoints.ofx3;
point2.yX = newPoints.ofy1;
point2.yY = newPoints.ofy2;
point2.yZ = newPoints.ofy3;
point2.zX = newPoints.ofz1;
point2.zY = newPoints.ofz2;
point2.zZ = newPoints.ofz3;
Points point3;
point3.OriginX = 0;
point3.OriginY = 0;
point3.OriginZ = 0;
point3.xX = 100;
point3.xY = 100;
point3.xZ = 0;
point3.yX = 0;
point3.yY = 0;
point3.yZ = 100;
point3.zX = 100;
point3.zY = 0;
point3.zZ = 0;
double rotationMatrix[3][3];
calculateRotationMatrix(point1, rotationMatrix);
double roll, pitch, yaw;
extractRPY(rotationMatrix, roll, pitch, yaw);
double rotationMatrix2[3][3];
calculateRotationMatrix(point3, rotationMatrix2);
double roll2, pitch2, yaw2;
extractRPY(rotationMatrix2, roll2, pitch2, yaw2);
//Point3d rotationAngles = calculateRotationAngles(point1, point3);
//Points newPoint = applyAngleDifference(angleDifference, point3);
//std::vector<cv::Point3d> result=getoffsetPos(oldPoints, oldPoints);
QThread::msleep(100);
}
void ALGO_Correction(std::shared_ptr <detData> data)
{
test();
if (data->correctionData == nullptr)
return;
#pragma region XOY_DEFINE
@@ -4,7 +4,7 @@
#include <backStruct.h>
#include <cameraManager.h>
#include <jsonParsing.h>
#define debugTesting 0
#define debugTesting 1
static correctionStruct* correctionData;
void triggerManager::initialization(backEndManager* input)
{
+6 -2
View File
@@ -17,8 +17,12 @@
#define sColor_Black "22, 22, 22"
#define sColor_BlackWithWhite "45, 45, 45"
#define enableHandleAiModel 1
#define disableBackEndCheck 0
#define enableHandleAiModel 0
#define disableBackEndCheck 1
//窗口设置
#define windowMoveFreshRate 100//窗口移动每秒的刷新率
enum backEndError
{
dllNotFound,
+3 -1
View File
@@ -56,6 +56,7 @@ MainWindow::MainWindow(QWidget* parent)
blurEffectAnimation->setEndValue(0);
blurEffectAnimation->start();
showMaximized();
}
MainWindow::~MainWindow()
@@ -145,7 +146,8 @@ void MainWindow::topBarInit()
"QPushButton:hover { background-color: rgb(%1); }").arg(sColor_Gold)); // 设置透明背景和悬停时的背景色
maximizeRestoreButton->setFixedHeight(topBarHeight);
maximizeRestoreButton->setFixedWidth(buttonWidth);
QObject::connect(maximizeRestoreButton, &QPushButton::clicked, this, &MainWindow::toggleMaximize); // 点击最大化/还原按钮
//QObject::connect(maximizeRestoreButton, &QPushButton::clicked, this, &MainWindow::toggleMaximize); // 点击最大化/还原按钮
QObject::connect(maximizeRestoreButton, &QPushButton::clicked, this, [=]() {toggleMaximize(reverse); }); // 点击最大化/还原按钮
rightLayout->addWidget(maximizeRestoreButton);
//关闭窗口
+53 -10
View File
@@ -73,21 +73,54 @@ private:
void topBarInit();
void leftBarInit();
//窗口移动
#pragma region 窗口移动变量
QTime lastWindowMoveTime;
int freshDelay = 1000 / windowMoveFreshRate;
bool setNormalAndMove = false;//判断是否从最大化开始移动
#pragma endregion
//窗口缩放
#pragma region 窗口缩放变量
enum toggleMaximizeEnum
{
reverse,
normal,
max
};
#pragma endregion
public slots:
void minimizeWindow()
{
showMinimized();
}
void toggleMaximize()
void toggleMaximize(toggleMaximizeEnum toggleMaximizeEnumIn = reverse)
{
if (isMaximized()) {
switch (toggleMaximizeEnumIn)
{
case reverse:
if (isMaximized()) {
showNormal();
maximizeRestoreButton->setIcon(QPixmap(":/topBar/icon/fullscreen.png"));
}
else {
showMaximized();
maximizeRestoreButton->setIcon(QPixmap(":/topBar/icon/fullscreen-exit.png"));
}
break;
case normal:
showNormal();
maximizeRestoreButton->setIcon(QPixmap(":/topBar/icon/fullscreen.png")); // 设置图标
}
else {
maximizeRestoreButton->setIcon(QPixmap(":/topBar/icon/fullscreen.png"));
break;
case max:
showMaximized();
maximizeRestoreButton->setIcon(QPixmap(":/topBar/icon/fullscreen-exit.png")); // 设置图标
maximizeRestoreButton->setIcon(QPixmap(":/topBar/icon/fullscreen-exit.png"));
break;
}
_displayWidget->windowChanged();
}
void switchstackedWidgets(int index)
@@ -148,7 +181,6 @@ public slots:
blurEffectAnimation->start();
}
}
void recvPixmapImageWidget(showImgPack* pack)
{
_displayWidget->_ImageWidgetManager.showImg(pack->img, pack->type, pack->index);
@@ -175,21 +207,32 @@ public slots:
_loadingWidget.initLog->append(strIn);
}
}
signals:
void reInitSignal();
protected:
void mouseDoubleClickEvent(QMouseEvent* event) override
{
toggleMaximize(reverse);
QMainWindow::mouseDoubleClickEvent(event);
}
void mousePressEvent(QMouseEvent* event) override {
if (event->buttons() == Qt::LeftButton) {
if (event->button() == Qt::LeftButton) {
startPos = event->globalPos();
startFrameGeometry = frameGeometry();
isMoving = true;
lastWindowMoveTime.start(); // 按下时开始计时
}
QMainWindow::mousePressEvent(event);
}
void mouseMoveEvent(QMouseEvent* event) override {
if (event->buttons() == Qt::LeftButton && isMoving) {
move(startFrameGeometry.topLeft() + (event->globalPos() - startPos));
QPoint pos = event->globalPos();
if (std::max(std::abs(pos.x()- startPos.x()), std::abs(pos.y() - startPos.y()))>10)
if(lastWindowMoveTime.elapsed()> freshDelay)//每秒刷新100次
{
lastWindowMoveTime.restart();
move(startFrameGeometry.topLeft() + (event->globalPos() - startPos));
}
}
QMainWindow::mouseMoveEvent(event);
}