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