diff --git a/App/WorkpieceHole/Doc/MODBUS_PROTOCOL.md b/App/WorkpieceHole/Doc/MODBUS_PROTOCOL.md index 4a2f7d48..fd38ed4e 100644 --- a/App/WorkpieceHole/Doc/MODBUS_PROTOCOL.md +++ b/App/WorkpieceHole/Doc/MODBUS_PROTOCOL.md @@ -144,147 +144,3 @@ | 读取失败 | 记录日志，下次轮询重试 | | 写入失败 | 记录日志，返回失败状态 | | 超时 | 默认1秒超时，触发重连 | - -## 8. 配置文件 - -配置文件路径：`config/config.xml` - -```xml - -``` - -> **注意**: robotServerIp 留空表示不使用机械臂直连 - -## 9. 代码接口 - -### 9.1 PLCModbusClient 类 - -```cpp -// 寄存器地址默认值（代码协议地址 = D寄存器 + 1） -static constexpr int DEFAULT_ADDR_PHOTO_REQUEST = 1001; // D1000: 拍照请求 -static constexpr int DEFAULT_ADDR_DATA_COMPLETE = 1003; // D1002: 数据完成 -static constexpr int DEFAULT_ADDR_COORD_DATA_START = 2001; // D2000: 坐标数据起始 - -// 每个点占用的寄存器数量（6个float × 2寄存器/float = 12寄存器） -static constexpr int REGS_PER_POINT = 12; - -// 最大点数 -static constexpr int MAX_POINTS = 10; - -// 坐标数据结构（float类型，大端序） -struct CoordinateData { - float x = 0.0f; // X坐标 (mm) - float y = 0.0f; // Y坐标 (mm) - float z = 0.0f; // Z坐标 (mm) - float pitch = 0.0f; // Pitch角度 (°) - float roll = 0.0f; // Roll角度 (°) - float yaw = 0.0f; // Yaw角度 (°) -}; - -// 初始化连接 -bool Initialize(const std::string& plcIP, int plcPort = 502, const RegisterConfig& regConfig = RegisterConfig()); - -// 关闭连接 -void Shutdown(); - -// 启动/停止轮询 -void StartPolling(int intervalMs = 100); -void StopPolling(); - -// 设置拍照触发回调 -void SetPhotoTriggerCallback(PhotoTriggerCallback callback); - -// 清除拍照请求（写0到D1000） -bool ClearPhotoRequest(); - -// 发送单个坐标数据到PLC（写入D2000开始） -bool SendCoordinateToPLC(const CoordinateData& coord, int pointIndex = 0); - -// 发送多个坐标数据到PLC -bool SendCoordinatesToPLC(const std::vector& coords); - -// 通知数据输出完成（写1到D1002） -bool NotifyDataComplete(); - -// 连接状态查询 -bool IsPLCConnected() const; -``` - -### 9.2 信号 - -```cpp -// 拍照请求信号 -void photoRequested(int cameraIndex); - -// 连接状态变化 -void connectionStateChanged(bool plcConnected); - -// 正在重连 -void reconnecting(const QString& deviceName, int attemptCount); - -// 错误发生 -void errorOccurred(const QString& errorMsg); -``` - -## 10. 使用示例 - -```cpp -// 创建客户端 -PLCModbusClient* client = new PLCModbusClient(this); - -// 从配置读取IP并初始化（只连接PLC） -PLCModbusClient::RegisterConfig regConfig; -regConfig.addrPhotoRequest = 1001; // D1000 -regConfig.addrDataComplete = 1003; // D1002 -regConfig.addrCoordDataStart = 2001; // D2000 - -client->Initialize( - config.plcRobotServerConfig.plcServerIp, - config.plcRobotServerConfig.plcServerPort, - regConfig -); - -// 设置拍照回调 -client->SetPhotoTriggerCallback([this, client](int cameraIndex) { - // 1. 清除拍照请求 - client->ClearPhotoRequest(); - - // 2. 执行拍照和算法 - auto results = doDetection(cameraIndex); - - // 3. 发送多个坐标点到PLC（D2000开始，float大端序） - std::vector coords; - for (const auto& result : results) { - PLCModbusClient::CoordinateData coord; - coord.x = result.x; // float (mm) - coord.y = result.y; // float (mm) - coord.z = result.z; // float (mm) - coord.pitch = result.pitch; // float (°) - coord.roll = result.roll; // float (°) - coord.yaw = result.yaw; // float (°) - coords.push_back(coord); - } - client->SendCoordinatesToPLC(coords); - - // 4. 通知完成 - client->NotifyDataComplete(); -}); - -// 启动轮询 -client->StartPolling(100); - -// 程序退出时 -client->Shutdown(); -``` - -## 11. 版本历史 - -| 版本 | 日期 | 说明 | -|------|------|------| -| 1.0 | 2025-01 | 初始版本，支持PLC和机械臂 | -| 1.1 | 2025-01 | 移除机械臂直连，坐标数据输出到PLC | -| 1.2 | 2025-01 | 坐标数据改为float大端序，支持最多10个点 | diff --git a/App/WorkpieceHole/WorkpieceHoleApp/Presenter/Inc/DetectPresenter.h b/App/WorkpieceHole/WorkpieceHoleApp/Presenter/Inc/DetectPresenter.h index 5e21e233..6a99d91c 100644 --- a/App/WorkpieceHole/WorkpieceHoleApp/Presenter/Inc/DetectPresenter.h +++ b/App/WorkpieceHole/WorkpieceHoleApp/Presenter/Inc/DetectPresenter.h @@ -31,6 +31,7 @@ public: LaserDataLoader& dataLoader, const double clibMatrix[16], int eulerOrder, + int dirVectorInvert, WorkpieceHoleDetectionResult& detectionResult); }; diff --git a/App/WorkpieceHole/WorkpieceHoleApp/Presenter/Inc/PLCModbusClient.h b/App/WorkpieceHole/WorkpieceHoleApp/Presenter/Inc/PLCModbusClient.h index 180d4b5f..8fad59b1 100644 --- a/App/WorkpieceHole/WorkpieceHoleApp/Presenter/Inc/PLCModbusClient.h +++ b/App/WorkpieceHole/WorkpieceHoleApp/Presenter/Inc/PLCModbusClient.h @@ -108,6 +108,7 @@ private: // 连接管理 bool checkConnection(); bool tryConnectPLC(); + void disconnectPLC(); // 主动断开连接（用于触发重连） // 寄存器操作 int readPhotoRequest(); diff --git a/App/WorkpieceHole/WorkpieceHoleApp/Presenter/Src/DetectPresenter.cpp b/App/WorkpieceHole/WorkpieceHoleApp/Presenter/Src/DetectPresenter.cpp index d4abb466..65eb489d 100644 --- a/App/WorkpieceHole/WorkpieceHoleApp/Presenter/Src/DetectPresenter.cpp +++ b/App/WorkpieceHole/WorkpieceHoleApp/Presenter/Src/DetectPresenter.cpp @@ -1,9 +1,11 @@ #include "DetectPresenter.h" #include "workpieceHolePositioning_Export.h" +#include "SG_baseAlgo_Export.h" #include #include #include #include +#include #include "CoordinateTransform.h" #include "VrConvert.h" @@ -25,6 +27,7 @@ int DetectPresenter::DetectWorkpieceHole( LaserDataLoader& dataLoader, const double clibMatrix[16], int eulerOrder, + int dirVectorInvert, WorkpieceHoleDetectionResult& detectionResult) { if (laserLines.empty()) { @@ -173,9 +176,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齐次变换矩阵，避免欧拉角转换误差 - CTHomogeneousMatrix calibHMatrix; - memcpy(calibHMatrix.data, clibMatrix, sizeof(double) * 16); + // 从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(i, j) = clibMatrix[i * 4 + j]; + } + T.at(i, 0) = clibMatrix[i * 4 + 3]; + } // 构建用于可视化的点数组 std::vector> objOps; @@ -192,47 +202,72 @@ int DetectPresenter::DetectWorkpieceHole( // 六轴转换：位置+姿态（转换到机械臂坐标系） SVzNL3DPoint centerEye = workpiece.center; - // 从方向向量构建旋转矩阵 - // 算法输出的 x_dir, y_dir, z_dir 是工件坐标系的三个轴在相机坐标系下的方向 - CTRotationMatrix rotMatrix; - // 第一列：X轴方向 - rotMatrix.at(0, 0) = workpiece.x_dir.x; - rotMatrix.at(1, 0) = workpiece.x_dir.y; - rotMatrix.at(2, 0) = workpiece.x_dir.z; - // 第二列：Y轴方向 - rotMatrix.at(0, 1) = workpiece.y_dir.x; - rotMatrix.at(1, 1) = workpiece.y_dir.y; - rotMatrix.at(2, 1) = workpiece.y_dir.z; - // 第三列：Z轴方向 - rotMatrix.at(0, 2) = workpiece.z_dir.x; - rotMatrix.at(1, 2) = workpiece.z_dir.y; - rotMatrix.at(2, 2) = workpiece.z_dir.z; + // 1. 位置转换：使用 pointRT_2 进行坐标变换 + cv::Point3d ptEye(centerEye.x, centerEye.y, centerEye.z); + cv::Point3d ptRobot; + pointRT_2(R, T, ptEye, ptRobot); - // 将旋转矩阵转换为欧拉角（使用配置的旋转顺序） - CTEulerAngles eulerAngles = CCoordinateTransform::rotationMatrixToEuler(rotMatrix, static_cast(eulerOrder)); + // 2. 姿态转换：按照测试代码的方式处理方向向量 + // 从算法输出获取方向向量 + std::vector 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); - // 构建相机坐标系下的完整位姿 - CTCameraPose cameraPose; - cameraPose.x = centerEye.x; - cameraPose.y = centerEye.y; - cameraPose.z = centerEye.z; - cameraPose.rx = eulerAngles.roll; - cameraPose.ry = eulerAngles.pitch; - cameraPose.rz = eulerAngles.yaw; + // 根据配置决定方向向量反向 + 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); + 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); + 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); + break; + } - // 进行六轴Eye-to-Hand转换（位置+姿态） - // 使用配置的旋转顺序 - CTRobotPose robotPose; - CCoordinateTransform::sixAxisEyeToHandCalcGraspPose(cameraPose, calibHMatrix, static_cast(eulerOrder), robotPose); + // 使用 pointRotate 对方向向量进行旋转变换 + std::vector dirVectors_robot; + for (int j = 0; j < 3; j++) { + cv::Point3d rt_pt; + pointRotate(R, dirVectors_eye[j], rt_pt); + dirVectors_robot.push_back(rt_pt); + } + + // 3. 构建旋转矩阵（按照测试代码的方式） + double R_pose[3][3]; + R_pose[0][0] = dirVectors_robot[0].x; + R_pose[0][1] = dirVectors_robot[1].x; + R_pose[0][2] = dirVectors_robot[2].x; + R_pose[1][0] = dirVectors_robot[0].y; + R_pose[1][1] = dirVectors_robot[1].y; + R_pose[1][2] = dirVectors_robot[2].y; + R_pose[2][0] = dirVectors_robot[0].z; + R_pose[2][1] = dirVectors_robot[1].z; + R_pose[2][2] = dirVectors_robot[2].z; + + // 4. 使用 rotationMatrixToEulerZYX 转换为欧拉角（外旋ZYX，即RZ-RY-RX） + SSG_EulerAngles robotRpy = rotationMatrixToEulerZYX(R_pose); // 将机器人坐标系下的位姿添加到positions列表 HolePosition centerRobotPos; - centerRobotPos.x = robotPose.x; - centerRobotPos.y = robotPose.y; - centerRobotPos.z = robotPose.z; - centerRobotPos.roll = CCoordinateTransform::radToDeg(robotPose.rx); - centerRobotPos.pitch = CCoordinateTransform::radToDeg(robotPose.ry); - centerRobotPos.yaw = CCoordinateTransform::radToDeg(robotPose.rz); + centerRobotPos.x = ptRobot.x; + centerRobotPos.y = ptRobot.y; + centerRobotPos.z = ptRobot.z; + centerRobotPos.roll = robotRpy.roll; // 已经是角度 + centerRobotPos.pitch = robotRpy.pitch; // 已经是角度 + centerRobotPos.yaw = robotRpy.yaw; // 已经是角度 detectionResult.positions.push_back(centerRobotPos); @@ -246,16 +281,19 @@ int DetectPresenter::DetectWorkpieceHole( objOps.push_back(holePoints); if(debugParam.enableDebug && debugParam.printDetailLog){ - LOG_INFO("[Algo Thread] Direction vectors: X=(%.3f,%.3f,%.3f), Y=(%.3f,%.3f,%.3f), Z=(%.3f,%.3f,%.3f)\n", + 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, workpiece.y_dir.x, workpiece.y_dir.y, workpiece.y_dir.z, workpiece.z_dir.x, workpiece.z_dir.y, workpiece.z_dir.z); - LOG_INFO("[Algo Thread] Center Eye Coords: X=%.2f, Y=%.2f, Z=%.2f, R=%.4f, P=%.4f, Y=%.4f\n", - cameraPose.x, cameraPose.y, cameraPose.z, - CCoordinateTransform::radToDeg(cameraPose.rx), CCoordinateTransform::radToDeg(cameraPose.ry), CCoordinateTransform::radToDeg(cameraPose.rz)); + LOG_INFO("[Algo Thread] Direction vectors (robot): X=(%.3f,%.3f,%.3f), Y=(%.3f,%.3f,%.3f), Z=(%.3f,%.3f,%.3f)\n", + 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", - robotPose.x, robotPose.y, robotPose.z, - CCoordinateTransform::radToDeg(robotPose.rx), CCoordinateTransform::radToDeg(robotPose.ry), CCoordinateTransform::radToDeg(robotPose.rz)); + ptRobot.x, ptRobot.y, ptRobot.z, + centerRobotPos.roll, centerRobotPos.pitch, centerRobotPos.yaw); } } diff --git a/App/WorkpieceHole/WorkpieceHoleApp/Presenter/Src/PLCModbusClient.cpp b/App/WorkpieceHole/WorkpieceHoleApp/Presenter/Src/PLCModbusClient.cpp index 61e64125..e7570523 100644 --- a/App/WorkpieceHole/WorkpieceHoleApp/Presenter/Src/PLCModbusClient.cpp +++ b/App/WorkpieceHole/WorkpieceHoleApp/Presenter/Src/PLCModbusClient.cpp @@ -240,6 +240,10 @@ void PLCModbusClient::pollThreadFunc() notifyPhotoRequested(1); } m_lastPhotoRequestState = currentState; + } else { + // 读取失败，可能是连接断开，主动断开连接以触发重连 + LOG_WARNING("Failed to read photo request, disconnecting to trigger reconnect\n"); + disconnectPLC(); } std::this_thread::sleep_for(std::chrono::milliseconds(m_pollIntervalMs)); @@ -277,6 +281,16 @@ bool PLCModbusClient::tryConnectPLC() return true; } +void PLCModbusClient::disconnectPLC() +{ + std::lock_guard locker(m_mutex); + + if (m_plcClient) { + m_plcClient->Disconnect(); + LOG_INFO("PLC disconnected for reconnection\n"); + } +} + int PLCModbusClient::readPhotoRequest() { std::lock_guard locker(m_mutex); diff --git a/App/WorkpieceHole/WorkpieceHoleApp/Presenter/Src/WorkpieceHolePresenter.cpp b/App/WorkpieceHole/WorkpieceHoleApp/Presenter/Src/WorkpieceHolePresenter.cpp index c3ef9653..84de85dc 100644 --- a/App/WorkpieceHole/WorkpieceHoleApp/Presenter/Src/WorkpieceHolePresenter.cpp +++ b/App/WorkpieceHole/WorkpieceHoleApp/Presenter/Src/WorkpieceHolePresenter.cpp @@ -380,11 +380,15 @@ int WorkpieceHolePresenter::ProcessAlgoDetection(std::vectorDetectWorkpieceHole(m_currentCameraIndex, detectionDataCache, algorithmParams, debugParam, m_dataLoader, - currentClibMatrix.clibMatrix, eulerOrder, detectionResult); + currentClibMatrix.clibMatrix, eulerOrder, dirVectorInvert, detectionResult); // 根据项目类型选择处理方式 if (GetStatusCallback()) { QString err = QString("错误:%1").arg(nRet); @@ -729,8 +733,9 @@ int WorkpieceHolePresenter::InitPLCModbus() LOG_INFO("PLC connection state changed: PLC=%s\n", plcConnected ? "connected" : "disconnected"); - // 通知 UI 更新连接状态 + // 通知 UI 更新连接状态（PLC连接状态关联到机械臂状态指示器） if (auto pStatus = GetStatusCallback()) { + pStatus->OnRobotConnectionChanged(plcConnected); std::string statusMsg = std::string("PLC:") + (plcConnected ? "已连接" : "断开"); pStatus->OnStatusUpdate(statusMsg); } @@ -743,6 +748,15 @@ int WorkpieceHolePresenter::InitPLCModbus() } }); + // 设置重连回调，通知用户正在尝试重连 + m_pPLCModbusClient->SetReconnectingCallback([this](const std::string& device, int attempt) { + LOG_INFO("Attempting to reconnect %s (attempt %d)\n", device.c_str(), attempt); + if (auto pStatus = GetStatusCallback()) { + std::string statusMsg = device + "正在重连(第" + std::to_string(attempt) + "次)..."; + pStatus->OnStatusUpdate(statusMsg); + } + }); + // 构建寄存器地址配置 PLCModbusClient::RegisterConfig regConfig; regConfig.addrPhotoRequest = configResult.plcRobotServerConfig.registerConfig.addrPhotoRequest; @@ -825,6 +839,11 @@ void WorkpieceHolePresenter::SendCoordinateDataToPLC(const WorkpieceHoleDetectio return; } + // 获取姿态输出顺序配置 + ConfigResult configResult = m_pConfigManager->GetConfigResult(); + int poseOutputOrder = configResult.plcRobotServerConfig.poseOutputOrder; + LOG_INFO("Using pose output order: %d\n", poseOutputOrder); + // 构建坐标数据列表（每个工件的中心点+姿态，最多10个） std::vector coords; int pointCount = std::min(static_cast(result.positions.size()), PLCModbusClient::MAX_POINTS); @@ -837,13 +856,50 @@ void WorkpieceHolePresenter::SendCoordinateDataToPLC(const WorkpieceHoleDetectio coord.x = static_cast(pos.x); coord.y = static_cast(pos.y); coord.z = static_cast(pos.z); - coord.roll = static_cast(pos.roll); - coord.pitch = static_cast(pos.pitch); - coord.yaw = static_cast(pos.yaw); + + // 根据姿态输出顺序配置调整输出 + switch (poseOutputOrder) { + case POSE_ORDER_RPY: // Roll, Pitch, Yaw（默认） + coord.roll = static_cast(pos.roll); + coord.pitch = static_cast(pos.pitch); + coord.yaw = static_cast(pos.yaw); + break; + case POSE_ORDER_RYP: // Roll, Yaw, Pitch + coord.roll = static_cast(pos.roll); + coord.pitch = static_cast(pos.yaw); + coord.yaw = static_cast(pos.pitch); + break; + case POSE_ORDER_PRY: // Pitch, Roll, Yaw + coord.roll = static_cast(pos.pitch); + coord.pitch = static_cast(pos.roll); + coord.yaw = static_cast(pos.yaw); + break; + case POSE_ORDER_PYR: // Pitch, Yaw, Roll + coord.roll = static_cast(pos.pitch); + coord.pitch = static_cast(pos.yaw); + coord.yaw = static_cast(pos.roll); + break; + case POSE_ORDER_YRP: // Yaw, Roll, Pitch + coord.roll = static_cast(pos.yaw); + coord.pitch = static_cast(pos.roll); + coord.yaw = static_cast(pos.pitch); + break; + case POSE_ORDER_YPR: // Yaw, Pitch, Roll + coord.roll = static_cast(pos.yaw); + coord.pitch = static_cast(pos.pitch); + coord.yaw = static_cast(pos.roll); + break; + default: // 默认 RPY + coord.roll = static_cast(pos.roll); + coord.pitch = static_cast(pos.pitch); + coord.yaw = static_cast(pos.yaw); + break; + } coords.push_back(coord); - LOG_INFO("Workpiece[%d]: X=%.2f, Y=%.2f, Z=%.2f, P=%.2f, R=%.2f, Y=%.2f\n", i, coord.x, coord.y, coord.z, coord.roll, coord.pitch, coord.yaw); + LOG_INFO("Workpiece[%d]: X=%.2f, Y=%.2f, Z=%.2f, R1=%.2f, R2=%.2f, R3=%.2f (order=%d)\n", + i, coord.x, coord.y, coord.z, coord.roll, coord.pitch, coord.yaw, poseOutputOrder); } LOG_INFO("Sending %d workpiece positions to PLC\n", pointCount); diff --git a/App/WorkpieceHole/WorkpieceHoleApp/Version.h b/App/WorkpieceHole/WorkpieceHoleApp/Version.h index c0a1f52d..8066616c 100644 --- a/App/WorkpieceHole/WorkpieceHoleApp/Version.h +++ b/App/WorkpieceHole/WorkpieceHoleApp/Version.h @@ -3,7 +3,7 @@ // 版本字符串 #define WORKPIECEHOLE_VERSION_STRING "1.0.1" -#define WORKPIECEHOLE_BUILD_STRING "2" +#define WORKPIECEHOLE_BUILD_STRING "5" #define WORKPIECEHOLE_FULL_VERSION_STRING "V" WORKPIECEHOLE_VERSION_STRING "_" WORKPIECEHOLE_BUILD_STRING // 构建日期 diff --git a/App/WorkpieceHole/WorkpieceHoleApp/Version.md b/App/WorkpieceHole/WorkpieceHoleApp/Version.md index 8edbb191..850c184e 100644 --- a/App/WorkpieceHole/WorkpieceHoleApp/Version.md +++ b/App/WorkpieceHole/WorkpieceHoleApp/Version.md @@ -1,4 +1,10 @@ # 1.0.1 +## build_4 +1. 增加了协议姿态输出顺序 + +## build_3 2026-02-01 +1. 更新了算法，输出的姿态 + ## build_2 2026-02-01 1. 协议测试修正 diff --git a/App/WorkpieceHole/WorkpieceHoleApp/WorkpieceHoleApp.pro b/App/WorkpieceHole/WorkpieceHoleApp/WorkpieceHoleApp.pro index e2625c61..11a14436 100644 --- a/App/WorkpieceHole/WorkpieceHoleApp/WorkpieceHoleApp.pro +++ b/App/WorkpieceHole/WorkpieceHoleApp/WorkpieceHoleApp.pro @@ -4,7 +4,7 @@ QT += core gui network serialport greaterThan(QT_MAJOR_VERSION, 4): QT += widgets -CONFIG += c++17 +CONFIG += c++11 # 项目名称 TARGET = WorkpieceHoleApp @@ -44,6 +44,10 @@ INCLUDEPATH += $$PWD/../../../DataUtils/CoordinateTransform/Inc # eigen 依赖 INCLUDEPATH += $$PWD/../../../SDK/eigen-3.3.9 +#opencv +INCLUDEPATH += $$PWD/../../../SDK/OpenCV320/include + + # ModbusTCP 客户端库（只包含导出接口目录，不包含内部 modbus 头文件） INCLUDEPATH += $$PWD/../../../Module/ModbusTCPClient/Inc diff --git a/App/WorkpieceHole/WorkpieceHoleApp/dialogalgoarg.cpp b/App/WorkpieceHole/WorkpieceHoleApp/dialogalgoarg.cpp index cc0d53f4..0dda9be8 100644 --- a/App/WorkpieceHole/WorkpieceHoleApp/dialogalgoarg.cpp +++ b/App/WorkpieceHole/WorkpieceHoleApp/dialogalgoarg.cpp @@ -38,6 +38,12 @@ DialogAlgoarg::DialogAlgoarg(ConfigManager* configManager, QWidget *parent) // 初始化旋转顺序下拉框 InitEulerOrderComboBox(); + // 初始化姿态输出顺序下拉框 + InitPoseOutputOrderComboBox(); + + // 初始化方向向量反向下拉框 + InitDirVectorInvertComboBox(); + // 从配置文件加载数据到界面 LoadConfigToUI(); @@ -387,19 +393,56 @@ void DialogAlgoarg::InitEulerOrderComboBox() ui->comboBox_eulerOrder->setCurrentIndex(0); } +void DialogAlgoarg::InitPoseOutputOrderComboBox() +{ + // 添加姿态输出顺序选项 + ui->comboBox_poseOutputOrder->addItem("RPY (Roll, Pitch, Yaw)", POSE_ORDER_RPY); // 默认 + ui->comboBox_poseOutputOrder->addItem("RYP (Roll, Yaw, Pitch)", POSE_ORDER_RYP); + ui->comboBox_poseOutputOrder->addItem("PRY (Pitch, Roll, Yaw)", POSE_ORDER_PRY); + ui->comboBox_poseOutputOrder->addItem("PYR (Pitch, Yaw, Roll)", POSE_ORDER_PYR); + ui->comboBox_poseOutputOrder->addItem("YRP (Yaw, Roll, Pitch)", POSE_ORDER_YRP); + ui->comboBox_poseOutputOrder->addItem("YPR (Yaw, Pitch, Roll)", POSE_ORDER_YPR); + + // 默认选择 RPY + ui->comboBox_poseOutputOrder->setCurrentIndex(0); +} + +void DialogAlgoarg::InitDirVectorInvertComboBox() +{ + // 添加方向向量反向选项 + ui->comboBox_dirVectorInvert->addItem("不反向", DIR_INVERT_NONE); + ui->comboBox_dirVectorInvert->addItem("XY反向", DIR_INVERT_XY); + ui->comboBox_dirVectorInvert->addItem("XZ反向", DIR_INVERT_XZ); + ui->comboBox_dirVectorInvert->addItem("YZ反向（默认）", DIR_INVERT_YZ); + + // 默认选择 YZ反向（兼容原有行为） + ui->comboBox_dirVectorInvert->setCurrentIndex(3); +} + void DialogAlgoarg::LoadPlcRobotServerConfigToUI(const VrPlcRobotServerConfig& config) { if (!ui) return; + // 加载PLC服务端配置 ui->lineEdit_plcServerIp->setText(QString::fromStdString(config.plcServerIp)); ui->lineEdit_plcServerPort->setText(QString::number(config.plcServerPort)); - ui->lineEdit_robotServerIp->setText(QString::fromStdString(config.robotServerIp)); - ui->lineEdit_robotServerPort->setText(QString::number(config.robotServerPort)); // 加载PLC寄存器地址配置 ui->lineEdit_addrPhotoRequest->setText(QString::number(config.registerConfig.addrPhotoRequest)); ui->lineEdit_addrDataComplete->setText(QString::number(config.registerConfig.addrDataComplete)); ui->lineEdit_addrCoordDataStart->setText(QString::number(config.registerConfig.addrCoordDataStart)); + + // 加载姿态输出顺序 + int poseOrderIndex = ui->comboBox_poseOutputOrder->findData(config.poseOutputOrder); + if (poseOrderIndex >= 0) { + ui->comboBox_poseOutputOrder->setCurrentIndex(poseOrderIndex); + } + + // 加载方向向量反向配置 + int dirInvertIndex = ui->comboBox_dirVectorInvert->findData(config.dirVectorInvert); + if (dirInvertIndex >= 0) { + ui->comboBox_dirVectorInvert->setCurrentIndex(dirInvertIndex); + } } bool DialogAlgoarg::SavePlcRobotServerConfigFromUI(VrPlcRobotServerConfig& config) @@ -420,19 +463,6 @@ bool DialogAlgoarg::SavePlcRobotServerConfigFromUI(VrPlcRobotServerConfig& confi return false; } - // 获取机械臂服务端IP - QString robotIp = ui->lineEdit_robotServerIp->text().trimmed(); - if (robotIp.isEmpty()) { - return false; - } - config.robotServerIp = robotIp.toStdString(); - - // 获取机械臂服务端端口 - config.robotServerPort = ui->lineEdit_robotServerPort->text().toInt(&ok); - if (!ok || config.robotServerPort <= 0 || config.robotServerPort > 65535) { - return false; - } - // 获取PLC寄存器地址配置 config.registerConfig.addrPhotoRequest = ui->lineEdit_addrPhotoRequest->text().toInt(&ok); if (!ok || config.registerConfig.addrPhotoRequest < 0) { @@ -449,5 +479,11 @@ bool DialogAlgoarg::SavePlcRobotServerConfigFromUI(VrPlcRobotServerConfig& confi return false; } + // 获取姿态输出顺序 + config.poseOutputOrder = ui->comboBox_poseOutputOrder->currentData().toInt(); + + // 获取方向向量反向配置 + config.dirVectorInvert = ui->comboBox_dirVectorInvert->currentData().toInt(); + return true; } diff --git a/App/WorkpieceHole/WorkpieceHoleApp/dialogalgoarg.h b/App/WorkpieceHole/WorkpieceHoleApp/dialogalgoarg.h index ed6e7ab8..35ab1a7f 100644 --- a/App/WorkpieceHole/WorkpieceHoleApp/dialogalgoarg.h +++ b/App/WorkpieceHole/WorkpieceHoleApp/dialogalgoarg.h @@ -40,9 +40,11 @@ private: bool SaveFilterParamFromUI(VrOutlierFilterParam& param); bool SaveGrowParamFromUI(VrTreeGrowParam& param); - // 网络配置（PLC和机械臂服务端） + // 网络配置（PLC服务端） void LoadPlcRobotServerConfigToUI(const VrPlcRobotServerConfig& config); bool SavePlcRobotServerConfigFromUI(VrPlcRobotServerConfig& config); + void InitPoseOutputOrderComboBox(); + void InitDirVectorInvertComboBox(); // 手眼标定矩阵 void LoadCalibMatrixToUI(); diff --git a/App/WorkpieceHole/WorkpieceHoleApp/dialogalgoarg.ui b/App/WorkpieceHole/WorkpieceHoleApp/dialogalgoarg.ui index aa5aa1cc..e2283f98 100644 --- a/App/WorkpieceHole/WorkpieceHoleApp/dialogalgoarg.ui +++ b/App/WorkpieceHole/WorkpieceHoleApp/dialogalgoarg.ui @@ -62,7 +62,7 @@ QGroupBox { color: rgb(221, 225, 233); border: 1px solid rgb(100, 100, 100); bor QGroupBox::title { subcontrol-origin: margin; left: 10px; padding: 0 3px 0 3px; } - 0 + 5 @@ -809,7 +809,7 @@ QComboBox QAbstractItemView { 10 20 671 - 251 + 291 @@ -826,7 +826,7 @@ QComboBox QAbstractItemView { 20 40 631 - 200 + 246 @@ -950,106 +950,120 @@ QComboBox QAbstractItemView { - - - - - - - 10 - 290 - 671 - 201 - - - - - 16 - - - - 机械臂服务端配置 - - - - - 20 - 40 - 631 - 148 - - - - - + + - 16 + 14 - 机械臂服务端IP地址 + 姿态输出顺序 - - + + - 16 + 14 - - 例如: 192.168.1.20 + + QComboBox { + background-color: rgb(50, 52, 56); + color: rgb(221, 225, 233); + border: 1px solid rgb(80, 82, 86); + border-radius: 4px; + padding: 5px; +} +QComboBox::drop-down { + border: none; +} +QComboBox QAbstractItemView { + background-color: rgb(50, 52, 56); + color: rgb(221, 225, 233); + selection-background-color: rgb(60, 120, 180); +} - - + + - 16 + 14 - 机械臂服务端端口 + 方向向量反向 - - + + - 16 + 14 - - 例如: 502 + + QComboBox { + background-color: rgb(50, 52, 56); + color: rgb(221, 225, 233); + border: 1px solid rgb(80, 82, 86); + border-radius: 4px; + padding: 5px; +} +QComboBox::drop-down { + border: none; +} +QComboBox QAbstractItemView { + background-color: rgb(50, 52, 56); + color: rgb(221, 225, 233); + selection-background-color: rgb(60, 120, 180); +} - + - + - 190 + 2 670 - 100 - 45 + 751 + 44 - - - 16 - true - - - - QPushButton { + + + + + Qt::Horizontal + + + + 178 + 20 + + + + + + + + + 16 + true + + + + QPushButton { background-color: rgb(60, 120, 180); color: rgb(221, 225, 233); border: none; @@ -1062,28 +1076,35 @@ QPushButton:hover { QPushButton:pressed { background-color: rgb(40, 100, 160); } - - - 保存 - - - - - - 410 - 670 - 100 - 45 - - - - - 16 - true - - - - QPushButton { + + + 保存 + + + + + + + Qt::Horizontal + + + + 50 + 20 + + + + + + + + + 16 + true + + + + QPushButton { background-color: rgb(120, 120, 120); color: rgb(221, 225, 233); border: none; @@ -1096,10 +1117,26 @@ QPushButton:hover { QPushButton:pressed { background-color: rgb(100, 100, 100); } - - - 取消 - + + + 取消 + + + + + + + Qt::Horizontal + + + + 178 + 20 + + + + + diff --git a/App/WorkpieceHole/WorkpieceHoleConfig/Inc/IVrConfig.h b/App/WorkpieceHole/WorkpieceHoleConfig/Inc/IVrConfig.h index 7bc0c3da..6687b6a7 100644 --- a/App/WorkpieceHole/WorkpieceHoleConfig/Inc/IVrConfig.h +++ b/App/WorkpieceHole/WorkpieceHoleConfig/Inc/IVrConfig.h @@ -107,6 +107,35 @@ struct VrPlcRegisterConfig VrPlcRegisterConfig() = default; }; +/** + * @brief 姿态输出顺序枚举 + * + * 定义机械臂姿态数据的输出顺序 + */ +enum VrPoseOutputOrder +{ + POSE_ORDER_RPY = 0, // Roll, Pitch, Yaw（默认） + POSE_ORDER_RYP = 1, // Roll, Yaw, Pitch + POSE_ORDER_PRY = 2, // Pitch, Roll, Yaw + POSE_ORDER_PYR = 3, // Pitch, Yaw, Roll + POSE_ORDER_YRP = 4, // Yaw, Roll, Pitch + POSE_ORDER_YPR = 5 // Yaw, Pitch, Roll +}; + +/** + * @brief 方向向量反向配置枚举 + * + * 定义机械臂方向向量的反向配置 + * 用于调整相机坐标系到机械臂坐标系的方向向量转换 + */ +enum VrDirVectorInvert +{ + DIR_INVERT_NONE = 0, // 不反向 + DIR_INVERT_XY = 1, // X和Y方向反向 + DIR_INVERT_XZ = 2, // X和Z方向反向 + DIR_INVERT_YZ = 3 // Y和Z方向反向（默认，兼容原有行为） +}; + /** * @brief PLC和机械臂服务端配置 * @@ -121,6 +150,8 @@ struct VrPlcRobotServerConfig std::string robotServerIp = "192.168.0.90"; // 机械臂服务端IP地址（配天机械臂） int robotServerPort = 502; // 机械臂服务端端口（Modbus TCP默认502） VrPlcRegisterConfig registerConfig; // PLC 寄存器地址配置 + int poseOutputOrder = POSE_ORDER_RPY; // 姿态输出顺序，默认RPY + int dirVectorInvert = DIR_INVERT_YZ; // 方向向量反向配置，默认YZ反向（兼容原有行为） // 显式赋值构造函数 VrPlcRobotServerConfig& operator=(const VrPlcRobotServerConfig& other) { @@ -130,6 +161,8 @@ struct VrPlcRobotServerConfig robotServerIp = other.robotServerIp; robotServerPort = other.robotServerPort; registerConfig = other.registerConfig; + poseOutputOrder = other.poseOutputOrder; + dirVectorInvert = other.dirVectorInvert; } return *this; } @@ -140,7 +173,9 @@ struct VrPlcRobotServerConfig , plcServerPort(other.plcServerPort) , robotServerIp(other.robotServerIp) , robotServerPort(other.robotServerPort) - , registerConfig(other.registerConfig) { + , registerConfig(other.registerConfig) + , poseOutputOrder(other.poseOutputOrder) + , dirVectorInvert(other.dirVectorInvert) { } // 默认构造函数 diff --git a/App/WorkpieceHole/WorkpieceHoleConfig/Src/VrConfig.cpp b/App/WorkpieceHole/WorkpieceHoleConfig/Src/VrConfig.cpp index 38e4265e..1bd93ff0 100644 --- a/App/WorkpieceHole/WorkpieceHoleConfig/Src/VrConfig.cpp +++ b/App/WorkpieceHole/WorkpieceHoleConfig/Src/VrConfig.cpp @@ -274,6 +274,18 @@ int CVrConfig::LoadConfig(const std::string& filePath, ConfigResult& configResul configResult.plcRobotServerConfig.registerConfig.addrDataComplete = tcpServerConfigElement->IntAttribute("addrDataComplete"); if (tcpServerConfigElement->Attribute("addrCoordDataStart")) configResult.plcRobotServerConfig.registerConfig.addrCoordDataStart = tcpServerConfigElement->IntAttribute("addrCoordDataStart"); + + // 解析姿态输出顺序配置 + if (tcpServerConfigElement->Attribute("poseOutputOrder")) + configResult.plcRobotServerConfig.poseOutputOrder = tcpServerConfigElement->IntAttribute("poseOutputOrder"); + else + configResult.plcRobotServerConfig.poseOutputOrder = POSE_ORDER_RPY; // 默认RPY + + // 解析方向向量反向配置 + if (tcpServerConfigElement->Attribute("dirVectorInvert")) + configResult.plcRobotServerConfig.dirVectorInvert = tcpServerConfigElement->IntAttribute("dirVectorInvert"); + else + configResult.plcRobotServerConfig.dirVectorInvert = DIR_INVERT_YZ; // 默认YZ反向（兼容原有行为） } return LOAD_CONFIG_SUCCESS; @@ -431,6 +443,10 @@ bool CVrConfig::SaveConfig(const std::string& filePath, ConfigResult& configResu tcpServerConfigElement->SetAttribute("addrPhotoRequest", configResult.plcRobotServerConfig.registerConfig.addrPhotoRequest); tcpServerConfigElement->SetAttribute("addrDataComplete", configResult.plcRobotServerConfig.registerConfig.addrDataComplete); tcpServerConfigElement->SetAttribute("addrCoordDataStart", configResult.plcRobotServerConfig.registerConfig.addrCoordDataStart); + // 保存姿态输出顺序配置 + tcpServerConfigElement->SetAttribute("poseOutputOrder", configResult.plcRobotServerConfig.poseOutputOrder); + // 保存方向向量反向配置 + tcpServerConfigElement->SetAttribute("dirVectorInvert", configResult.plcRobotServerConfig.dirVectorInvert); root->InsertEndChild(tcpServerConfigElement); // 保存到文件 diff --git a/App/WorkpieceHole/WorkpieceHoleConfig/config/config.xml b/App/WorkpieceHole/WorkpieceHoleConfig/config/config.xml index b0e1b966..a9a4a5c6 100644 --- a/App/WorkpieceHole/WorkpieceHoleConfig/config/config.xml +++ b/App/WorkpieceHole/WorkpieceHoleConfig/config/config.xml @@ -24,5 +24,5 @@ - + diff --git a/AppAlgo/workpieceHolePositioning/Arm/aarch64/libbaseAlgorithm.so b/AppAlgo/workpieceHolePositioning/Arm/aarch64/libbaseAlgorithm.so index f72ae99a..ca3a5326 100644 Binary files a/AppAlgo/workpieceHolePositioning/Arm/aarch64/libbaseAlgorithm.so and b/AppAlgo/workpieceHolePositioning/Arm/aarch64/libbaseAlgorithm.so differ diff --git a/AppAlgo/workpieceHolePositioning/Arm/aarch64/libworkpieceHolePositioning.so b/AppAlgo/workpieceHolePositioning/Arm/aarch64/libworkpieceHolePositioning.so index 9c2ab543..3ac8d924 100644 Binary files a/AppAlgo/workpieceHolePositioning/Arm/aarch64/libworkpieceHolePositioning.so and b/AppAlgo/workpieceHolePositioning/Arm/aarch64/libworkpieceHolePositioning.so differ diff --git a/AppAlgo/workpieceHolePositioning/Inc/SG_baseAlgo_Export.h b/AppAlgo/workpieceHolePositioning/Inc/SG_baseAlgo_Export.h new file mode 100644 index 00000000..cca6972f --- /dev/null +++ b/AppAlgo/workpieceHolePositioning/Inc/SG_baseAlgo_Export.h @@ -0,0 +1,755 @@ +#pragma once + +#include "SG_algo_Export.h" +#include +#include + + +//˳Ⱥ:z޷Ϊεж +SG_APISHARED_EXPORT void sg_lineDataRemoveOutlier( + SVzNL3DPosition* lineData, + int dataSize, + SSG_outlierFilterParam filterParam, + std::vector& filerData, + std::vector& noisePts); +//˳Ⱥ㣺z޷,ıԭʼ +SG_APISHARED_EXPORT void sg_lineDataRemoveOutlier_changeOriginData( + SVzNL3DPosition* lineData, + int dataSize, + SSG_outlierFilterParam filterParam); +//˳Ⱥ㣺z޷, vecotr +SG_APISHARED_EXPORT void wd_vectorDataRemoveOutlier_overwrite( + std::vector& a_line, + SSG_outlierFilterParam filterParam); +//˳Ⱥ㣺޷Ϊεж +SG_APISHARED_EXPORT void sg_lineDataRemoveOutlier_ptDistMethod( + SVzNL3DPosition* lineData, + int dataSize, + SSG_outlierFilterParam filterParam, + std::vector& filerData, + std::vector& noisePts); + +//ƽ +SG_APISHARED_EXPORT void sg_lineDataSmoothing( + std::vector& input, + int smoothWin, + std::vector& output); +//ֶƽӰֶζ˵ +SG_APISHARED_EXPORT void sg_lineSegSmoothing( + std::vector& input, + double seg_y_deltaTh, //ֶεYڴ˼Ϊµķֶ + double seg_z_deltaTh,//ֶεZڴ˼Ϊµķֶ + int smoothWin, + std::vector& output); + +//VZ_APISHARED_EXPORT void sg_getLineMeanVar(); +SG_APISHARED_EXPORT void sg_getLineLVFeature( + SVzNL3DPosition* lineData, + int dataSize, + int lineIdx, + const SSG_slopeParam slopeParam, + const SSG_VFeatureParam valleyPara, + SSG_lineFeature* line_features); + +//ȡɨ߹յ +SG_APISHARED_EXPORT void sg_getLineCornerFeature( + SVzNL3DPosition* lineData, + int dataSize, + int lineIdx, + const SSG_cornerParam cornerPara, //scaleͨȡbagH1/4 + SSG_lineFeature* line_features); + +// ԼMask(nPointIdxת嶨壩ָĵȡϵĹյ +SG_APISHARED_EXPORT void sg_maskData_getLineCornerFeature( + std::vector< SVzNL3DPosition>& lineData, + int lineIdx, + int maskID, + const SSG_cornerParam cornerPara, //scaleͨȡbagH1/4 + std::vector& features); + +//ȡGapGapCornerɵ +SG_APISHARED_EXPORT void sg_getLineGapFeature( + std::vector& lineData, //ɨ + int lineIdx, //ǰɨ + const SSG_cornerParam cornerPara, + const SVzNLRangeD gapParam, + std::vector& line_gaps); + +SG_APISHARED_EXPORT void sg_getLineCornerFeature_BQ( + SVzNL3DPosition* lineData, + int dataSize, + int lineIdx, + double refSteppingZ, + const SSG_cornerParam cornerPara, + std::vector& line_features); + +/// +/// ȡϵĹյ˵㸽ĹյΪϸյ +/// Segcornerȡ +/// nPointIdx¶Feature +/// 㷨̣ +/// 1ǰǺͺ +/// 2սǣ˳ʱΪʱΪ +/// 3սǵļֵ +/// 4жϹսǷΪ +/// +SG_APISHARED_EXPORT void sg_getLineContourCornerFeature_groundMask_BQ( + std::vector& lineData, + std::vector groundMask, + int lineIdx, + double contourWin, //groundԵΧ + const SSG_cornerParam cornerPara, //scaleͨȡbagH1/4 + std::vector& line_features); + +SG_APISHARED_EXPORT void wd_getLineDataIntervals( + std::vector& lineData, + const SSG_lineSegParam lineSegPara, + std::vector& segs); + +// С㼯Сڴ޷ֱߣ +const int MIN_POINT_COUNT = 3; +//ʹö˵ֱߣ㵽ֱߵľ룬޵ķָ +SG_APISHARED_EXPORT void split( + SSG_RUN a_run, + std::vector< SVzNL3DPosition>& lineData, + const double maxError, + std::vector< SSG_RUN>& lineSegs); + +/// +/// ȡϵ䡢zֵVLͣ⣨PSM) +/// seg˵㣺z +/// nPointIdx¶Feature +/// 㷨̣ +/// 1ǰǺͺ +/// 2սǣ˳ʱΪʱΪ +/// 3սǵļֵ +/// 4жϹսǷΪ +/// +SG_APISHARED_EXPORT void wd_getLineCornerFeature_PSM( + SVzNL3DPosition* lineData, + int dataSize, + int lineIdx, + const double groundZ, + const SSG_cornerParam cornerPara, + SSG_lineFeature* line_features); + +/// ȡϵԲϰλԲĿȷ +/// seg˵㣺z +/// nPointIdx¶Feature +/// 㷨̣ +/// 1ǰǺͺ +/// 2սǣ˳ʱΪʱΪ +/// 3սǵļֵ +/// 4жϹսǷΪ +/// +SG_APISHARED_EXPORT void wd_getRingArcFeature( + std::vector< SVzNL3DPosition>& lineData, + int lineIdx, + const SSG_cornerParam cornerPara, + SVzNLRangeD ringArcWidth, //ӵĻ + std::vector& line_ringArcs // +); + +/// +/// ȡϵĹյPSM LVTypeFeature, BQȹյ㷨Ļϵİ汾 +/// nPointIdx¶Feature +/// 㷨̣ +/// 1ǰǺͺ +/// 2սǣ˳ʱΪʱΪ +/// 3սǵļֵ +/// 4жϹսǷΪ +/// +SG_APISHARED_EXPORT void wd_getLineCorerFeature( + std::vector< SVzNL3DPosition>& lineData, + int lineIdx, + const SSG_cornerParam cornerPara, + std::vector& line_cornerFeatures //յ +); + +/// ȡϵĹյPSM LVTypeFeature, BQȹյ㷨Ļϵİ汾 +/// ʹƽм +/// nPointIdx¶Feature +/// 㷨̣ +/// 1ǰǺͺ +/// 2սǣ˳ʱΪʱΪ +/// 3սǵļֵ +/// 4жϹսǷΪ +/// +SG_APISHARED_EXPORT void wd_getLineCorerFeature_accelerate( + std::vector< SVzNL3DPosition>& lineData, + int lineIdx, + const SSG_cornerParam cornerPara, + const double pointInterval, + std::vector& segs, + std::vector& line_cornerFeatures //յ +); + +//ȡ͹ +SG_APISHARED_EXPORT void wd_getLineRaisedFeature( + std::vector< SVzNL3DPosition>& lineData, + int lineIdx, + SSG_raisedFeatureParam raisedFeaturePara, //͹ + std::vector& line_raisedFeatures //͹ +); + +//ֱȡsplit-and-merge˼򻯣յֱߴֱ +SG_APISHARED_EXPORT void wd_surfaceLineSegment( + std::vector< SVzNL3DPosition>& lineData, + int lineIdx, + SVzNLRangeD lineLenRange, + const SSG_lineSegParam lineSegPara, + std::vector& lineSegs, + std::vector& invlaidLineSegs); + +#if 0 +//һıԵȡ +//㷨ȻȡɨϸΣȻԶεĶ˵дõԵ +void wd_getSurfaceEdgeFeatures( + std::vector< SVzNL3DPosition>& lineData, + int lineIdx, + const SSG_lineSegParam lineSegPara, + std::vector& edgeFeatures); +#endif + +/// +/// ȡϵJumping +/// nPointIdx¶Feature +/// 㷨̣ +/// 1ǰǺͺ +/// 2սǣ˳ʱΪʱΪ +/// 3սǵļֵ +/// 4жϹսǷΪ +/// +SG_APISHARED_EXPORT void sg_getLineJumpFeature_cornerMethod( + SVzNL3DPosition* lineData, + int dataSize, + int lineIdx, + const SSG_cornerParam cornerPara, //scaleͨȡbagH1/4 + std::vector< SSG_basicFeature1D>& jumpFeatures); + +SG_APISHARED_EXPORT void wd_getLineGloveArcs( + std::vector& lineData, + int lineIdx, + const SSG_gloveArcParam arcPara, + std::vector& gloveArcs); + +/// ȡϵԲ +SG_APISHARED_EXPORT void sg_getLineUpperSemiCircleFeature( + SVzNL3DPosition* lineData, + int dataSize, + int lineIdx, + const double sieveDiameter, + const SSG_slopeParam slopePara, + std::vector< SSG_featureSemiCircle>& line_features, + cv::Mat& holeMask); + +/// +/// ȡϵļֵ㣨ֵͼСֵ㣩 +/// +/// +SG_APISHARED_EXPORT void sg_getLineLocalPeaks( + SVzNL3DPosition* lineData, + int dataSize, + int lineIdx, + const double scaleWin, + std::vector< SSG_basicFeature1D>& localMax, + std::vector< SSG_basicFeature1D>& localMin); + +SG_APISHARED_EXPORT void sg_getFlatLineLocalPeaks_vector( + std::vector& lineData, + int lineIdx, + const double scaleWin, + const double minPkHeighth, + const double holeR, + std::vector< SSG_basicFeature1D>& localMax); + +SG_APISHARED_EXPORT void sg_getLineDownJumps( + SVzNL3DPosition* lineData, + int dataSize, + int lineIdx, + const double jumpTh, + std::vector< SSG_basicFeature1D>& downJumps); + +//feature +SG_APISHARED_EXPORT void sg_getFeatureGrowingTrees( + std::vector& lineFeatures, + std::vector& trees, + SSG_treeGrowParam growParam); + +//feature:ȽtypeֻжλǷ +SG_APISHARED_EXPORT void wd_getFeatureGrowingTrees_noTypeMatch( + std::vector& lineFeatures, + std::vector& feature_trees, + std::vector& ending_trees, + SSG_treeGrowParam growParam); + +//segment feature +SG_APISHARED_EXPORT void wd_getSegFeatureGrowingTrees( + std::vector>& all_lineFeatures, + std::vector& feature_trees, + SSG_treeGrowParam growParam); + +SG_APISHARED_EXPORT void sg_lineFeaturesGrowing( + int lineIdx, + bool isLastLine, + std::vector& features, + std::vector& trees, + SSG_treeGrowParam growParam); + +//corner +SG_APISHARED_EXPORT void sg_cornerFeaturesGrowing( + std::vector>& cornerFeatures, + std::vector& trees, + SSG_treeGrowParam growParam); + +//gap +SG_APISHARED_EXPORT void sg_lineGapsGrowing( + int lineIdx, + bool isLastLine, + std::vector& features, + std::vector& trees, + SSG_treeGrowParam growParam); + +//SG_APISHARED_EXPORT void sg_getTreeROI(SSG_featureTree* a_tree); + +//ending +SG_APISHARED_EXPORT void sg_getEndingGrowingTrees( + std::vector& lineEndings, + SVzNL3DLaserLine* laser3DPoints, + bool isVScan, + int featureType, + std::vector& trees, + SSG_treeGrowParam growParam); + +//endingֱɨʱֻˮƽˮƽɨʱֻдֱ +SG_APISHARED_EXPORT void sg_getEndingGrowingTrees_angleCheck( + std::vector& lineEndings, + SVzNL3DLaserLine* laser3DPoints, + bool isVScan, + int featureType, + std::vector& trees, + SSG_treeGrowParam growParam, + double angleCheckScale); + +//ɨϵ +SG_APISHARED_EXPORT void sg_LVFeatureGrowing( + std::vector& lineFeatures, + std::vector& trees, + SSG_bagParam bagParam, + SSG_treeGrowParam growParam, + std::vector& edgePts_0, + std::vector& edgePts_1); + +SG_APISHARED_EXPORT void sg_peakFeatureGrowing( + std::vector>& lineFeatures, + std::vector& trees, + SSG_treeGrowParam growParam); + +//semiCircle +SG_APISHARED_EXPORT void sg_getFeatureGrowingTrees_semiCircle( + std::vector< SSG_featureSemiCircle>& lineFeatures, + const int lineIdx, + const int lineSize, + std::vector& trees, + std::vector& stopTrees, + std::vector& invalidTrees, //ƳЩܽĿֳɶӶƳ + SSG_treeGrowParam growParam); + +SSG_meanVar _computeMeanVar(double* data, int size); + +///ֲߵ㣨zС㣩 +///ÿβڳȵһ롣Ծֲߵбǣֹظ¼ +SG_APISHARED_EXPORT void sg_getLocalPeaks( + SVzNL3DLaserLine* scanLines, + int lineNum, + std::vector& peaks, + SSG_localPkParam searchWin); + +/// +/// ԾֲߵΪӽ +/// һͬӵΪԲԲɨ裬¼ɨ赽ı߽ +/// +//SG_APISHARED_EXPORT void sg_peakPolarScan(cv::Mat& edgeMask, SVzNL2DPoint a_peak, SSG_polarScanParam polarScanParam, std::vector< SSG_2DValueI>& rgnContour); +//Peakˮƽֱɨõ߽ +SG_APISHARED_EXPORT void sg_peakXYScan( + SVzNL3DLaserLine* laser3DPoints, + int lineNum, + cv::Mat& featureEdgeMask, + SSG_2DValueI a_peak, + SSG_treeGrowParam growParam, + SSG_bagParam bagParam, + bool rgnPtAsEdge, + std::vector< SSG_lineConotours>& topContour, + std::vector< SSG_lineConotours>& bottomContour, + std::vector< SSG_lineConotours>& leftContour, + std::vector< SSG_lineConotours>& rightContour, + int* maxEdgeId_top, + int* maxEdgeId_btm, + int* maxEdgeId_left, + int* maxEdgeId_right); + +//ȡedgeIdͬı߽ +SG_APISHARED_EXPORT void sg_getContourPts( + std::vector< SSG_lineConotours>& contour_all, + int vldEdgeId, + std::vector< SSG_2DValueI>& contourFilter, + int* lowLevelFlag); + +//ӱ߽ȡedgeIdͬı߽ +SG_APISHARED_EXPORT void sg_getPairingContourPts( + std::vector& contourPairs, + std::vector& idPairs, + std::vector< SSG_conotourPair>& contourFilter, + SVzNLRangeD range, + bool isTBDir, + int* lowLevelFlag_0, + int* lowLevelFlag_1); + +//ȡ̾ı߽ +SG_APISHARED_EXPORT void sg_contourPostProc( + std::vector< SSG_contourPtInfo>& contour, + int maxEdgeIdx, + double sameConturDistTh, + std::vector< SSG_2DValueI>& contourFilter, + int sideID, + int* blockFlag); + +//任 +//input, outputΪfloat +SG_APISHARED_EXPORT void sg_distanceTrans(const cv::Mat input, cv::Mat& output, int distType); + +/// +/// 5x5ʽѰlocalPeaks +/// +/// +/// +SG_APISHARED_EXPORT void sg_getLocalPeaks_distTransform( + cv::Mat& input, + std::vector& peaks, + SSG_localPkParam searchWin); + +/// +/// ʹģ巨ȡֱ +/// ˮƽֱյdeltaZһСķΧڣյҲdeltaYһСķΧ +/// +SG_APISHARED_EXPORT void sg_getLineRigthAngleFeature( + SVzNL3DPosition* lineData, + int dataSize, + int lineIdx, + const SSG_lineRightAngleParam templatePara_HF, + const SSG_lineRightAngleParam templatePara_FH, + const SSG_lineRightAngleParam templatePara_HR, + const SSG_lineRightAngleParam templatePara_RH, + SSG_lineFeature* line_features); + +//ɨROI +SG_APISHARED_EXPORT SVzNL3DRangeD sg_getScanDataROI( + SVzNL3DLaserLine* laser3DPoints, + int lineNum); +//ɨROI: vecotrʽ +SG_APISHARED_EXPORT SVzNL3DRangeD sg_getScanDataROI_vector( + std::vector< std::vector>& scanLines +); + +//ROI: vecotrʽ +SG_APISHARED_EXPORT SVzNL3DRangeD wd_getPointCloudROI( + std::vector& scanData); + +//ƵROIscale: vecotrʽ +SG_APISHARED_EXPORT SWD_pointCloudPara wd_getPointCloudPara( + std::vector< std::vector>& scanLines); + +//XYƽֱ +SG_APISHARED_EXPORT void lineFitting( + std::vector< SVzNL3DPoint>& inliers, + double* _k, + double* _b); + +//ϳֱͨ߷ֱ̣ +SG_APISHARED_EXPORT void lineFitting_abc( + std::vector< SVzNL3DPoint>& inliers, + double* _a, + double* _b, + double* _c); + +/** + * @brief ռֱС + * @param points ά㼯2㣬壩 + * @param center ֱߵģ׼P0 + * @param direction ֱߵķvλ + * @return Ƿɹ㼯Чtrue + */ +SG_APISHARED_EXPORT bool fitLine3DLeastSquares( + const std::vector& points, + SVzNL3DPoint& center, + SVzNL3DPoint& direction); + +//ԲС +SG_APISHARED_EXPORT double fitCircleByLeastSquare( + const std::vector& pointArray, + SVzNL3DPoint& center, + double& radius); + +//С y=ax^2 + bx + c +SG_APISHARED_EXPORT bool leastSquareParabolaFitEigen( + const std::vector& points, + double& a, double& b, double& c, + double& mse, double& max_err); + +//Zֵ +SG_APISHARED_EXPORT double computeMeanZ(std::vector< SVzNL3DPoint>& pts); + +//ǶȲֵ0-180ȷΧ +SG_APISHARED_EXPORT double computeAngleDiff(double theta1, double theta2); + +//ֱ߽ +SG_APISHARED_EXPORT SVzNL3DPoint computeLineCrossPt_abs( + double a1, double b1, double c1, + double a2, double b2, double c2); + +//2ֱ߷ +SG_APISHARED_EXPORT void compute2ptLine( + SVzNL3DPoint pt1, + SVzNL3DPoint pt2, + double* _a, double* _b, double* _c); + +//2ֱ߷,ʹýṹ +SG_APISHARED_EXPORT void compute2ptLine_2( + double x1, double y1, + double x2, double y2, + double* _a, double* _b, double* _c); + +//ת45Ⱥֱ߷ +SG_APISHARED_EXPORT void rotateLine45Deg( + double _a, double _b, double _c, + double x0, double y0, + double* r_a, double* r_b, double* r_c); + +//ֱ߽Ƕ +SG_APISHARED_EXPORT double getLineAngle(const double _a, const double _b, const double _c); + +//2D +SG_APISHARED_EXPORT double compute2DLen(SVzNL3DPoint pt1, SVzNL3DPoint pt2); + +//XYƽζ(p0Ž) +SG_APISHARED_EXPORT double computeXOYVertexAngle(SVzNL3DPoint p0, SVzNL3DPoint p1, SVzNL3DPoint p2); + +//㵽ֱ߾ +SG_APISHARED_EXPORT double computePtDistToLine(double x0, double y0, double a, double b, double c); + +//㴹㣬ֱ߷̣y = kx + b +SG_APISHARED_EXPORT SVzNL2DPointD sx_getFootPoint( + double x0, + double y0, + double k, + double b); + +//㴹㣬ֱ߷̣ax+by+c = 0 +SG_APISHARED_EXPORT SVzNL2DPointD sx_getFootPoint_abc( + double x0, + double y0, + double A, + double B, + double C); + +//Bresenham㷨 +SG_APISHARED_EXPORT void drawLine( + int x0, + int y0, + int x1, + int y1, + std::vector& pts); + +/// +/// ע +/// +/// ĿΪ1 հ׵Ϊ0 +/// עÿΪrgnID, ID2ʼ +/// +SG_APISHARED_EXPORT void SG_TwoPassLabel( + const cv::Mat& bwImg, + cv::Mat& labImg, + std::vector& labelRgns, + int connectivity = 4); + +//: z = Ax + By + C +//res: [0]=A, [1]= B, [2]=-1.0, [3]=C, +SG_APISHARED_EXPORT void vzCaculateLaserPlane( + std::vector Points3ds, + std::vector& res); + +//һƽƽ +//пһƽͲοƽƽ棬ߵƽеƽ +//תΪƽƽ淨ΪֱIJ +SG_APISHARED_EXPORT SSG_planeCalibPara sg_getPlaneCalibPara( + SVzNL3DLaserLine* laser3DPoints, + int lineNum); + +SG_APISHARED_EXPORT SSG_planeCalibPara sg_HCameraVScan_getGroundCalibPara( + std::vector< std::vector>& scanLines); + +//һƽΪˮƽƽ(zΪ̶ֵ +SG_APISHARED_EXPORT SSG_planeCalibPara adjustPlaneToXYPlane( + double plane_A, double plane_B, double plane_C //ƽ淨 +); + +//һƽƽ +//пһƽͲοƽƽ棬ߵƽеƽ +//תΪƽƽ淨ΪֱIJ +SG_APISHARED_EXPORT SSG_planeCalibPara sg_getPlaneCalibPara2( + std::vector< std::vector>& scanLines); + +//Կװһƽƽȡ׵ɨ߽ƽ +SG_APISHARED_EXPORT SSG_planeCalibPara sg_getHolePlaneCalibPara( + std::vector< std::vector>& scanLines); + +//һƽƽ +//ROIڵĵƽϣƽ +//תΪƽƽ淨ΪֱIJ +SG_APISHARED_EXPORT SSG_planeCalibPara sg_getPlaneCalibPara_ROIs( + SVzNL3DLaserLine* laser3DPoints, + int lineNum, + std::vector& ROIs); + +//һƽƽ +//ROIڵĵƽϣƽ +//תΪƽƽ淨ΪֱIJ +SG_APISHARED_EXPORT SSG_planeCalibPara sg_getPlaneCalibPara2_ROI( + std::vector< std::vector>& scanLines, + SVzNL3DRangeD roi); + +//ת +SG_APISHARED_EXPORT SSG_planeCalibPara wd_computeRTMatrix( + SVzNL3DPoint& vector1, + SVzNL3DPoint& vector2); + +// תŷǣZ-Y-X˳ +SG_APISHARED_EXPORT SSG_EulerAngles rotationMatrixToEulerZYX(const double R[3][3]); +// ŷǼתZ-Y-X˳ +SG_APISHARED_EXPORT void eulerToRotationMatrixZYX(const SSG_EulerAngles& angles, double R[3][3]); + +//̬ƽȥ +///camPoseRΪ3x3 +SG_APISHARED_EXPORT void lineDataRT( + SVzNL3DLaserLine* a_line, + const double* camPoseR, + double groundH); +SG_APISHARED_EXPORT void lineDataRT_vector( + std::vector< SVzNL3DPosition>& a_line, + const double* camPoseR, + double groundH); +SG_APISHARED_EXPORT void HCamera_lineDataRT_vector( + std::vector< SVzNL3DPosition>& a_line, + const double* camPoseR, + double groundH); +SG_APISHARED_EXPORT void lineDataRT_RGBD( + SVzNLXYZRGBDLaserLine* a_line, + const double* camPoseR, + double groundH); + +//ڵľ +SG_APISHARED_EXPORT void sg_pointClustering( + std::vector< SVzNL3DPosition>& pts, + double clusterDist, + std::vector>& objClusters //result +); + +//դϵĴڵڵľ࣬3D +//ʹvector2άṹ +SG_APISHARED_EXPORT void wd_gridPointClustering( + std::vector>& featureMask, + std::vector>& feature3DInfo, + int clusterCheckWin, // + SSG_treeGrowParam growParam,// + int clusterID, //ǰClusterID + std::vector< SVzNL2DPoint>& a_cluster, //result + SVzNL3DRangeD& clusterRoi //roi3D +); + +//ɨݵľ +SG_APISHARED_EXPORT void wd_gridPointClustering_2( + std::vector>& gridData, + std::vector>& pointMaskInfo, //ֹʱظѡ + int clusterCheckWin, // + SSG_treeGrowParam growParam,// + int clusterID, //ǰClusterID + std::vector< SVzNL2DPoint>& a_cluster, //result + SVzNL3DRangeD& clusterRoi //roi3D +); + +//ʹþ෽8ͨͨ +SG_APISHARED_EXPORT void wd_gridPointClustering_labelling( + std::vector>& featureMask, + std::vector>& feature3DInfo, + int clusterID, //ǰClusterID + std::vector< SVzNL2DPoint>& a_cluster, //result + SVzNLRect& clusterRoi //roi2D +); + +//դݽXYƽϵͶӰĿհ׵вֵ +SG_APISHARED_EXPORT void pointClout2DProjection( + std::vector< std::vector>& gridScanData, + SVzNLRangeD x_range, + SVzNLRangeD y_range, + double scale, + double cuttingGrndZ, + int edgeSkip, + double inerPolateDistTh, //ֵֵڴֵIJֵ + cv::Mat& projectionData,//ͶӰݣʼΪһֵ1e+6 + cv::Mat& backIndexing //ڻ3D +); + +//դݽXYƽϵͶӰZֵĿհ׵вֵ +SG_APISHARED_EXPORT void pointClout2DQuantization( + std::vector< std::vector>& gridScanData, + SVzNLRangeD x_range, + SVzNLRangeD y_range, + double scale, + double cuttingGrndZ, + int edgeSkip, + double inerPolateDistTh, //ֵֵڴֵIJֵ + std::vector> quantiData, //ݣʼΪһֵ1e+6 + std::vector> backIndexing //ڻ3D +); + +//ˮ㷨 +SG_APISHARED_EXPORT void watershed(SWD_waterShedImage& img); +// ӵзˮ㷨 +SG_APISHARED_EXPORT void wd_seedWatershed( + SWD_waterShedImage& img, + std::vector& watershedSeeds, //ӵ + int maxLevel, //ˮλ + int startMakerID //ʼMarkerID +); + +//˲Ԥ +SG_APISHARED_EXPORT void wd_noiseFilter( + std::vector< std::vector>& scanLines, + const SSG_outlierFilterParam filterParam, + int* errCode); + +// Eigenʵ֣pts1pts2תƽƾ +//Ҫ +SG_APISHARED_EXPORT void caculateRT( + const std::vector& pts1, + const std::vector& pts2, + cv::Mat& R, cv::Mat& T, + cv::Point3d& C1, cv::Point3d& C2); + +//תƽƺλ +SG_APISHARED_EXPORT void pointRT(const cv::Mat& R, const cv::Mat& T, + const cv::Point3d& originPt, const cv::Point3d& rtOriginPT, //RT(תƽƣǰ + const cv::Point3d& pt, cv::Point3d& rtPt); //RTǰĵ + +//תƽƺλ +SG_APISHARED_EXPORT void pointRT_2(const cv::Mat& R, const cv::Mat& T, + const cv::Point3d& pt, cv::Point3d& rtPt); //RTǰĵ + +SG_APISHARED_EXPORT void pointRotate(const cv::Mat& R, + const cv::Point3d& pt, cv::Point3d& rtPt); //Rotateǰĵ + +//ŷǼת +SG_APISHARED_EXPORT void WD_EulerRpyToRotation(double rpy[3], double matrix3d[9]); + +//ŷǼ㷽ʸ +SG_APISHARED_EXPORT void WD_EulerRpyToDirVectors(double rpy[3], std::vector& dirVectors); diff --git a/AppAlgo/workpieceHolePositioning/Windows/x64/Debug/baseAlgorithm.dll b/AppAlgo/workpieceHolePositioning/Windows/x64/Debug/baseAlgorithm.dll index 06478396..6dc95dcb 100644 Binary files a/AppAlgo/workpieceHolePositioning/Windows/x64/Debug/baseAlgorithm.dll and b/AppAlgo/workpieceHolePositioning/Windows/x64/Debug/baseAlgorithm.dll differ diff --git a/AppAlgo/workpieceHolePositioning/Windows/x64/Debug/baseAlgorithm.lib b/AppAlgo/workpieceHolePositioning/Windows/x64/Debug/baseAlgorithm.lib index a48f72bd..0149c7d7 100644 Binary files a/AppAlgo/workpieceHolePositioning/Windows/x64/Debug/baseAlgorithm.lib and b/AppAlgo/workpieceHolePositioning/Windows/x64/Debug/baseAlgorithm.lib differ diff --git a/AppAlgo/workpieceHolePositioning/Windows/x64/Debug/baseAlgorithm.pdb b/AppAlgo/workpieceHolePositioning/Windows/x64/Debug/baseAlgorithm.pdb index 3d4765b1..1ead6b9c 100644 Binary files a/AppAlgo/workpieceHolePositioning/Windows/x64/Debug/baseAlgorithm.pdb and b/AppAlgo/workpieceHolePositioning/Windows/x64/Debug/baseAlgorithm.pdb differ diff --git a/AppAlgo/workpieceHolePositioning/Windows/x64/Debug/workpieceHolePositioning.dll b/AppAlgo/workpieceHolePositioning/Windows/x64/Debug/workpieceHolePositioning.dll index 8c6f8cee..911d5806 100644 Binary files a/AppAlgo/workpieceHolePositioning/Windows/x64/Debug/workpieceHolePositioning.dll and b/AppAlgo/workpieceHolePositioning/Windows/x64/Debug/workpieceHolePositioning.dll differ diff --git a/AppAlgo/workpieceHolePositioning/Windows/x64/Debug/workpieceHolePositioning.pdb b/AppAlgo/workpieceHolePositioning/Windows/x64/Debug/workpieceHolePositioning.pdb index 45c47cb2..0694f509 100644 Binary files a/AppAlgo/workpieceHolePositioning/Windows/x64/Debug/workpieceHolePositioning.pdb and b/AppAlgo/workpieceHolePositioning/Windows/x64/Debug/workpieceHolePositioning.pdb differ diff --git a/AppAlgo/workpieceHolePositioning/Windows/x64/Release/baseAlgorithm.dll b/AppAlgo/workpieceHolePositioning/Windows/x64/Release/baseAlgorithm.dll index 67231993..eb0cd44c 100644 Binary files a/AppAlgo/workpieceHolePositioning/Windows/x64/Release/baseAlgorithm.dll and b/AppAlgo/workpieceHolePositioning/Windows/x64/Release/baseAlgorithm.dll differ diff --git a/AppAlgo/workpieceHolePositioning/Windows/x64/Release/baseAlgorithm.lib b/AppAlgo/workpieceHolePositioning/Windows/x64/Release/baseAlgorithm.lib index f26642ed..2b5745a1 100644 Binary files a/AppAlgo/workpieceHolePositioning/Windows/x64/Release/baseAlgorithm.lib and b/AppAlgo/workpieceHolePositioning/Windows/x64/Release/baseAlgorithm.lib differ diff --git a/AppAlgo/workpieceHolePositioning/Windows/x64/Release/baseAlgorithm.pdb b/AppAlgo/workpieceHolePositioning/Windows/x64/Release/baseAlgorithm.pdb index 099c832e..4ddaf57e 100644 Binary files a/AppAlgo/workpieceHolePositioning/Windows/x64/Release/baseAlgorithm.pdb and b/AppAlgo/workpieceHolePositioning/Windows/x64/Release/baseAlgorithm.pdb differ diff --git a/AppAlgo/workpieceHolePositioning/Windows/x64/Release/workpieceHolePositioning.dll b/AppAlgo/workpieceHolePositioning/Windows/x64/Release/workpieceHolePositioning.dll index df5eb94a..2ed1c3e7 100644 Binary files a/AppAlgo/workpieceHolePositioning/Windows/x64/Release/workpieceHolePositioning.dll and b/AppAlgo/workpieceHolePositioning/Windows/x64/Release/workpieceHolePositioning.dll differ diff --git a/AppAlgo/workpieceHolePositioning/Windows/x64/Release/workpieceHolePositioning.pdb b/AppAlgo/workpieceHolePositioning/Windows/x64/Release/workpieceHolePositioning.pdb index f18c28cf..000ad049 100644 Binary files a/AppAlgo/workpieceHolePositioning/Windows/x64/Release/workpieceHolePositioning.pdb and b/AppAlgo/workpieceHolePositioning/Windows/x64/Release/workpieceHolePositioning.pdb differ diff --git a/AppAlgo/workpieceHolePositioning/workpieceHolePositioning_test.cpp b/AppAlgo/workpieceHolePositioning/workpieceHolePositioning_test.cpp index a1c4e848..8b503d4d 100644 --- a/AppAlgo/workpieceHolePositioning/workpieceHolePositioning_test.cpp +++ b/AppAlgo/workpieceHolePositioning/workpieceHolePositioning_test.cpp @@ -12,6 +12,7 @@ #include #include #include +#include "SG_baseAlgo_Export.h" typedef struct { @@ -153,6 +154,12 @@ void _outputWorkpieceInfo(char* fileName, std::vector< WD_workpieceInfo>& workpi } sprintf_s(dataStr, 50, " center: (%g, %g, %g)", workpiecePositioning[i].center.x, workpiecePositioning[i].center.y, workpiecePositioning[i].center.z); sw << dataStr << std::endl; + sprintf_s(dataStr, 50, " x_dir: (%g, %g, %g)", workpiecePositioning[i].x_dir.x, workpiecePositioning[i].x_dir.y, workpiecePositioning[i].x_dir.z); + sw << dataStr << std::endl; + sprintf_s(dataStr, 50, " y_dir: (%g, %g, %g)", workpiecePositioning[i].y_dir.x, workpiecePositioning[i].y_dir.y, workpiecePositioning[i].y_dir.z); + sw << dataStr << std::endl; + sprintf_s(dataStr, 50, " z_dir: (%g, %g, %g)", workpiecePositioning[i].z_dir.x, workpiecePositioning[i].z_dir.y, workpiecePositioning[i].z_dir.z); + sw << dataStr << std::endl; } sw.close(); } @@ -368,20 +375,27 @@ void _outputRGBDResult_RGBD( size = 2; for (int i = 0; i < objNumber; i++) { + SVzNL3DPoint dirPt; + dirPt = { workpiecePositioning[i].center.x + workpiecePositioning[i].y_dir.x * 10, + workpiecePositioning[i].center.y + workpiecePositioning[i].y_dir.y * 10, + workpiecePositioning[i].center.z + workpiecePositioning[i].y_dir.z * 10 }; sw << "Poly_" << lineIdx << "_2" << std::endl; sw << "{" << workpiecePositioning[i].center.x << "," << workpiecePositioning[i].center.y << "," << workpiecePositioning[i].center.z << "}-"; sw << "{0,0}-{0,0}-"; sw << "{" << (int)rgb.r << "," << (int)rgb.g << "," << (int)rgb.b << "," << size << "}" << std::endl; - sw << "{" << workpiecePositioning[i].y_dir.x << "," << workpiecePositioning[i].y_dir.y << "," << workpiecePositioning[i].y_dir.z << "}-"; + sw << "{" << dirPt.x << "," << dirPt.y << "," << dirPt.z << "}-"; sw << "{0,0}-{0,0}-"; sw << "{" << (int)rgb.r << "," << (int)rgb.g << "," << (int)rgb.b << "," << size << "}" << std::endl; lineIdx++; + dirPt = { workpiecePositioning[i].center.x + workpiecePositioning[i].z_dir.x * 10, + workpiecePositioning[i].center.y + workpiecePositioning[i].z_dir.y * 10, + workpiecePositioning[i].center.z + workpiecePositioning[i].z_dir.z * 10 }; sw << "Poly_" << lineIdx << "_2" << std::endl; sw << "{" << workpiecePositioning[i].center.x << "," << workpiecePositioning[i].center.y << "," << workpiecePositioning[i].center.z << "}-"; sw << "{0,0}-{0,0}-"; sw << "{" << (int)rgb.r << "," << (int)rgb.g << "," << (int)rgb.b << "," << size << "}" << std::endl; - sw << "{" << workpiecePositioning[i].z_dir.x << "," << workpiecePositioning[i].z_dir.y << "," << workpiecePositioning[i].z_dir.z << "}-"; + sw << "{" << dirPt.x << "," << dirPt.y << "," << dirPt.z << "}-"; sw << "{0,0}-{0,0}-"; sw << "{" << (int)rgb.r << "," << (int)rgb.g << "," << (int)rgb.b << "," << size << "}" << std::endl; lineIdx++; @@ -398,15 +412,17 @@ void _outputRGBDResult_RGBD( lineIdx++; } } + SVzNL3DPoint dirPt = { workpiecePositioning[0].center.x + workpiecePositioning[0].y_dir.x * 10, + workpiecePositioning[0].center.y + workpiecePositioning[0].y_dir.y * 10, + workpiecePositioning[0].center.z + workpiecePositioning[0].y_dir.z * 10 }; sw << "Poly_" << lineIdx << "_2" << std::endl; sw << "{" << workpiecePositioning[0].center.x << "," << workpiecePositioning[0].center.y << "," << workpiecePositioning[0].center.z << "}-"; sw << "{0,0}-{0,0}-"; sw << "{" << (int)rgb.r << "," << (int)rgb.g << "," << (int)rgb.b << "," << size << "}" << std::endl; - sw << "{" << workpiecePositioning[0].y_dir.x << "," << workpiecePositioning[0].y_dir.y << "," << workpiecePositioning[0].y_dir.z << "}-"; + sw << "{" << dirPt.x << "," << dirPt.y << "," << dirPt.z << "}-"; sw << "{0,0}-{0,0}-"; sw << "{" << (int)rgb.r << "," << (int)rgb.g << "," << (int)rgb.b << "," << size << "}" << std::endl; - sw.close(); } @@ -423,7 +439,8 @@ SVzNL3DPoint _pointRT(SVzNL3DPoint& origin, const double* R, const double* T) } #define TEST_COMPUTE_CALIB_PARA 0 -#define TEST_COMPUTE_HOLE 1 +#define TEST_COMPUTE_HOLE 0 +#define TEST_COMPUTE_RT 1 #define TEST_GROUP 1 int main() { @@ -433,15 +450,107 @@ int main() }; SVzNLRange fileIdx[TEST_GROUP] = { - {1,4}, + {6,6}, }; const char* ver = wd_workpieceHolePositioningVersion(); printf("ver:%s\n", ver); +#if TEST_COMPUTE_RT + std::vector pts_eye; + pts_eye.resize(6); + std::vector pts_robot; + pts_robot.resize(6); + pts_eye[0] = { 187.22, -99.43, 1797.70 }; //, R = 0.6413, P = 0.0302, Y = -91.1494 + pts_eye[1] = { 186.50, -140.52, 1797.69 }; // R = 0.6413, P = 0.0302, Y = -88.8130 + pts_eye[2] = { 256.73, -93.55, 1797.83 }; //, R = 0.6413, P = 0.0302, Y = -89.3432 + pts_eye[3] = { 236.69, -48.82, 1797.77 }; // R = 0.6413, P = 0.0302, Y = -89.5285 + pts_eye[4] = { 173.62, 13.92, 1797.30 };// R = 0.6413, P = 0.0302, Y = -89.6450 + pts_eye[5] = { 232.11, -153.38, 1797.88 };// R = 0.6413, P = 0.0302, Y = -82.0278 + + //A:绕z, B：绕y， C:绕x + pts_robot[0] = { 55.830, -984.472, 97.687 }; // A:0.018 B : -0.638 C : -0.292 + pts_robot[1] = { 95.469, -985.345, 97.782 }; // A:-1.032 B : -0.354 C : 1.001 + pts_robot[2] = { 47.908, -1053.709, 97.802 }; // A:-1.032 B : -0.356 C : 0.998 + pts_robot[3] = { 3.646, -1033.152, 96.494 }; // A:-1.028 B : -0.367 C : 0.442 + pts_robot[4] = { -58.843, -969.793, 95.297 }; // A:-1.029 B : -0.367 C : 0.442 + pts_robot[5] = { 108.031, -1029.803, 98.333 }; // A:-9.208 B : -0.367 C : 0.442 + + //使用前6组数据 + std::vector test_pts_eye; + std::vector test_pts_robot; + for (int i = 0; i < 5; i++) + { + test_pts_eye.push_back(pts_eye[i]); + test_pts_robot.push_back(pts_robot[i]); + } + //对空间两组对应点计算旋转平移矩阵 + // Eigen库实现 + cv::Mat R, T; + cv::Point3d C_eye, C_robot; + caculateRT( + test_pts_eye, + test_pts_robot, + R, T, + C_eye, C_robot); + + std::cout << "方向向量转换结果：" << std::endl; + std::cout << std::fixed << std::setprecision(6); // 固定小数位数为6 + std::cout << R << std::endl; + std::cout << T << std::endl; + + //验算6轴姿态 + std::vector verify_pts_eye; + verify_pts_eye.insert(verify_pts_eye.end(), pts_eye.begin(), pts_eye.end()); + cv::Point3d a_center = { 232.997, -173.533, 1795.9 }; + verify_pts_eye.push_back(a_center); + std::vector> pose_eye; + pose_eye.resize(7); + for (int i = 0; i < 7; i++) + pose_eye[i].resize(3); + pose_eye[0][0] = { -0.020, -1.000, -0.011 }; pose_eye[0][1] = { 1.000, -0.020, -0.001 }; pose_eye[0][2] = { 0.001, -0.011, 1.000 }; + pose_eye[1][0] = { 0.021,-1.000,-0.011 }; pose_eye[1][1] = { 1.000,0.021,-0.000 }; pose_eye[1][2] = { 0.001,-0.011,1.000 }; + pose_eye[2][0] = { 0.011,-1.000,-0.011 }; pose_eye[2][1] = { 1.000,0.011,-0.000 }; pose_eye[2][2] = { 0.001,-0.011,1.000 }; + pose_eye[3][0] = { 0.008,-1.000,-0.011 }; pose_eye[3][1] = { 1.000,0.008,-0.000 }; pose_eye[3][2] = { 0.001,-0.011,1.000 }; + pose_eye[4][0] = { 0.006,-1.000,-0.011 }; pose_eye[4][1] = { 1.000,0.006,-0.000 }; pose_eye[4][2] = { 0.001,-0.011,1.000 }; + pose_eye[5][0] = { 0.139,-0.990,-0.011 }; pose_eye[5][1] = { 0.990,0.139,0.001 }; pose_eye[5][2] = { 0.001,-0.011,1.000 }; + pose_eye[6][0] = { 0.136746, -0.990563, -0.00926168 }; pose_eye[6][1] = { 0.990606, 0.136747, 0.000517805 }; pose_eye[6][2] = { 0.000753588, -0.00924548, 0.999957 }; + for (int i = 0; i < 7; i++) + { + cv::Point3d rtPt; + pointRT_2(R, T, verify_pts_eye[i], rtPt); //RT前后的点 + + std::vector dirVectors_eye = pose_eye[i]; + //dirVectors_eye[0] = { -dirVectors_eye[0].x, -dirVectors_eye[0].y, -dirVectors_eye[0].z }; + dirVectors_eye[1] = { -dirVectors_eye[1].x, -dirVectors_eye[1].y, -dirVectors_eye[1].z }; + dirVectors_eye[2] = { -dirVectors_eye[2].x, -dirVectors_eye[2].y, -dirVectors_eye[2].z }; + std::vector dirVectors_robot; + for (int j = 0; j < 3; j++) + { + cv::Point3d rt_pt; + pointRotate(R, dirVectors_eye[j], rt_pt); + dirVectors_robot.push_back(rt_pt); + } + //生成旋转矩阵 + double R_pose[3][3]; + R_pose[0][0] = dirVectors_robot[0].x; + R_pose[0][1] = dirVectors_robot[1].x; + R_pose[0][2] = dirVectors_robot[2].x; + R_pose[1][0] = dirVectors_robot[0].y; + R_pose[1][1] = dirVectors_robot[1].y; + R_pose[1][2] = dirVectors_robot[2].y; + R_pose[2][0] = dirVectors_robot[0].z; + R_pose[2][1] = dirVectors_robot[1].z; + R_pose[2][2] = dirVectors_robot[2].z; + SSG_EulerAngles test_rpy = rotationMatrixToEulerZYX(R_pose); + std::cout << i << ":" << std::endl; + std::cout << rtPt.x << "," << rtPt.y << "," << rtPt.z << std::endl; + std::cout << test_rpy.roll << "," << test_rpy.pitch << "," << test_rpy.yaw << std::endl; + } +#endif #if TEST_COMPUTE_CALIB_PARA char _calib_datafile[256]; - sprintf_s(_calib_datafile, "%sLaserData_ground.txt", dataPath[0]); + sprintf_s(_calib_datafile, "%sLaserData_ground2.txt", dataPath[0]); int lineNum = 0; float lineV = 0.0f; int dataCalib = 0; @@ -471,7 +580,7 @@ int main() sprintf_s(_out_file, "%sscanData_ground_calib_verify.txt", dataPath[0]); int headNullLines = 0; _outputScanDataFile_vector(_out_file, scanData, false, &headNullLines); - +#if 0 for (int fidx = fileIdx[0].nMin; fidx <= fileIdx[0].nMax; fidx++) { //fidx =4; @@ -491,6 +600,7 @@ int main() int headNullLines = 0; _outputScanDataFile_vector(_scan_file, scanLines, false, &headNullLines); } +#endif printf("%s: calib done!\n", _calib_datafile); } #endif