工件孔的检测算法更新，公共绘图接口的更新
This commit is contained in:
杰仔
parent
5f4be9b154
commit
7046696afb
@ -28,18 +28,6 @@ HECEulerOrder ToHandEyeEulerOrder(int eulerOrder)
|
||||
}
|
||||
}
|
||||
|
||||
HECCalibResult ToHandEyeCalibResult(const double clibMatrix[16])
|
||||
{
|
||||
HECCalibResult calibResult;
|
||||
for (int i = 0; i < 3; ++i) {
|
||||
for (int j = 0; j < 3; ++j) {
|
||||
calibResult.R.at(i, j) = clibMatrix[i * 4 + j];
|
||||
}
|
||||
calibResult.T.at(i) = clibMatrix[i * 4 + 3];
|
||||
}
|
||||
return calibResult;
|
||||
}
|
||||
|
||||
} // namespace
|
||||
|
||||
DetectPresenter::DetectPresenter(/* args */)
|
||||
@ -145,7 +133,7 @@ int DetectPresenter::DetectRod(
|
||||
return ERR_CODE(DEV_NOT_FIND);
|
||||
}
|
||||
|
||||
const HECCalibResult calibResult = ToHandEyeCalibResult(clibMatrix);
|
||||
const HECCalibResult calibResult = HECCalibResult::fromHomogeneousArray(clibMatrix);
|
||||
const HECEulerOrder hecEulerOrder = ToHandEyeEulerOrder(eulerOrder);
|
||||
|
||||
// 构建检测结果：生成点云图像
|
||||
|
||||
@ -13,7 +13,6 @@
|
||||
#include "IYWorkpieceHoleStatus.h"
|
||||
#include "PointCloudImageUtils.h"
|
||||
#include "VrDateUtils.h"
|
||||
#include "CoordinateTransform.h"
|
||||
|
||||
class DetectPresenter
|
||||
{
|
||||
|
||||
@ -68,40 +68,6 @@ public:
|
||||
// 手眼标定矩阵管理
|
||||
CalibMatrix GetClibMatrix(int index) const;
|
||||
|
||||
// ============ 坐标转换接口 ============
|
||||
|
||||
/**
|
||||
* @brief 将算法检测结果（相机坐标系）转换为机器人坐标系
|
||||
*
|
||||
* @param workpieceInfo 算法输出的工件信息（相机坐标系）
|
||||
* @param robotPose 机器人当前位姿
|
||||
* @param handEye 六轴手眼标定矩阵
|
||||
* @param targetPose 输出的目标抓取位姿（机器人坐标系）
|
||||
*
|
||||
* 使用说明：
|
||||
* - Eye-in-Hand 模式：相机固定在机器人末端
|
||||
* - 算法输出的 x_dir, y_dir, z_dir 为方向向量
|
||||
* - 输出的 targetPose 为机器人基坐标系下的位姿
|
||||
*/
|
||||
void ConvertWorkpieceToRobotPose(const WD_workpieceInfo& workpieceInfo,
|
||||
const CTRobotPose& robotPose,
|
||||
const CTSixAxisCalibResult& handEye,
|
||||
CTRobotPose& targetPose);
|
||||
|
||||
/**
|
||||
* @brief 将算法检测结果转换为机器人坐标系（Eye-to-Hand模式）
|
||||
*
|
||||
* @param workpieceInfo 算法输出的工件信息（相机坐标系）
|
||||
* @param cameraToBase 相机到机器人基座的标定矩阵
|
||||
* @param targetPose 输出的目标抓取位姿（机器人坐标系）
|
||||
*
|
||||
* 使用说明：
|
||||
* - Eye-to-Hand 模式：相机固定在外部
|
||||
*/
|
||||
void ConvertWorkpieceToRobotPoseEyeToHand(const WD_workpieceInfo& workpieceInfo,
|
||||
const CTSixAxisCalibResult& cameraToBase,
|
||||
CTRobotPose& targetPose);
|
||||
|
||||
// 实现IVrConfigChangeNotify接口
|
||||
virtual void OnConfigChanged(const ConfigResult& configResult) override;
|
||||
|
||||
|
||||
@ -2,13 +2,9 @@
|
||||
#include "workpieceHolePositioning_Export.h"
|
||||
#include "SG_baseAlgo_Export.h"
|
||||
#include "AlgoParamConverter.h"
|
||||
#include "IHandEyeCalib.h"
|
||||
#include <fstream>
|
||||
#include <QPainter>
|
||||
#include <QPen>
|
||||
#include <QColor>
|
||||
#include <opencv2/opencv.hpp>
|
||||
#include "CoordinateTransform.h"
|
||||
#include "VrConvert.h"
|
||||
#include <memory>
|
||||
|
||||
DetectPresenter::DetectPresenter(/* args */)
|
||||
{
|
||||
@ -91,15 +87,15 @@ int DetectPresenter::DetectWorkpieceHole(
|
||||
AlgoParamConverter::LogAlgoParams("[Algo Thread]",
|
||||
workpiecePara, lineSegPara, filterParam, growParam, groundCalibPara, clibMatrix);
|
||||
}
|
||||
|
||||
#if 0
|
||||
// 数据预处理：调平和去除地面（使用当前相机的调平参数）
|
||||
if(cameraCalibParam){
|
||||
LOG_DEBUG("Processing data with plane calibration\n");
|
||||
double groundH = -1;
|
||||
for(size_t i = 0; i < xyzData.size(); i++){
|
||||
wd_lineDataR(xyzData[i], cameraCalibParam->planeCalib, groundH);
|
||||
wd_lineDataR(xyzData[i], cameraCalibParam->planeCalib, -1);
|
||||
}
|
||||
}
|
||||
#endif
|
||||
|
||||
int errCode = 0;
|
||||
CVrTimeUtils oTimeUtils;
|
||||
@ -124,19 +120,16 @@ int DetectPresenter::DetectWorkpieceHole(
|
||||
LOG_INFO("wd_workpieceHolePositioning: found %zu workpieces, err=%d runtime=%.3fms\n", workpiecePositioning.size(), errCode, oTimeUtils.GetElapsedTimeInMilliSec());
|
||||
ERR_CODE_RETURN(errCode);
|
||||
|
||||
// 从4x4齐次变换矩阵中提取旋转矩阵R(3x3)和平移向量T(3x1)
|
||||
// clibMatrix 是行优先存储的4x4矩阵
|
||||
cv::Mat R = cv::Mat::zeros(3, 3, CV_64F);
|
||||
cv::Mat T = cv::Mat::zeros(3, 1, CV_64F);
|
||||
for (int i = 0; i < 3; i++) {
|
||||
for (int j = 0; j < 3; j++) {
|
||||
R.at<double>(i, j) = clibMatrix[i * 4 + j];
|
||||
}
|
||||
T.at<double>(i, 0) = clibMatrix[i * 4 + 3];
|
||||
// 创建手眼标定实例
|
||||
std::unique_ptr<IHandEyeCalib, decltype(&DestroyHandEyeCalibInstance)> handEyeCalib(
|
||||
CreateHandEyeCalibInstance(),
|
||||
DestroyHandEyeCalibInstance);
|
||||
if (!handEyeCalib) {
|
||||
LOG_ERROR("Failed to create HandEyeCalib instance\n");
|
||||
return ERR_CODE(DEV_NOT_FIND);
|
||||
}
|
||||
|
||||
// 构建用于可视化的点数组
|
||||
std::vector<std::vector<SVzNL3DPoint>> objOps;
|
||||
const HECCalibResult calibResult = HECCalibResult::fromHomogeneousArray(clibMatrix);
|
||||
|
||||
// 处理每个工件的检测结果
|
||||
for (size_t i = 0; i < workpiecePositioning.size(); i++) {
|
||||
@ -148,52 +141,44 @@ int DetectPresenter::DetectWorkpieceHole(
|
||||
}
|
||||
|
||||
// 六轴转换：位置+姿态（转换到机械臂坐标系）
|
||||
SVzNL3DPoint centerEye = workpiece.center;
|
||||
HECPoint3D eyeCenter(workpiece.center.x, workpiece.center.y, workpiece.center.z);
|
||||
|
||||
// 1. 位置转换：使用 pointRT_2 进行坐标变换
|
||||
cv::Point3d ptEye(centerEye.x, centerEye.y, centerEye.z);
|
||||
cv::Point3d ptRobot;
|
||||
pointRT_2(R, T, ptEye, ptRobot);
|
||||
|
||||
// 2. 姿态转换：按照测试代码的方式处理方向向量
|
||||
// 从算法输出获取方向向量
|
||||
std::vector<cv::Point3d> dirVectors_eye(3);
|
||||
dirVectors_eye[0] = cv::Point3d(workpiece.x_dir.x, workpiece.x_dir.y, workpiece.x_dir.z);
|
||||
dirVectors_eye[1] = cv::Point3d(workpiece.y_dir.x, workpiece.y_dir.y, workpiece.y_dir.z);
|
||||
dirVectors_eye[2] = cv::Point3d(workpiece.z_dir.x, workpiece.z_dir.y, workpiece.z_dir.z);
|
||||
// 构建方向向量（眼坐标系）
|
||||
std::vector<HECPoint3D> dirVectors_eye(3);
|
||||
dirVectors_eye[0] = HECPoint3D(workpiece.x_dir.x, workpiece.x_dir.y, workpiece.x_dir.z);
|
||||
dirVectors_eye[1] = HECPoint3D(workpiece.y_dir.x, workpiece.y_dir.y, workpiece.y_dir.z);
|
||||
dirVectors_eye[2] = HECPoint3D(workpiece.z_dir.x, workpiece.z_dir.y, workpiece.z_dir.z);
|
||||
|
||||
// 根据配置决定方向向量反向
|
||||
switch (dirVectorInvert) {
|
||||
case DIR_INVERT_NONE:
|
||||
// 不反向
|
||||
break;
|
||||
case DIR_INVERT_XY:
|
||||
// X和Y方向反向
|
||||
dirVectors_eye[0] = cv::Point3d(-dirVectors_eye[0].x, -dirVectors_eye[0].y, -dirVectors_eye[0].z);
|
||||
dirVectors_eye[1] = cv::Point3d(-dirVectors_eye[1].x, -dirVectors_eye[1].y, -dirVectors_eye[1].z);
|
||||
dirVectors_eye[0] = dirVectors_eye[0] * (-1.0);
|
||||
dirVectors_eye[1] = dirVectors_eye[1] * (-1.0);
|
||||
break;
|
||||
case DIR_INVERT_XZ:
|
||||
// X和Z方向反向
|
||||
dirVectors_eye[0] = cv::Point3d(-dirVectors_eye[0].x, -dirVectors_eye[0].y, -dirVectors_eye[0].z);
|
||||
dirVectors_eye[2] = cv::Point3d(-dirVectors_eye[2].x, -dirVectors_eye[2].y, -dirVectors_eye[2].z);
|
||||
dirVectors_eye[0] = dirVectors_eye[0] * (-1.0);
|
||||
dirVectors_eye[2] = dirVectors_eye[2] * (-1.0);
|
||||
break;
|
||||
case DIR_INVERT_YZ:
|
||||
default:
|
||||
// Y和Z方向反向（默认，兼容原有行为）
|
||||
dirVectors_eye[1] = cv::Point3d(-dirVectors_eye[1].x, -dirVectors_eye[1].y, -dirVectors_eye[1].z);
|
||||
dirVectors_eye[2] = cv::Point3d(-dirVectors_eye[2].x, -dirVectors_eye[2].y, -dirVectors_eye[2].z);
|
||||
dirVectors_eye[1] = dirVectors_eye[1] * (-1.0);
|
||||
dirVectors_eye[2] = dirVectors_eye[2] * (-1.0);
|
||||
break;
|
||||
}
|
||||
|
||||
// 使用 pointRotate 对方向向量进行旋转变换
|
||||
std::vector<cv::Point3d> dirVectors_robot;
|
||||
// 1. 位置转换：R * P_eye + T
|
||||
HECPoint3D ptRobot;
|
||||
handEyeCalib->TransformPoint(calibResult.R, calibResult.T, eyeCenter, ptRobot);
|
||||
|
||||
// 2. 方向向量旋转变换（仅旋转，不平移）
|
||||
std::vector<HECPoint3D> dirVectors_robot(3);
|
||||
for (int j = 0; j < 3; j++) {
|
||||
cv::Point3d rt_pt;
|
||||
pointRotate(R, dirVectors_eye[j], rt_pt);
|
||||
dirVectors_robot.push_back(rt_pt);
|
||||
handEyeCalib->RotatePoint(calibResult.R, dirVectors_eye[j], dirVectors_robot[j]);
|
||||
}
|
||||
|
||||
// 3. 构建旋转矩阵（按照测试代码的方式）
|
||||
// 3. 构建旋转矩阵（列向量形式）
|
||||
double R_pose[3][3];
|
||||
R_pose[0][0] = dirVectors_robot[0].x;
|
||||
R_pose[0][1] = dirVectors_robot[1].x;
|
||||
@ -205,7 +190,7 @@ int DetectPresenter::DetectWorkpieceHole(
|
||||
R_pose[2][1] = dirVectors_robot[1].z;
|
||||
R_pose[2][2] = dirVectors_robot[2].z;
|
||||
|
||||
// 4. 使用 rotationMatrixToEulerZYX 转换为欧拉角（外旋ZYX，即RZ-RY-RX）
|
||||
// 4. 使用算法库的 rotationMatrixToEulerZYX 转换为欧拉角（返回角度）
|
||||
SSG_EulerAngles robotRpy = rotationMatrixToEulerZYX(R_pose);
|
||||
|
||||
// 将机器人坐标系下的位姿添加到positions列表
|
||||
@ -213,21 +198,12 @@ int DetectPresenter::DetectWorkpieceHole(
|
||||
centerRobotPos.x = ptRobot.x;
|
||||
centerRobotPos.y = ptRobot.y;
|
||||
centerRobotPos.z = ptRobot.z;
|
||||
centerRobotPos.roll = robotRpy.roll; // 已经是角度
|
||||
centerRobotPos.pitch = robotRpy.pitch; // 已经是角度
|
||||
centerRobotPos.yaw = robotRpy.yaw; // 已经是角度
|
||||
centerRobotPos.roll = robotRpy.roll;
|
||||
centerRobotPos.pitch = robotRpy.pitch;
|
||||
centerRobotPos.yaw = robotRpy.yaw;
|
||||
|
||||
detectionResult.positions.push_back(centerRobotPos);
|
||||
|
||||
// 添加中心点到可视化列表
|
||||
std::vector<SVzNL3DPoint> holePoints;
|
||||
for(size_t j = 0; j < workpiece.holes.size(); j++){
|
||||
SVzNL3DPoint holeEye = workpiece.holes[j];
|
||||
holePoints.push_back(holeEye);
|
||||
}
|
||||
holePoints.push_back(centerEye);
|
||||
objOps.push_back(holePoints);
|
||||
|
||||
if(debugParam.enableDebug && debugParam.printDetailLog){
|
||||
LOG_INFO("[Algo Thread] Direction vectors (eye): X=(%.3f,%.3f,%.3f), Y=(%.3f,%.3f,%.3f), Z=(%.3f,%.3f,%.3f)\n",
|
||||
workpiece.x_dir.x, workpiece.x_dir.y, workpiece.x_dir.z,
|
||||
@ -237,111 +213,29 @@ int DetectPresenter::DetectWorkpieceHole(
|
||||
dirVectors_robot[0].x, dirVectors_robot[0].y, dirVectors_robot[0].z,
|
||||
dirVectors_robot[1].x, dirVectors_robot[1].y, dirVectors_robot[1].z,
|
||||
dirVectors_robot[2].x, dirVectors_robot[2].y, dirVectors_robot[2].z);
|
||||
LOG_INFO("[Algo Thread] Center Eye Coords: X=%.2f, Y=%.2f, Z=%.2f\n",
|
||||
centerEye.x, centerEye.y, centerEye.z);
|
||||
LOG_INFO("[Algo Thread] Center Robot Coords: X=%.2f, Y=%.2f, Z=%.2f, R=%.4f, P=%.4f, Y=%.4f\n",
|
||||
LOG_INFO("[Algo Thread] Center Eye Coords: X=%.3f, Y=%.3f, Z=%.3f\n",
|
||||
workpiece.center.x, workpiece.center.y, workpiece.center.z);
|
||||
LOG_INFO("[Algo Thread] Center Robot Coords: X=%.3f, Y=%.3f, Z=%.3f, R=%.4f, P=%.4f, Y=%.4f\n",
|
||||
ptRobot.x, ptRobot.y, ptRobot.z,
|
||||
centerRobotPos.roll, centerRobotPos.pitch, centerRobotPos.yaw);
|
||||
}
|
||||
}
|
||||
|
||||
// 从点云数据生成投影图像（单色灰色点云 + 红色孔位标记）
|
||||
// 使用 PointCloudCanvas 生成点云图像（灰色底图 + 红色孔位标记 + 中心点编号）
|
||||
{
|
||||
// 固定图像尺寸
|
||||
int imgRows = 992;
|
||||
int imgCols = 1056;
|
||||
int x_skip = 50;
|
||||
int y_skip = 50;
|
||||
|
||||
// 计算点云范围
|
||||
double xMin = 1e10, xMax = -1e10, yMin = 1e10, yMax = -1e10;
|
||||
for (const auto& line : xyzData) {
|
||||
for (const auto& pt : line) {
|
||||
if (pt.pt3D.z < 1e-4) continue;
|
||||
xMin = std::min(xMin, (double)pt.pt3D.x);
|
||||
xMax = std::max(xMax, (double)pt.pt3D.x);
|
||||
yMin = std::min(yMin, (double)pt.pt3D.y);
|
||||
yMax = std::max(yMax, (double)pt.pt3D.y);
|
||||
PointCloudCanvas canvas = PointCloudCanvas::Create(xyzData);
|
||||
for (size_t i = 0; i < workpiecePositioning.size(); i++) {
|
||||
const WD_workpieceInfo& workpiece = workpiecePositioning[i];
|
||||
// 红色小圆点标记每个孔
|
||||
for (size_t j = 0; j < workpiece.holes.size(); j++) {
|
||||
canvas.drawPoint(workpiece.holes[j].x, workpiece.holes[j].y, QColor(255, 0, 0), 4);
|
||||
}
|
||||
// 中心点标记 + 白色编号
|
||||
canvas.drawPoint(workpiece.center.x, workpiece.center.y, QColor(255, 0, 0), 4);
|
||||
canvas.drawText(workpiece.center.x, workpiece.center.y,
|
||||
QString("%1").arg(i + 1), Qt::white, 14);
|
||||
}
|
||||
|
||||
// 不扩展边界，直接使用点云范围
|
||||
|
||||
// 计算投影比例
|
||||
double y_rows = (double)(imgRows - y_skip * 2);
|
||||
double x_cols = (double)(imgCols - x_skip * 2);
|
||||
double x_scale = (xMax - xMin) / x_cols;
|
||||
double y_scale = (yMax - yMin) / y_rows;
|
||||
if (x_scale < y_scale)
|
||||
x_scale = y_scale;
|
||||
else
|
||||
y_scale = x_scale;
|
||||
|
||||
// 创建图像
|
||||
QImage image(imgCols, imgRows, QImage::Format_RGB888);
|
||||
image.fill(Qt::black);
|
||||
QPainter painter(&image);
|
||||
painter.setRenderHint(QPainter::Antialiasing);
|
||||
|
||||
// 绘制点云数据 - 使用单色灰色
|
||||
QColor grayColor(150, 150, 150);
|
||||
for (const auto& scanLine : xyzData) {
|
||||
for (const auto& point : scanLine) {
|
||||
if (point.pt3D.z < 1e-4) continue;
|
||||
|
||||
int px = (int)((point.pt3D.x - xMin) / x_scale + x_skip);
|
||||
int py = (int)((point.pt3D.y - yMin) / y_scale + y_skip);
|
||||
|
||||
if (px >= 0 && px < imgCols && py >= 0 && py < imgRows) {
|
||||
painter.setPen(QPen(grayColor, 1));
|
||||
painter.drawPoint(px, py);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// 绘制孔位标记 - 使用红色
|
||||
if (!objOps.empty()) {
|
||||
QColor holeColor(255, 0, 0); // 红色
|
||||
painter.setPen(QPen(holeColor, 1));
|
||||
painter.setBrush(QBrush(holeColor));
|
||||
|
||||
for (size_t i = 0; i < objOps.size(); i++) {
|
||||
const auto& holeGroup = objOps[i];
|
||||
size_t groupSize = holeGroup.size();
|
||||
|
||||
for (size_t j = 0; j < groupSize; j++) {
|
||||
const SVzNL3DPoint& hole = holeGroup[j];
|
||||
|
||||
// 跳过全0的点
|
||||
if (fabs(hole.x) < 0.0001 && fabs(hole.y) < 0.0001 && fabs(hole.z) < 0.0001) {
|
||||
continue;
|
||||
}
|
||||
|
||||
int px = (int)((hole.x - xMin) / x_scale + x_skip);
|
||||
int py = (int)((hole.y - yMin) / y_scale + y_skip);
|
||||
|
||||
if (px >= 0 && px < imgCols && py >= 0 && py < imgRows) {
|
||||
// 绘制圆点标记
|
||||
int circleSize = 2;
|
||||
painter.drawEllipse(px - circleSize/2, py - circleSize/2, circleSize, circleSize);
|
||||
|
||||
// 只有中心点（最后一个点）才绘制编号
|
||||
if (j == groupSize - 1) {
|
||||
painter.setPen(QPen(Qt::white, 1));
|
||||
QFont font("Arial", 14, QFont::Bold);
|
||||
painter.setFont(font);
|
||||
painter.drawText(px + 8, py + 6, QString("%1").arg(i + 1));
|
||||
|
||||
// 恢复画笔
|
||||
painter.setPen(QPen(holeColor, 1));
|
||||
painter.setBrush(QBrush(holeColor));
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
detectionResult.image = image;
|
||||
detectionResult.image = canvas.image();
|
||||
}
|
||||
|
||||
if(debugParam.enableDebug && debugParam.saveDebugImage){
|
||||
|
||||
@ -255,88 +255,6 @@ CalibMatrix WorkpieceHolePresenter::GetClibMatrix(int index) const
|
||||
return clibMatrix;
|
||||
}
|
||||
|
||||
// ============ 坐标转换实现 ============
|
||||
|
||||
void WorkpieceHolePresenter::ConvertWorkpieceToRobotPose(const WD_workpieceInfo& workpieceInfo,
|
||||
const CTRobotPose& robotPose,
|
||||
const CTSixAxisCalibResult& handEye,
|
||||
CTRobotPose& targetPose)
|
||||
{
|
||||
// 1. 从方向向量构建旋转矩阵
|
||||
CTRotationMatrix rotMatrix;
|
||||
rotMatrix.at(0, 0) = workpieceInfo.x_dir.x;
|
||||
rotMatrix.at(1, 0) = workpieceInfo.x_dir.y;
|
||||
rotMatrix.at(2, 0) = workpieceInfo.x_dir.z;
|
||||
rotMatrix.at(0, 1) = workpieceInfo.y_dir.x;
|
||||
rotMatrix.at(1, 1) = workpieceInfo.y_dir.y;
|
||||
rotMatrix.at(2, 1) = workpieceInfo.y_dir.z;
|
||||
rotMatrix.at(0, 2) = workpieceInfo.z_dir.x;
|
||||
rotMatrix.at(1, 2) = workpieceInfo.z_dir.y;
|
||||
rotMatrix.at(2, 2) = workpieceInfo.z_dir.z;
|
||||
|
||||
// 2. 将旋转矩阵转换为欧拉角
|
||||
CTEulerAngles eulerAngles = CCoordinateTransform::rotationMatrixToEuler(rotMatrix, CTEulerOrder::sZYX);
|
||||
|
||||
// 3. 构造相机坐标系下的目标位姿
|
||||
CTCameraPose cameraPose(
|
||||
workpieceInfo.center.x,
|
||||
workpieceInfo.center.y,
|
||||
workpieceInfo.center.z,
|
||||
eulerAngles.roll,
|
||||
eulerAngles.pitch,
|
||||
eulerAngles.yaw
|
||||
);
|
||||
|
||||
// 4. 使用六轴手眼转换（Eye-in-Hand模式）计算机器人目标位姿
|
||||
CCoordinateTransform::sixAxisEyeInHandCalcGraspPose(cameraPose, robotPose, handEye, targetPose);
|
||||
|
||||
LOG_DEBUG("ConvertWorkpieceToRobotPose: Camera(%f, %f, %f, %f, %f, %f) -> Robot(%f, %f, %f, %f, %f, %f)\n",
|
||||
cameraPose.x, cameraPose.y, cameraPose.z,
|
||||
cameraPose.rx, cameraPose.ry, cameraPose.rz,
|
||||
targetPose.x, targetPose.y, targetPose.z,
|
||||
targetPose.rx, targetPose.ry, targetPose.rz);
|
||||
}
|
||||
|
||||
void WorkpieceHolePresenter::ConvertWorkpieceToRobotPoseEyeToHand(const WD_workpieceInfo& workpieceInfo,
|
||||
const CTSixAxisCalibResult& cameraToBase,
|
||||
CTRobotPose& targetPose)
|
||||
{
|
||||
// 1. 从方向向量构建旋转矩阵
|
||||
CTRotationMatrix rotMatrix;
|
||||
rotMatrix.at(0, 0) = workpieceInfo.x_dir.x;
|
||||
rotMatrix.at(1, 0) = workpieceInfo.x_dir.y;
|
||||
rotMatrix.at(2, 0) = workpieceInfo.x_dir.z;
|
||||
rotMatrix.at(0, 1) = workpieceInfo.y_dir.x;
|
||||
rotMatrix.at(1, 1) = workpieceInfo.y_dir.y;
|
||||
rotMatrix.at(2, 1) = workpieceInfo.y_dir.z;
|
||||
rotMatrix.at(0, 2) = workpieceInfo.z_dir.x;
|
||||
rotMatrix.at(1, 2) = workpieceInfo.z_dir.y;
|
||||
rotMatrix.at(2, 2) = workpieceInfo.z_dir.z;
|
||||
|
||||
// 2. 将旋转矩阵转换为欧拉角
|
||||
CTEulerAngles eulerAngles = CCoordinateTransform::rotationMatrixToEuler(rotMatrix, CTEulerOrder::sZYX);
|
||||
|
||||
// 3. 构造相机坐标系下的目标位姿
|
||||
CTCameraPose cameraPose(
|
||||
workpieceInfo.center.x,
|
||||
workpieceInfo.center.y,
|
||||
workpieceInfo.center.z,
|
||||
eulerAngles.roll,
|
||||
eulerAngles.pitch,
|
||||
eulerAngles.yaw
|
||||
);
|
||||
|
||||
// 4. 使用六轴手眼转换（Eye-to-Hand模式）计算机器人目标位姿
|
||||
CCoordinateTransform::sixAxisEyeToHandCalcGraspPose(cameraPose, cameraToBase, targetPose);
|
||||
|
||||
LOG_DEBUG("ConvertWorkpieceToRobotPoseEyeToHand: Camera(%f, %f, %f, %f, %f, %f) -> Robot(%f, %f, %f, %f, %f, %f)\n",
|
||||
cameraPose.x, cameraPose.y, cameraPose.z,
|
||||
cameraPose.rx, cameraPose.ry, cameraPose.rz,
|
||||
targetPose.x, targetPose.y, targetPose.z,
|
||||
targetPose.rx, targetPose.ry, targetPose.rz);
|
||||
}
|
||||
|
||||
|
||||
void WorkpieceHolePresenter::CheckAndUpdateWorkStatus()
|
||||
{
|
||||
if (m_bCameraConnected) {
|
||||
|
||||
@ -1,6 +1,7 @@
|
||||
# 1.1.0
|
||||
## build_0 2026-04-28
|
||||
1. 更新内存泄露问题
|
||||
1. 更新算法
|
||||
2. 更新内存泄露问题
|
||||
|
||||
# 1.0.3
|
||||
## build_0 2026-03-24
|
||||
|
||||
@ -42,6 +42,8 @@ INCLUDEPATH += $$PWD/../../../Module/AuthModule/Inc
|
||||
|
||||
# 坐标变换库
|
||||
INCLUDEPATH += $$PWD/../../../Utils/DataUtils/CoordinateTransform/Inc
|
||||
# 手眼标定模块
|
||||
INCLUDEPATH += $$PWD/../../../Module/HandEyeCalib/Inc
|
||||
# eigen 依赖
|
||||
INCLUDEPATH += $$PWD/../../../SDK/eigen-3.3.9
|
||||
|
||||
@ -110,6 +112,7 @@ win32:CONFIG(debug, debug|release) {
|
||||
LIBS += -L../../../VrNets/debug -lVrModbus
|
||||
LIBS += -L../../../VrNets/debug -lVrTcpServer
|
||||
LIBS += -L../../../Utils/DataUtils/CoordinateTransform/debug -lCoordinateTransformd
|
||||
LIBS += -L../../../Module/HandEyeCalib/debug -lHandEyeCalib
|
||||
|
||||
}else:win32:CONFIG(release, debug|release){
|
||||
LIBS += -L../../../Utils/VrUtils/release -lVrUtils
|
||||
@ -126,6 +129,7 @@ win32:CONFIG(debug, debug|release) {
|
||||
LIBS += -L../../../VrNets/release -lVrModbus
|
||||
LIBS += -L../../../VrNets/release -lVrTcpServer
|
||||
LIBS += -L../../../Utils/DataUtils/CoordinateTransform/release -lCoordinateTransform
|
||||
LIBS += -L../../../Module/HandEyeCalib/release -lHandEyeCalib
|
||||
}else:unix:!macx {
|
||||
# Unix/Linux平台库链接（包括交叉编译）
|
||||
# 注意链接顺序：依赖关系从高到低（被依赖的库放在后面）
|
||||
@ -143,6 +147,7 @@ win32:CONFIG(debug, debug|release) {
|
||||
LIBS += -L../../../VrNets -lVrTcpServer
|
||||
LIBS += -L../../../Utils/VrUtils -lVrUtils
|
||||
LIBS += -L../../../Utils/DataUtils/CoordinateTransform -lCoordinateTransform
|
||||
LIBS += -L../../../Module/HandEyeCalib -lHandEyeCalib
|
||||
|
||||
# 添加系统库依赖
|
||||
LIBS += -lpthread
|
||||
|
||||
@ -953,6 +953,19 @@ QGroupBox::title { subcontrol-origin: margin; left: 10px; padding: 0 3px 0 3px;
|
||||
</property>
|
||||
</widget>
|
||||
</item>
|
||||
<item>
|
||||
<spacer name="horizontalSpacer_3">
|
||||
<property name="orientation">
|
||||
<enum>Qt::Horizontal</enum>
|
||||
</property>
|
||||
<property name="sizeHint" stdset="0">
|
||||
<size>
|
||||
<width>40</width>
|
||||
<height>20</height>
|
||||
</size>
|
||||
</property>
|
||||
</spacer>
|
||||
</item>
|
||||
<item>
|
||||
<widget class="QPushButton" name="btn_camer_cancel">
|
||||
<property name="minimumSize">
|
||||
|
||||
Binary file not shown.
Binary file not shown.
@ -39,6 +39,13 @@ typedef struct
|
||||
SWD3DPoint point;
|
||||
}SWDIndexing3DPoint;
|
||||
|
||||
typedef struct
|
||||
{
|
||||
int lineIdx;
|
||||
int ptIdx;
|
||||
SVzNL3DPoint point;
|
||||
}SWDIndexingVzPoint;
|
||||
|
||||
typedef struct
|
||||
{
|
||||
int start;
|
||||
@ -604,6 +611,7 @@ typedef struct
|
||||
double backwardDiffZ;
|
||||
double pre_stepDist;
|
||||
double post_stepDist;
|
||||
double curr_z;
|
||||
double forward_z;
|
||||
double backward_z;
|
||||
}SSG_pntDirAngle;
|
||||
|
||||
@ -32,6 +32,10 @@
|
||||
|
||||
//汽车轮眉高度测量
|
||||
#define SX_ERR_INVALID_ARC -2301
|
||||
#define SX_ERR_INVALID_WHEEL_EDGE -2302
|
||||
#define SX_ERR_NO_WHEEL_COMMON_EDGE -2303
|
||||
#define SX_ERR_NO_WHEEL_ARC -2304
|
||||
|
||||
|
||||
//糖袋子拆线
|
||||
#define SX_ERR_NO_MARK -2401
|
||||
|
||||
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
@ -29,6 +29,9 @@
|
||||
<string notr="true">background-color: rgb(25, 26, 28);</string>
|
||||
</property>
|
||||
<layout class="QVBoxLayout" name="mainLayout">
|
||||
<property name="spacing">
|
||||
<number>6</number>
|
||||
</property>
|
||||
<property name="leftMargin">
|
||||
<number>40</number>
|
||||
</property>
|
||||
@ -41,15 +44,11 @@
|
||||
<property name="bottomMargin">
|
||||
<number>25</number>
|
||||
</property>
|
||||
<property name="spacing">
|
||||
<number>6</number>
|
||||
</property>
|
||||
<item>
|
||||
<widget class="QLabel" name="labelAppName">
|
||||
<property name="font">
|
||||
<font>
|
||||
<pointsize>22</pointsize>
|
||||
<weight>75</weight>
|
||||
<bold>true</bold>
|
||||
</font>
|
||||
</property>
|
||||
@ -82,22 +81,6 @@
|
||||
</property>
|
||||
</widget>
|
||||
</item>
|
||||
<item>
|
||||
<spacer name="spacerAfterVersion">
|
||||
<property name="orientation">
|
||||
<enum>Qt::Vertical</enum>
|
||||
</property>
|
||||
<property name="sizeType">
|
||||
<enum>QSizePolicy::Fixed</enum>
|
||||
</property>
|
||||
<property name="sizeHint" stdset="0">
|
||||
<size>
|
||||
<width>0</width>
|
||||
<height>10</height>
|
||||
</size>
|
||||
</property>
|
||||
</spacer>
|
||||
</item>
|
||||
<item>
|
||||
<widget class="Line" name="line">
|
||||
<property name="styleSheet">
|
||||
@ -109,32 +92,13 @@
|
||||
</widget>
|
||||
</item>
|
||||
<item>
|
||||
<spacer name="spacerAfterLine">
|
||||
<property name="orientation">
|
||||
<enum>Qt::Vertical</enum>
|
||||
</property>
|
||||
<property name="sizeType">
|
||||
<enum>QSizePolicy::Fixed</enum>
|
||||
</property>
|
||||
<property name="sizeHint" stdset="0">
|
||||
<size>
|
||||
<width>0</width>
|
||||
<height>8</height>
|
||||
</size>
|
||||
</property>
|
||||
</spacer>
|
||||
</item>
|
||||
<item>
|
||||
<layout class="QGridLayout" name="infoGrid">
|
||||
<layout class="QGridLayout" name="infoGrid" columnstretch="0,0">
|
||||
<property name="horizontalSpacing">
|
||||
<number>16</number>
|
||||
</property>
|
||||
<property name="verticalSpacing">
|
||||
<number>12</number>
|
||||
</property>
|
||||
<property name="columnStretch" stdset="0">
|
||||
<string>1,4</string>
|
||||
</property>
|
||||
<item row="0" column="0">
|
||||
<widget class="QLabel" name="labelMachineCodeTitle">
|
||||
<property name="font">
|
||||
@ -149,7 +113,7 @@
|
||||
<string>机器码</string>
|
||||
</property>
|
||||
<property name="alignment">
|
||||
<set>Qt::AlignRight|Qt::AlignVCenter</set>
|
||||
<set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
|
||||
</property>
|
||||
</widget>
|
||||
</item>
|
||||
@ -185,7 +149,7 @@
|
||||
<string>CPU序列号</string>
|
||||
</property>
|
||||
<property name="alignment">
|
||||
<set>Qt::AlignRight|Qt::AlignVCenter</set>
|
||||
<set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
|
||||
</property>
|
||||
</widget>
|
||||
</item>
|
||||
@ -221,7 +185,7 @@
|
||||
<string>硬盘序列号</string>
|
||||
</property>
|
||||
<property name="alignment">
|
||||
<set>Qt::AlignRight|Qt::AlignVCenter</set>
|
||||
<set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
|
||||
</property>
|
||||
</widget>
|
||||
</item>
|
||||
@ -257,7 +221,7 @@
|
||||
<string>算法版本</string>
|
||||
</property>
|
||||
<property name="alignment">
|
||||
<set>Qt::AlignRight|Qt::AlignVCenter</set>
|
||||
<set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
|
||||
</property>
|
||||
</widget>
|
||||
</item>
|
||||
@ -301,6 +265,12 @@
|
||||
<property name="orientation">
|
||||
<enum>Qt::Horizontal</enum>
|
||||
</property>
|
||||
<property name="sizeHint" stdset="0">
|
||||
<size>
|
||||
<width>0</width>
|
||||
<height>0</height>
|
||||
</size>
|
||||
</property>
|
||||
</spacer>
|
||||
</item>
|
||||
<item>
|
||||
@ -331,6 +301,12 @@ border-radius: 4px;</string>
|
||||
<property name="orientation">
|
||||
<enum>Qt::Horizontal</enum>
|
||||
</property>
|
||||
<property name="sizeHint" stdset="0">
|
||||
<size>
|
||||
<width>0</width>
|
||||
<height>0</height>
|
||||
</size>
|
||||
</property>
|
||||
</spacer>
|
||||
</item>
|
||||
</layout>
|
||||
|
||||
@ -392,6 +392,22 @@ struct HECCalibResult
|
||||
|
||||
HECCalibResult() = default;
|
||||
|
||||
/**
|
||||
* @brief 从4x4齐次变换矩阵（行优先double[16]）构造标定结果
|
||||
* 提取旋转矩阵R(3x3)和平移向量T(3x1)
|
||||
*/
|
||||
static HECCalibResult fromHomogeneousArray(const double matrix[16])
|
||||
{
|
||||
HECCalibResult result;
|
||||
for (int i = 0; i < 3; ++i) {
|
||||
for (int j = 0; j < 3; ++j) {
|
||||
result.R.at(i, j) = matrix[i * 4 + j];
|
||||
}
|
||||
result.T.at(i) = matrix[i * 4 + 3];
|
||||
}
|
||||
return result;
|
||||
}
|
||||
|
||||
// 转换为齐次变换矩阵
|
||||
HECHomogeneousMatrix toHomogeneousMatrix() const
|
||||
{
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user