工件定位基本调试通
This commit is contained in:
yiyi
parent
6f32e3b9af
commit
d0574aec23
@ -144,147 +144,3 @@
|
||||
| 读取失败 | 记录日志，下次轮询重试 |
|
||||
| 写入失败 | 记录日志，返回失败状态 |
|
||||
| 超时 | 默认1秒超时，触发重连 |
|
||||
|
||||
## 8. 配置文件
|
||||
|
||||
配置文件路径：`config/config.xml`
|
||||
|
||||
```xml
|
||||
<TcpServerConfig
|
||||
plcServerIp="192.168.0.88"
|
||||
plcServerPort="502"
|
||||
robotServerIp=""
|
||||
robotServerPort="502"/>
|
||||
```
|
||||
|
||||
> **注意**: 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<CoordinateData>& 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<PLCModbusClient::CoordinateData> 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个点 |
|
||||
|
||||
@ -31,6 +31,7 @@ public:
|
||||
LaserDataLoader& dataLoader,
|
||||
const double clibMatrix[16],
|
||||
int eulerOrder,
|
||||
int dirVectorInvert,
|
||||
WorkpieceHoleDetectionResult& detectionResult);
|
||||
|
||||
};
|
||||
|
||||
@ -108,6 +108,7 @@ private:
|
||||
// 连接管理
|
||||
bool checkConnection();
|
||||
bool tryConnectPLC();
|
||||
void disconnectPLC(); // 主动断开连接（用于触发重连）
|
||||
|
||||
// 寄存器操作
|
||||
int readPhotoRequest();
|
||||
|
||||
@ -1,9 +1,11 @@
|
||||
#include "DetectPresenter.h"
|
||||
#include "workpieceHolePositioning_Export.h"
|
||||
#include "SG_baseAlgo_Export.h"
|
||||
#include <fstream>
|
||||
#include <QPainter>
|
||||
#include <QPen>
|
||||
#include <QColor>
|
||||
#include <opencv2/opencv.hpp>
|
||||
#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<double>(i, j) = clibMatrix[i * 4 + j];
|
||||
}
|
||||
T.at<double>(i, 0) = clibMatrix[i * 4 + 3];
|
||||
}
|
||||
|
||||
// 构建用于可视化的点数组
|
||||
std::vector<std::vector<SVzNL3DPoint>> 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<CTEulerOrder>(eulerOrder));
|
||||
// 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);
|
||||
|
||||
// 构建相机坐标系下的完整位姿
|
||||
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<CTEulerOrder>(eulerOrder), robotPose);
|
||||
// 使用 pointRotate 对方向向量进行旋转变换
|
||||
std::vector<cv::Point3d> 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);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
@ -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<std::mutex> locker(m_mutex);
|
||||
|
||||
if (m_plcClient) {
|
||||
m_plcClient->Disconnect();
|
||||
LOG_INFO("PLC disconnected for reconnection\n");
|
||||
}
|
||||
}
|
||||
|
||||
int PLCModbusClient::readPhotoRequest()
|
||||
{
|
||||
std::lock_guard<std::mutex> locker(m_mutex);
|
||||
|
||||
@ -380,11 +380,15 @@ int WorkpieceHolePresenter::ProcessAlgoDetection(std::vector<std::pair<EVzResult
|
||||
int eulerOrder = configResult.handEyeCalibMatrix.eulerOrder;
|
||||
LOG_INFO("[Algo Thread] Using euler order: %d\n", eulerOrder);
|
||||
|
||||
// 获取方向向量反向配置
|
||||
int dirVectorInvert = configResult.plcRobotServerConfig.dirVectorInvert;
|
||||
LOG_INFO("[Algo Thread] Using dir vector invert: %d\n", dirVectorInvert);
|
||||
|
||||
WorkpieceHoleDetectionResult detectionResult;
|
||||
|
||||
int nRet = m_pDetectPresenter->DetectWorkpieceHole(m_currentCameraIndex, detectionDataCache,
|
||||
algorithmParams, debugParam, m_dataLoader,
|
||||
currentClibMatrix.clibMatrix, eulerOrder, detectionResult);
|
||||
currentClibMatrix.clibMatrix, eulerOrder, dirVectorInvert, detectionResult);
|
||||
// 根据项目类型选择处理方式
|
||||
if (GetStatusCallback<IYWorkpieceHoleStatus>()) {
|
||||
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<IYWorkpieceHoleStatus>()) {
|
||||
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<IYWorkpieceHoleStatus>()) {
|
||||
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<PLCModbusClient::CoordinateData> coords;
|
||||
int pointCount = std::min(static_cast<int>(result.positions.size()), PLCModbusClient::MAX_POINTS);
|
||||
@ -837,13 +856,50 @@ void WorkpieceHolePresenter::SendCoordinateDataToPLC(const WorkpieceHoleDetectio
|
||||
coord.x = static_cast<float>(pos.x);
|
||||
coord.y = static_cast<float>(pos.y);
|
||||
coord.z = static_cast<float>(pos.z);
|
||||
coord.roll = static_cast<float>(pos.roll);
|
||||
coord.pitch = static_cast<float>(pos.pitch);
|
||||
coord.yaw = static_cast<float>(pos.yaw);
|
||||
|
||||
// 根据姿态输出顺序配置调整输出
|
||||
switch (poseOutputOrder) {
|
||||
case POSE_ORDER_RPY: // Roll, Pitch, Yaw（默认）
|
||||
coord.roll = static_cast<float>(pos.roll);
|
||||
coord.pitch = static_cast<float>(pos.pitch);
|
||||
coord.yaw = static_cast<float>(pos.yaw);
|
||||
break;
|
||||
case POSE_ORDER_RYP: // Roll, Yaw, Pitch
|
||||
coord.roll = static_cast<float>(pos.roll);
|
||||
coord.pitch = static_cast<float>(pos.yaw);
|
||||
coord.yaw = static_cast<float>(pos.pitch);
|
||||
break;
|
||||
case POSE_ORDER_PRY: // Pitch, Roll, Yaw
|
||||
coord.roll = static_cast<float>(pos.pitch);
|
||||
coord.pitch = static_cast<float>(pos.roll);
|
||||
coord.yaw = static_cast<float>(pos.yaw);
|
||||
break;
|
||||
case POSE_ORDER_PYR: // Pitch, Yaw, Roll
|
||||
coord.roll = static_cast<float>(pos.pitch);
|
||||
coord.pitch = static_cast<float>(pos.yaw);
|
||||
coord.yaw = static_cast<float>(pos.roll);
|
||||
break;
|
||||
case POSE_ORDER_YRP: // Yaw, Roll, Pitch
|
||||
coord.roll = static_cast<float>(pos.yaw);
|
||||
coord.pitch = static_cast<float>(pos.roll);
|
||||
coord.yaw = static_cast<float>(pos.pitch);
|
||||
break;
|
||||
case POSE_ORDER_YPR: // Yaw, Pitch, Roll
|
||||
coord.roll = static_cast<float>(pos.yaw);
|
||||
coord.pitch = static_cast<float>(pos.pitch);
|
||||
coord.yaw = static_cast<float>(pos.roll);
|
||||
break;
|
||||
default: // 默认 RPY
|
||||
coord.roll = static_cast<float>(pos.roll);
|
||||
coord.pitch = static_cast<float>(pos.pitch);
|
||||
coord.yaw = static_cast<float>(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);
|
||||
|
||||
@ -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
|
||||
|
||||
// 构建日期
|
||||
|
||||
@ -1,4 +1,10 @@
|
||||
# 1.0.1
|
||||
## build_4
|
||||
1. 增加了协议姿态输出顺序
|
||||
|
||||
## build_3 2026-02-01
|
||||
1. 更新了算法，输出的姿态
|
||||
|
||||
## build_2 2026-02-01
|
||||
1. 协议测试修正
|
||||
|
||||
|
||||
@ -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
|
||||
|
||||
|
||||
@ -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;
|
||||
}
|
||||
|
||||
@ -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();
|
||||
|
||||
@ -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; }</string>
|
||||
</property>
|
||||
<property name="currentIndex">
|
||||
<number>0</number>
|
||||
<number>5</number>
|
||||
</property>
|
||||
<widget class="QWidget" name="tab_workpiece">
|
||||
<attribute name="title">
|
||||
@ -809,7 +809,7 @@ QComboBox QAbstractItemView {
|
||||
<x>10</x>
|
||||
<y>20</y>
|
||||
<width>671</width>
|
||||
<height>251</height>
|
||||
<height>291</height>
|
||||
</rect>
|
||||
</property>
|
||||
<property name="font">
|
||||
@ -826,7 +826,7 @@ QComboBox QAbstractItemView {
|
||||
<x>20</x>
|
||||
<y>40</y>
|
||||
<width>631</width>
|
||||
<height>200</height>
|
||||
<height>246</height>
|
||||
</rect>
|
||||
</property>
|
||||
<layout class="QGridLayout">
|
||||
@ -950,106 +950,120 @@ QComboBox QAbstractItemView {
|
||||
</property>
|
||||
</widget>
|
||||
</item>
|
||||
</layout>
|
||||
</widget>
|
||||
</widget>
|
||||
<widget class="QGroupBox" name="groupBox_robot">
|
||||
<property name="geometry">
|
||||
<rect>
|
||||
<x>10</x>
|
||||
<y>290</y>
|
||||
<width>671</width>
|
||||
<height>201</height>
|
||||
</rect>
|
||||
</property>
|
||||
<property name="font">
|
||||
<font>
|
||||
<pointsize>16</pointsize>
|
||||
</font>
|
||||
</property>
|
||||
<property name="title">
|
||||
<string>机械臂服务端配置</string>
|
||||
</property>
|
||||
<widget class="QWidget" name="layoutWidget_robot">
|
||||
<property name="geometry">
|
||||
<rect>
|
||||
<x>20</x>
|
||||
<y>40</y>
|
||||
<width>631</width>
|
||||
<height>148</height>
|
||||
</rect>
|
||||
</property>
|
||||
<layout class="QVBoxLayout">
|
||||
<item>
|
||||
<widget class="QLabel" name="label_robotServerIp">
|
||||
<item row="5" column="0">
|
||||
<widget class="QLabel" name="label_poseOutputOrder">
|
||||
<property name="font">
|
||||
<font>
|
||||
<pointsize>16</pointsize>
|
||||
<pointsize>14</pointsize>
|
||||
</font>
|
||||
</property>
|
||||
<property name="text">
|
||||
<string>机械臂服务端IP地址</string>
|
||||
<string>姿态输出顺序</string>
|
||||
</property>
|
||||
</widget>
|
||||
</item>
|
||||
<item>
|
||||
<widget class="QLineEdit" name="lineEdit_robotServerIp">
|
||||
<item row="5" column="1">
|
||||
<widget class="QComboBox" name="comboBox_poseOutputOrder">
|
||||
<property name="font">
|
||||
<font>
|
||||
<pointsize>16</pointsize>
|
||||
<pointsize>14</pointsize>
|
||||
</font>
|
||||
</property>
|
||||
<property name="placeholderText">
|
||||
<string>例如: 192.168.1.20</string>
|
||||
<property name="styleSheet">
|
||||
<string notr="true">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);
|
||||
}</string>
|
||||
</property>
|
||||
</widget>
|
||||
</item>
|
||||
<item>
|
||||
<widget class="QLabel" name="label_robotServerPort">
|
||||
<item row="6" column="0">
|
||||
<widget class="QLabel" name="label_dirVectorInvert">
|
||||
<property name="font">
|
||||
<font>
|
||||
<pointsize>16</pointsize>
|
||||
<pointsize>14</pointsize>
|
||||
</font>
|
||||
</property>
|
||||
<property name="text">
|
||||
<string>机械臂服务端端口</string>
|
||||
<string>方向向量反向</string>
|
||||
</property>
|
||||
</widget>
|
||||
</item>
|
||||
<item>
|
||||
<widget class="QLineEdit" name="lineEdit_robotServerPort">
|
||||
<item row="6" column="1">
|
||||
<widget class="QComboBox" name="comboBox_dirVectorInvert">
|
||||
<property name="font">
|
||||
<font>
|
||||
<pointsize>16</pointsize>
|
||||
<pointsize>14</pointsize>
|
||||
</font>
|
||||
</property>
|
||||
<property name="placeholderText">
|
||||
<string>例如: 502</string>
|
||||
<property name="styleSheet">
|
||||
<string notr="true">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);
|
||||
}</string>
|
||||
</property>
|
||||
</widget>
|
||||
</item>
|
||||
</layout>
|
||||
</widget>
|
||||
</widget>
|
||||
</widget>
|
||||
</widget>
|
||||
</widget>
|
||||
<widget class="QPushButton" name="btn_camer_ok">
|
||||
<widget class="QWidget" name="">
|
||||
<property name="geometry">
|
||||
<rect>
|
||||
<x>190</x>
|
||||
<x>2</x>
|
||||
<y>670</y>
|
||||
<width>100</width>
|
||||
<height>45</height>
|
||||
<width>751</width>
|
||||
<height>44</height>
|
||||
</rect>
|
||||
</property>
|
||||
<property name="font">
|
||||
<font>
|
||||
<pointsize>16</pointsize>
|
||||
<bold>true</bold>
|
||||
</font>
|
||||
</property>
|
||||
<property name="styleSheet">
|
||||
<string notr="true">QPushButton {
|
||||
<layout class="QHBoxLayout" name="horizontalLayout">
|
||||
<item>
|
||||
<spacer name="horizontalSpacer">
|
||||
<property name="orientation">
|
||||
<enum>Qt::Horizontal</enum>
|
||||
</property>
|
||||
<property name="sizeHint" stdset="0">
|
||||
<size>
|
||||
<width>178</width>
|
||||
<height>20</height>
|
||||
</size>
|
||||
</property>
|
||||
</spacer>
|
||||
</item>
|
||||
<item>
|
||||
<widget class="QPushButton" name="btn_camer_ok">
|
||||
<property name="font">
|
||||
<font>
|
||||
<pointsize>16</pointsize>
|
||||
<bold>true</bold>
|
||||
</font>
|
||||
</property>
|
||||
<property name="styleSheet">
|
||||
<string notr="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);
|
||||
}</string>
|
||||
</property>
|
||||
<property name="text">
|
||||
<string>保存</string>
|
||||
</property>
|
||||
</widget>
|
||||
<widget class="QPushButton" name="btn_camer_cancel">
|
||||
<property name="geometry">
|
||||
<rect>
|
||||
<x>410</x>
|
||||
<y>670</y>
|
||||
<width>100</width>
|
||||
<height>45</height>
|
||||
</rect>
|
||||
</property>
|
||||
<property name="font">
|
||||
<font>
|
||||
<pointsize>16</pointsize>
|
||||
<bold>true</bold>
|
||||
</font>
|
||||
</property>
|
||||
<property name="styleSheet">
|
||||
<string notr="true">QPushButton {
|
||||
</property>
|
||||
<property name="text">
|
||||
<string> 保存 </string>
|
||||
</property>
|
||||
</widget>
|
||||
</item>
|
||||
<item>
|
||||
<spacer name="horizontalSpacer_3">
|
||||
<property name="orientation">
|
||||
<enum>Qt::Horizontal</enum>
|
||||
</property>
|
||||
<property name="sizeHint" stdset="0">
|
||||
<size>
|
||||
<width>50</width>
|
||||
<height>20</height>
|
||||
</size>
|
||||
</property>
|
||||
</spacer>
|
||||
</item>
|
||||
<item>
|
||||
<widget class="QPushButton" name="btn_camer_cancel">
|
||||
<property name="font">
|
||||
<font>
|
||||
<pointsize>16</pointsize>
|
||||
<bold>true</bold>
|
||||
</font>
|
||||
</property>
|
||||
<property name="styleSheet">
|
||||
<string notr="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);
|
||||
}</string>
|
||||
</property>
|
||||
<property name="text">
|
||||
<string>取消</string>
|
||||
</property>
|
||||
</property>
|
||||
<property name="text">
|
||||
<string> 取消 </string>
|
||||
</property>
|
||||
</widget>
|
||||
</item>
|
||||
<item>
|
||||
<spacer name="horizontalSpacer_2">
|
||||
<property name="orientation">
|
||||
<enum>Qt::Horizontal</enum>
|
||||
</property>
|
||||
<property name="sizeHint" stdset="0">
|
||||
<size>
|
||||
<width>178</width>
|
||||
<height>20</height>
|
||||
</size>
|
||||
</property>
|
||||
</spacer>
|
||||
</item>
|
||||
</layout>
|
||||
</widget>
|
||||
</widget>
|
||||
<resources/>
|
||||
|
||||
@ -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) {
|
||||
}
|
||||
|
||||
// 默认构造函数
|
||||
|
||||
@ -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);
|
||||
|
||||
// 保存到文件
|
||||
|
||||
@ -24,5 +24,5 @@
|
||||
<DebugParam enableDebug="false" savePointCloud="false" saveDebugImage="false" printDetailLog="false" debugOutputPath="./debug"/>
|
||||
<SerialConfig portName="COM1" baudRate="115200" dataBits="8" stopBits="1" parity="0" flowControl="0" enabled="false"/>
|
||||
<HandEyeCalibMatrix m0="1" m1="0" m2="0" m3="0" m4="0" m5="1" m6="0" m7="0" m8="0" m9="0" m10="1" m11="0" m12="0" m13="0" m14="0" m15="1"/>
|
||||
<TcpServerConfig plcServerIp="192.168.0.88" plcServerPort="502" robotServerIp="192.168.0.90" robotServerPort="502" addrPhotoRequest="1001" addrDataComplete="1003" addrCoordDataStart="2002"/>
|
||||
<TcpServerConfig plcServerIp="192.168.0.88" plcServerPort="502" robotServerIp="192.168.0.90" robotServerPort="502" addrPhotoRequest="1001" addrDataComplete="1003" addrCoordDataStart="2002" poseOutputOrder="0" dirVectorInvert="3"/>
|
||||
</WorkpieceHoleConfig>
|
||||
|
||||
Binary file not shown.
Binary file not shown.
755
AppAlgo/workpieceHolePositioning/Inc/SG_baseAlgo_Export.h
Normal file
755
AppAlgo/workpieceHolePositioning/Inc/SG_baseAlgo_Export.h
Normal file
@ -0,0 +1,755 @@
|
||||
#pragma once
|
||||
|
||||
#include "SG_algo_Export.h"
|
||||
#include <vector>
|
||||
#include <opencv2/opencv.hpp>
|
||||
|
||||
|
||||
//滤除离群点:z跳变门限方法（大于门限视为不连续，根据连续段点数量判断噪声）
|
||||
SG_APISHARED_EXPORT void sg_lineDataRemoveOutlier(
|
||||
SVzNL3DPosition* lineData,
|
||||
int dataSize,
|
||||
SSG_outlierFilterParam filterParam,
|
||||
std::vector<SVzNL3DPosition>& filerData,
|
||||
std::vector<int>& 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<SVzNL3DPosition>& a_line,
|
||||
SSG_outlierFilterParam filterParam);
|
||||
//滤除离群点：点距离门限方法（大于门限视为不连续，根据连续段点数量判断噪声）
|
||||
SG_APISHARED_EXPORT void sg_lineDataRemoveOutlier_ptDistMethod(
|
||||
SVzNL3DPosition* lineData,
|
||||
int dataSize,
|
||||
SSG_outlierFilterParam filterParam,
|
||||
std::vector<SVzNL3DPosition>& filerData,
|
||||
std::vector<int>& noisePts);
|
||||
|
||||
//平滑
|
||||
SG_APISHARED_EXPORT void sg_lineDataSmoothing(
|
||||
std::vector<SVzNL3DPosition>& input,
|
||||
int smoothWin,
|
||||
std::vector<SVzNL3DPosition>& output);
|
||||
//分段平滑，这样不会影响分段端点
|
||||
SG_APISHARED_EXPORT void sg_lineSegSmoothing(
|
||||
std::vector<SVzNL3DPosition>& input,
|
||||
double seg_y_deltaTh, //分段的Y间隔。大于此间隔，为新的分段
|
||||
double seg_z_deltaTh,//分段的Z间隔。大于此间隔，为新的分段
|
||||
int smoothWin,
|
||||
std::vector<SVzNL3DPosition>& 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通常取bagH的1/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通常取bagH的1/4
|
||||
std::vector<SSG_basicFeature1D>& features);
|
||||
|
||||
//提取Gap特征。Gap特征：由两个负的Corner构成的区域
|
||||
SG_APISHARED_EXPORT void sg_getLineGapFeature(
|
||||
std::vector<SVzNL3DPosition>& lineData, //扫描线
|
||||
int lineIdx, //当前扫描线序号
|
||||
const SSG_cornerParam cornerPara,
|
||||
const SVzNLRangeD gapParam,
|
||||
std::vector<SSG_basicFeatureGap>& line_gaps);
|
||||
|
||||
SG_APISHARED_EXPORT void sg_getLineCornerFeature_BQ(
|
||||
SVzNL3DPosition* lineData,
|
||||
int dataSize,
|
||||
int lineIdx,
|
||||
double refSteppingZ,
|
||||
const SSG_cornerParam cornerPara,
|
||||
std::vector<SSG_basicFeature1D>& line_features);
|
||||
|
||||
/// <summary>
|
||||
/// 提取激光线上的拐点特征，地面端点附近的拐点视为合格拐点
|
||||
/// 根据Seg进行corner提取
|
||||
/// nPointIdx被重新定义成Feature类型
|
||||
/// 算法流程：
|
||||
/// （1）逐点计算前向角和后向角
|
||||
/// （2）逐点计算拐角，顺时针为负，逆时针为正
|
||||
/// （3）搜索正拐角的极大值。
|
||||
/// （4）判断拐角是否为跳变
|
||||
/// </summary>
|
||||
SG_APISHARED_EXPORT void sg_getLineContourCornerFeature_groundMask_BQ(
|
||||
std::vector<SVzNL3DPosition>& lineData,
|
||||
std::vector<int> groundMask,
|
||||
int lineIdx,
|
||||
double contourWin, //ground边缘范围
|
||||
const SSG_cornerParam cornerPara, //scale通常取bagH的1/4
|
||||
std::vector<SSG_basicFeature1D>& line_features);
|
||||
|
||||
SG_APISHARED_EXPORT void wd_getLineDataIntervals(
|
||||
std::vector<SVzNL3DPosition>& lineData,
|
||||
const SSG_lineSegParam lineSegPara,
|
||||
std::vector<SSG_RUN>& 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);
|
||||
|
||||
/// <summary>
|
||||
/// 提取激光线上的特征：跳变、低于z阈值、V及L型，用于粒径检测（PSM)
|
||||
/// seg端点：z距离大于门限
|
||||
/// nPointIdx被重新定义成Feature类型
|
||||
/// 算法流程：
|
||||
/// （1）逐点计算前向角和后向角
|
||||
/// （2）逐点计算拐角，顺时针为负，逆时针为正
|
||||
/// （3）搜索正拐角的极大值。
|
||||
/// （4）判断拐角是否为跳变
|
||||
/// </summary>
|
||||
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）判断拐角是否为跳变
|
||||
/// </summary>
|
||||
SG_APISHARED_EXPORT void wd_getRingArcFeature(
|
||||
std::vector< SVzNL3DPosition>& lineData,
|
||||
int lineIdx,
|
||||
const SSG_cornerParam cornerPara,
|
||||
SVzNLRangeD ringArcWidth, //定子的环宽度
|
||||
std::vector<SWD_segFeature>& line_ringArcs //环
|
||||
);
|
||||
|
||||
/// <summary>
|
||||
/// 提取激光线上的拐点特征。是在PSM， LVTypeFeature, BQ等拐点算法的基础上的版本。
|
||||
/// nPointIdx被重新定义成Feature类型
|
||||
/// 算法流程：
|
||||
/// （1）逐点计算前向角和后向角
|
||||
/// （2）逐点计算拐角，顺时针为负，逆时针为正
|
||||
/// （3）搜索正拐角的极大值。
|
||||
/// （4）判断拐角是否为跳变
|
||||
/// </summary>
|
||||
SG_APISHARED_EXPORT void wd_getLineCorerFeature(
|
||||
std::vector< SVzNL3DPosition>& lineData,
|
||||
int lineIdx,
|
||||
const SSG_cornerParam cornerPara,
|
||||
std::vector<SSG_basicFeature1D>& line_cornerFeatures //拐点
|
||||
);
|
||||
|
||||
/// 提取激光线上的拐点特征。是在PSM， LVTypeFeature, BQ等拐点算法的基础上的版本。
|
||||
/// 使用平均点距进行加速
|
||||
/// nPointIdx被重新定义成Feature类型
|
||||
/// 算法流程：
|
||||
/// （1）逐点计算前向角和后向角
|
||||
/// （2）逐点计算拐角，顺时针为负，逆时针为正
|
||||
/// （3）搜索正拐角的极大值。
|
||||
/// （4）判断拐角是否为跳变
|
||||
/// </summary>
|
||||
SG_APISHARED_EXPORT void wd_getLineCorerFeature_accelerate(
|
||||
std::vector< SVzNL3DPosition>& lineData,
|
||||
int lineIdx,
|
||||
const SSG_cornerParam cornerPara,
|
||||
const double pointInterval,
|
||||
std::vector<SSG_RUN_EX>& segs,
|
||||
std::vector<SSG_basicFeature1D>& line_cornerFeatures //拐点特征
|
||||
);
|
||||
|
||||
//提取凸起段
|
||||
SG_APISHARED_EXPORT void wd_getLineRaisedFeature(
|
||||
std::vector< SVzNL3DPosition>& lineData,
|
||||
int lineIdx,
|
||||
SSG_raisedFeatureParam raisedFeaturePara, //凸起参数
|
||||
std::vector<SWD_segFeature>& 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<SSG_featureSemiCircle>& lineSegs,
|
||||
std::vector<SSG_featureSemiCircle>& invlaidLineSegs);
|
||||
|
||||
#if 0
|
||||
//对一个面的边缘特征进行提取。
|
||||
//此算法首先获取扫描线上各个段，然后对段的端点进行处理，得到边缘特征
|
||||
void wd_getSurfaceEdgeFeatures(
|
||||
std::vector< SVzNL3DPosition>& lineData,
|
||||
int lineIdx,
|
||||
const SSG_lineSegParam lineSegPara,
|
||||
std::vector<SWDIndexing3DPoint>& edgeFeatures);
|
||||
#endif
|
||||
|
||||
/// <summary>
|
||||
/// 提取激光线上的Jumping特征
|
||||
/// nPointIdx被重新定义成Feature类型
|
||||
/// 算法流程：
|
||||
/// （1）逐点计算前向角和后向角
|
||||
/// （2）逐点计算拐角，顺时针为负，逆时针为正
|
||||
/// （3）搜索正拐角的极大值。
|
||||
/// （4）判断拐角是否为跳变
|
||||
/// </summary>
|
||||
SG_APISHARED_EXPORT void sg_getLineJumpFeature_cornerMethod(
|
||||
SVzNL3DPosition* lineData,
|
||||
int dataSize,
|
||||
int lineIdx,
|
||||
const SSG_cornerParam cornerPara, //scale通常取bagH的1/4
|
||||
std::vector< SSG_basicFeature1D>& jumpFeatures);
|
||||
|
||||
SG_APISHARED_EXPORT void wd_getLineGloveArcs(
|
||||
std::vector<SVzNL3DPosition>& lineData,
|
||||
int lineIdx,
|
||||
const SSG_gloveArcParam arcPara,
|
||||
std::vector<SSG_basicFeature1D>& 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);
|
||||
|
||||
/// <summary>
|
||||
/// 提取激光线上的极值点（极大值点和极小值点）
|
||||
///
|
||||
/// </summary>
|
||||
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<SVzNL3DPosition>& 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<SSG_lineFeature>& lineFeatures,
|
||||
std::vector<SSG_featureTree>& trees,
|
||||
SSG_treeGrowParam growParam);
|
||||
|
||||
//对feature进行生长:不比较type，只判断位置是否相邻
|
||||
SG_APISHARED_EXPORT void wd_getFeatureGrowingTrees_noTypeMatch(
|
||||
std::vector<SSG_lineFeature>& lineFeatures,
|
||||
std::vector<SSG_featureTree>& feature_trees,
|
||||
std::vector<SSG_featureTree>& ending_trees,
|
||||
SSG_treeGrowParam growParam);
|
||||
|
||||
//对segment feature进行生长
|
||||
SG_APISHARED_EXPORT void wd_getSegFeatureGrowingTrees(
|
||||
std::vector<std::vector<SWD_segFeature>>& all_lineFeatures,
|
||||
std::vector<SWD_segFeatureTree>& feature_trees,
|
||||
SSG_treeGrowParam growParam);
|
||||
|
||||
SG_APISHARED_EXPORT void sg_lineFeaturesGrowing(
|
||||
int lineIdx,
|
||||
bool isLastLine,
|
||||
std::vector<SSG_basicFeature1D>& features,
|
||||
std::vector<SSG_featureTree>& trees,
|
||||
SSG_treeGrowParam growParam);
|
||||
|
||||
//corner特征生长
|
||||
SG_APISHARED_EXPORT void sg_cornerFeaturesGrowing(
|
||||
std::vector<std::vector<SSG_basicFeature1D>>& cornerFeatures,
|
||||
std::vector<SSG_featureTree>& trees,
|
||||
SSG_treeGrowParam growParam);
|
||||
|
||||
//gap特征生长
|
||||
SG_APISHARED_EXPORT void sg_lineGapsGrowing(
|
||||
int lineIdx,
|
||||
bool isLastLine,
|
||||
std::vector<SSG_basicFeatureGap>& features,
|
||||
std::vector<SSG_gapFeatureTree>& trees,
|
||||
SSG_treeGrowParam growParam);
|
||||
|
||||
//SG_APISHARED_EXPORT void sg_getTreeROI(SSG_featureTree* a_tree);
|
||||
|
||||
//对ending进行生长
|
||||
SG_APISHARED_EXPORT void sg_getEndingGrowingTrees(
|
||||
std::vector<SSG_2DValueI>& lineEndings,
|
||||
SVzNL3DLaserLine* laser3DPoints,
|
||||
bool isVScan,
|
||||
int featureType,
|
||||
std::vector<SSG_featureTree>& trees,
|
||||
SSG_treeGrowParam growParam);
|
||||
|
||||
//对ending进行生长，垂直扫描时只进行水平生长；水平扫描时只进行垂直生长
|
||||
SG_APISHARED_EXPORT void sg_getEndingGrowingTrees_angleCheck(
|
||||
std::vector<SSG_2DValueI>& lineEndings,
|
||||
SVzNL3DLaserLine* laser3DPoints,
|
||||
bool isVScan,
|
||||
int featureType,
|
||||
std::vector<SSG_featureTree>& trees,
|
||||
SSG_treeGrowParam growParam,
|
||||
double angleCheckScale);
|
||||
|
||||
//对扫描线上的
|
||||
SG_APISHARED_EXPORT void sg_LVFeatureGrowing(
|
||||
std::vector<SSG_lineFeature>& lineFeatures,
|
||||
std::vector<SSG_featureTree>& trees,
|
||||
SSG_bagParam bagParam,
|
||||
SSG_treeGrowParam growParam,
|
||||
std::vector<SSG_2DValueI>& edgePts_0,
|
||||
std::vector<SSG_2DValueI>& edgePts_1);
|
||||
|
||||
SG_APISHARED_EXPORT void sg_peakFeatureGrowing(
|
||||
std::vector<std::vector< SSG_basicFeature1D>>& lineFeatures,
|
||||
std::vector<SSG_featureTree>& 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<SSG_semiCircleFeatureTree>& trees,
|
||||
std::vector<SSG_semiCircleFeatureTree>& stopTrees,
|
||||
std::vector<SSG_semiCircleFeatureTree>& invalidTrees, //被移除的树，这些树可能将目标分成多个树，从而被移除。
|
||||
SSG_treeGrowParam growParam);
|
||||
|
||||
SSG_meanVar _computeMeanVar(double* data, int size);
|
||||
|
||||
///搜索局部最高点（z最小点）。
|
||||
///搜索方法：每次步进搜索窗口长度的一半。对局部最高点进行标记，防止被重复记录。
|
||||
SG_APISHARED_EXPORT void sg_getLocalPeaks(
|
||||
SVzNL3DLaserLine* scanLines,
|
||||
int lineNum,
|
||||
std::vector<SSG_2DValueI>& peaks,
|
||||
SSG_localPkParam searchWin);
|
||||
|
||||
/// <summary>
|
||||
/// 区域生长法：以局部最高点作为生长种子进行生长
|
||||
/// 生长方法与一般的区域生长不同：以种子点为圆心作圆周扫描，记录扫描到的边界
|
||||
/// </summary>
|
||||
//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<SSG_conotourPair>& contourPairs,
|
||||
std::vector<SSG_intPair>& 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);
|
||||
|
||||
/// <summary>
|
||||
/// 以5x5方式寻找localPeaks
|
||||
/// </summary>
|
||||
/// <param name="input"></param>
|
||||
/// <param name="peaks"></param>
|
||||
SG_APISHARED_EXPORT void sg_getLocalPeaks_distTransform(
|
||||
cv::Mat& input,
|
||||
std::vector<SSG_2DValueI>& peaks,
|
||||
SSG_localPkParam searchWin);
|
||||
|
||||
/// <summary>
|
||||
/// 使用模板法提取直角特征
|
||||
/// 水平向下直角特征：拐点左侧deltaZ在一个很小的范围内；拐点右侧deltaY在一个很小的范围内
|
||||
/// </summary>
|
||||
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<SVzNL3DPosition>>& scanLines
|
||||
);
|
||||
|
||||
//计算点云ROI: vecotr格式
|
||||
SG_APISHARED_EXPORT SVzNL3DRangeD wd_getPointCloudROI(
|
||||
std::vector<SVzNL3DPoint>& scanData);
|
||||
|
||||
//计算点云的ROI和scale: vecotr格式
|
||||
SG_APISHARED_EXPORT SWD_pointCloudPara wd_getPointCloudPara(
|
||||
std::vector< std::vector<SVzNL3DPosition>>& 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<SVzNL3DPoint>& points,
|
||||
SVzNL3DPoint& center,
|
||||
SVzNL3DPoint& direction);
|
||||
|
||||
//圆最小二乘拟合
|
||||
SG_APISHARED_EXPORT double fitCircleByLeastSquare(
|
||||
const std::vector<SVzNL3DPoint>& pointArray,
|
||||
SVzNL3DPoint& center,
|
||||
double& radius);
|
||||
|
||||
//抛物线最小二乘拟合 y=ax^2 + bx + c
|
||||
SG_APISHARED_EXPORT bool leastSquareParabolaFitEigen(
|
||||
const std::vector<cv::Point2d>& 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<SVzNL2DPoint>& pts);
|
||||
|
||||
/// <summary>
|
||||
/// 两步法标注
|
||||
/// </summary>
|
||||
/// <param name="bwImg"> 目标点为“1”， 空白点为“0”</param>
|
||||
/// <param name="labImg"> 标注结果。每个点为rgnID, ID从2开始 </param>
|
||||
/// <param name="labelRgns"></param>
|
||||
SG_APISHARED_EXPORT void SG_TwoPassLabel(
|
||||
const cv::Mat& bwImg,
|
||||
cv::Mat& labImg,
|
||||
std::vector<SSG_Region>& 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<cv::Point3f> Points3ds,
|
||||
std::vector<double>& res);
|
||||
|
||||
//计算一个平面调平参数。
|
||||
//数据输入中可以有一个地平面和参考调平平面，以最高的平面进行调平
|
||||
//旋转矩阵为调平参数，即将平面法向调整为垂直向量的参数
|
||||
SG_APISHARED_EXPORT SSG_planeCalibPara sg_getPlaneCalibPara(
|
||||
SVzNL3DLaserLine* laser3DPoints,
|
||||
int lineNum);
|
||||
|
||||
SG_APISHARED_EXPORT SSG_planeCalibPara sg_HCameraVScan_getGroundCalibPara(
|
||||
std::vector< std::vector<SVzNL3DPosition>>& scanLines);
|
||||
|
||||
//将一个平面调整为水平平面(z为固定值），
|
||||
SG_APISHARED_EXPORT SSG_planeCalibPara adjustPlaneToXYPlane(
|
||||
double plane_A, double plane_B, double plane_C //平面法向量
|
||||
);
|
||||
|
||||
//计算一个平面调平参数。
|
||||
//数据输入中可以有一个地平面和参考调平平面，以最高的平面进行调平
|
||||
//旋转矩阵为调平参数，即将平面法向调整为垂直向量的参数
|
||||
SG_APISHARED_EXPORT SSG_planeCalibPara sg_getPlaneCalibPara2(
|
||||
std::vector< std::vector<SVzNL3DPosition>>& scanLines);
|
||||
|
||||
//针对孔板计算一个平面调平参数：提取不经过孔的扫描线进行平面拟合
|
||||
SG_APISHARED_EXPORT SSG_planeCalibPara sg_getHolePlaneCalibPara(
|
||||
std::vector< std::vector<SVzNL3DPosition>>& scanLines);
|
||||
|
||||
//计算一个平面调平参数。
|
||||
//以数据输入中ROI以内的点进行平面拟合，计算调平参数
|
||||
//旋转矩阵为调平参数，即将平面法向调整为垂直向量的参数
|
||||
SG_APISHARED_EXPORT SSG_planeCalibPara sg_getPlaneCalibPara_ROIs(
|
||||
SVzNL3DLaserLine* laser3DPoints,
|
||||
int lineNum,
|
||||
std::vector<SVzNL3DRangeD>& ROIs);
|
||||
|
||||
//计算一个平面调平参数。
|
||||
//以数据输入中ROI以内的点进行平面拟合，计算调平参数
|
||||
//旋转矩阵为调平参数，即将平面法向调整为垂直向量的参数
|
||||
SG_APISHARED_EXPORT SSG_planeCalibPara sg_getPlaneCalibPara2_ROI(
|
||||
std::vector< std::vector<SVzNL3DPosition>>& 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<std::vector< SVzNL3DPosition>>& objClusters //result
|
||||
);
|
||||
|
||||
//基于栅格上点的窗口内的相邻点的聚类，聚类条件由3D点的邻域决定
|
||||
//使用vector构成2维结构体数组
|
||||
SG_APISHARED_EXPORT void wd_gridPointClustering(
|
||||
std::vector<std::vector<SSG_featureClusteringInfo>>& featureMask,
|
||||
std::vector<std::vector<SVzNL3DPoint>>& feature3DInfo,
|
||||
int clusterCheckWin, //搜索窗口
|
||||
SSG_treeGrowParam growParam,//聚类条件
|
||||
int clusterID, //当前Cluster的ID
|
||||
std::vector< SVzNL2DPoint>& a_cluster, //result
|
||||
SVzNL3DRangeD& clusterRoi //roi3D
|
||||
);
|
||||
|
||||
//对扫描数据的聚类
|
||||
SG_APISHARED_EXPORT void wd_gridPointClustering_2(
|
||||
std::vector<std::vector<SVzNL3DPosition>>& gridData,
|
||||
std::vector<std::vector<SSG_intPair>>& pointMaskInfo, //防止聚类时重复选中
|
||||
int clusterCheckWin, //搜索窗口
|
||||
SSG_treeGrowParam growParam,//聚类条件
|
||||
int clusterID, //当前Cluster的ID
|
||||
std::vector< SVzNL2DPoint>& a_cluster, //result
|
||||
SVzNL3DRangeD& clusterRoi //roi3D
|
||||
);
|
||||
|
||||
//使用聚类方法完成8连通连通域分析
|
||||
SG_APISHARED_EXPORT void wd_gridPointClustering_labelling(
|
||||
std::vector<std::vector<SSG_featureClusteringInfo>>& featureMask,
|
||||
std::vector<std::vector<SVzNL3DPoint>>& feature3DInfo,
|
||||
int clusterID, //当前Cluster的ID
|
||||
std::vector< SVzNL2DPoint>& a_cluster, //result
|
||||
SVzNLRect& clusterRoi //roi2D
|
||||
);
|
||||
|
||||
//对栅格化数据进行XY平面上的投影量化，并对量化产生的空白点进行插值
|
||||
SG_APISHARED_EXPORT void pointClout2DProjection(
|
||||
std::vector< std::vector<SVzNL3DPosition>>& gridScanData,
|
||||
SVzNLRangeD x_range,
|
||||
SVzNLRangeD y_range,
|
||||
double scale,
|
||||
double cuttingGrndZ,
|
||||
int edgeSkip,
|
||||
double inerPolateDistTh, //插值阈值。大于此阈值的不进行量化插值
|
||||
cv::Mat& projectionData,//投影量化数据，初始化为一个极大值1e+6
|
||||
cv::Mat& backIndexing //标记坐标索引，用于回找3D坐标
|
||||
);
|
||||
|
||||
//对栅格化数据进行XY平面上的投影量化，Z值保留，并对量化产生的空白点进行插值
|
||||
SG_APISHARED_EXPORT void pointClout2DQuantization(
|
||||
std::vector< std::vector<SVzNL3DPosition>>& gridScanData,
|
||||
SVzNLRangeD x_range,
|
||||
SVzNLRangeD y_range,
|
||||
double scale,
|
||||
double cuttingGrndZ,
|
||||
int edgeSkip,
|
||||
double inerPolateDistTh, //插值阈值。大于此阈值的不进行量化插值
|
||||
std::vector<std::vector<double>> quantiData, //量化数据，初始化为一个极大值1e+6
|
||||
std::vector<std::vector<SVzNL2DPoint>> backIndexing //标记坐标索引，用于回找3D坐标
|
||||
);
|
||||
|
||||
//分水岭算法
|
||||
SG_APISHARED_EXPORT void watershed(SWD_waterShedImage& img);
|
||||
// 根据输入的种子点进行分水岭算法
|
||||
SG_APISHARED_EXPORT void wd_seedWatershed(
|
||||
SWD_waterShedImage& img,
|
||||
std::vector<SSG_2DValueI>& watershedSeeds, //种子点
|
||||
int maxLevel, //最大水位
|
||||
int startMakerID //起始Marker的ID
|
||||
);
|
||||
|
||||
//点云滤波预处理
|
||||
SG_APISHARED_EXPORT void wd_noiseFilter(
|
||||
std::vector< std::vector<SVzNL3DPosition>>& scanLines,
|
||||
const SSG_outlierFilterParam filterParam,
|
||||
int* errCode);
|
||||
|
||||
// Eigen库实现：计算从pts1向pts2的旋转平移矩阵
|
||||
//需要
|
||||
SG_APISHARED_EXPORT void caculateRT(
|
||||
const std::vector<cv::Point3d>& pts1,
|
||||
const std::vector<cv::Point3d>& 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<cv::Point3d>& dirVectors);
|
||||
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.
Binary file not shown.
Binary file not shown.
@ -12,6 +12,7 @@
|
||||
#include <opencv2/opencv.hpp>
|
||||
#include <Windows.h>
|
||||
#include <limits>
|
||||
#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<cv::Point3d> pts_eye;
|
||||
pts_eye.resize(6);
|
||||
std::vector<cv::Point3d> 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<cv::Point3d> test_pts_eye;
|
||||
std::vector<cv::Point3d> 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<cv::Point3d> 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<std::vector< cv::Point3d>> 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<cv::Point3d> 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<cv::Point3d> 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
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user