snsopush/GrabBag
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

增加手眼转换；添加输入法提示以限制输入为数字，重构配置参数的加载和保存逻辑，增强日志记录功能，更新版本信息。

Browse Source
This commit is contained in:
杰仔 2025-06-23 00:05:19 +08:00
parent 1fb70d41e4
commit b355ac9763
22 changed files with 580 additions and 119 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
Doc/~$袋拆垛项目相机端通信协议.docx
View File

@ -1 +0,0 @@
useruser

2
GrabBagApp/GrabBagApp.pro
View File

@ -25,6 +25,7 @@ INCLUDEPATH += $$PWD/../VrEyeDevice/Inc
SOURCES += \
Presenter/Src/GrabBagPresenter.cpp \
Presenter/Src/LaserDataLoader.cpp \
Presenter/Src/PathManager.cpp \
Presenter/Src/RobotProtocol.cpp \
devstatus.cpp \
dialogcamera.cpp \
@ -37,6 +38,7 @@ SOURCES += \
HEADERS += \
Presenter/Inc/GrabBagPresenter.h \
Presenter/Inc/LaserDataLoader.h \
Presenter/Inc/PathManager.h \
Presenter/Inc/RobotProtocol.h \
IYGrabBagStatus.h \
Version.h \

4
GrabBagApp/Presenter/Inc/GrabBagPresenter.h
View File

@ -10,6 +10,7 @@
#include "VZNL_Types.h"
#include "SG_bagPositioning_Export.h"
#include "LaserDataLoader.h"
#include "PathManager.h"
#include <QImage>
#include <QPainter>
#include <QColor>
@ -86,7 +87,7 @@ private:
void _ClearDetectionDataCache();
// 发送检测结果给机械臂
void _SendDetectionResultToRobot(const std::vector<SSG_peakRgnInfo>& objOps, int cameraIndex);
void _SendDetectionResultToRobot(const DetectionResult& detectionResult, int cameraIndex);
private:
IVrConfig* m_vrConfig = nullptr;
@ -112,6 +113,7 @@ private:
// 算法参数成员变量
SG_bagPositionParam m_algoParam; // 算法参数
SSG_planeCalibPara m_planeCalibParam; // 平面校准参数
double m_clibMatrix[16]; // 手眼标定矩阵
// 调试数据加载器
LaserDataLoader m_dataLoader;

57
GrabBagApp/Presenter/Inc/PathManager.h Normal file
View File

@ -0,0 +1,57 @@
#ifndef PATHMANAGER_H
#define PATHMANAGER_H
#include <QString>
/**
* @brief 路径管理器类，统一管理应用程序的配置文件路径
*
* 该类负责根据不同操作系统提供合适的配置文件存储路径：
* - Windows: 程序目录
* - Linux: 用户配置目录 (~/.config/GrabBag/)
*/
class PathManager
{
public:
/**
* @brief 获取配置文件(config.xml)的完整路径
* @return 配置文件的完整路径
*/
static QString GetConfigFilePath();
/**
* @brief 获取手眼标定文件(clib.ini)的完整路径
* @return 手眼标定文件的完整路径
*/
static QString GetCalibrationFilePath();
private:
/**
* @brief 确保配置目录存在，如果不存在则创建
* @return 成功创建或目录已存在返回true，失败返回false
*/
static bool EnsureConfigDirectoryExists();
/**
* @brief 获取应用程序配置目录路径
* @return 配置目录的完整路径
*/
static QString GetAppConfigDirectory();
/**
* @brief 获取程序目录路径
* @return 程序目录的完整路径
*/
static QString GetProgramDirectory();
/**
* @brief 获取用户配置目录路径（仅Linux系统）
* @return 用户配置目录的完整路径
*/
static QString GetUserConfigDirectory();
};
#endif // PATHMANAGER_H

159
GrabBagApp/Presenter/Src/GrabBagPresenter.cpp
View File

@ -5,6 +5,8 @@
#include <QtCore/QFileInfo>
#include <QtCore/QDir>
#include <QtCore/QString>
#include <QtCore/QStandardPaths>
#include <QtCore/QFile>
#include <cmath>
#include <algorithm>
#include <QImage>
@ -12,6 +14,7 @@
#include "VrTimeUtils.h"
#include "SG_bagPositioning_Export.h"
#include "VrConvert.h"
GrabBagPresenter::GrabBagPresenter()
: m_vrConfig(nullptr)
@ -67,11 +70,8 @@ int GrabBagPresenter::Init()
m_vrConfig->SetConfigChangeNotify(this);
}
// 获取可执行文件路径
QString exePath = QCoreApplication::applicationFilePath();
// 获取可执行文件所在目录
QString configPath = QFileInfo(exePath).absoluteDir().path() + "/config.xml";
// 获取配置文件路径
QString configPath = PathManager::GetConfigFilePath();
// 读取IP列表
ConfigResult configResult = m_vrConfig->LoadConfig(configPath.toStdString());
@ -227,15 +227,49 @@ int GrabBagPresenter::InitRobotProtocol()
int GrabBagPresenter::InitAlgorithmParams()
{
LOG_DEBUG("Start initializing algorithm parameters\n");
// 获取可执行文件路径
QString exePath = QCoreApplication::applicationFilePath();
QString configPath = QFileInfo(exePath).absoluteDir().path() + "/config.xml";
// 初始化手眼标定矩阵为单位矩阵（4x4变换矩阵）
double initClibMatrix[16] = {
1.0, 0.0, 0.0, 0.0, // 第一行
0.0, 1.0, 0.0, 0.0, // 第二行
0.0, 0.0, 1.0, 0.0, // 第三行
0.0, 0.0, 0.0, 1.0 // 第四行
};
for (int i = 0; i < 16; i++) {
m_clibMatrix[i] = initClibMatrix[i];
}
// 获取手眼标定文件路径并确保文件存在
QString clibPath = PathManager::GetCalibrationFilePath();
LOG_INFO("Loading hand-eye calibration matrix from: %s\n", clibPath.toStdString().c_str());
if(QFile::exists(clibPath))
{
CVrConvert::LoadClibMatrix(clibPath.toStdString().c_str(), "clib", m_clibMatrix);
}
// 获取配置文件路径
QString configPath = PathManager::GetConfigFilePath();
LOG_INFO("Loading configuration from: %s\n", configPath.toStdString().c_str());
// 检查配置文件是否存在
QFileInfo configFileInfo(configPath);
if (!configFileInfo.exists()) {
LOG_ERROR("Configuration file does not exist: %s\n", configPath.toStdString().c_str());
return ERR_CODE(FILE_ERR_NOEXIST);
}
// 读取配置文件
ConfigResult configResult = m_vrConfig->LoadConfig(configPath.toStdString());
const VrAlgorithmParams& xmlParams = configResult.algorithmParams;
LOG_INFO("Loaded XML params - Bag: L=%.1f, W=%.1f, H=%.1f\n",
xmlParams.bagParam.bagL, xmlParams.bagParam.bagW, xmlParams.bagParam.bagH);
LOG_INFO("Loaded XML params - Pile: L=%.1f, W=%.1f, H=%.1f\n",
xmlParams.pileParam.pileL, xmlParams.pileParam.pileW, xmlParams.pileParam.pileH);
// 初始化算法参数结构
memset(&m_algoParam, 0, sizeof(SG_bagPositionParam));
@ -248,24 +282,12 @@ int GrabBagPresenter::InitAlgorithmParams()
m_algoParam.filterParam.continuityTh = xmlParams.filterParam.continuityTh;
m_algoParam.filterParam.outlierTh = xmlParams.filterParam.outlierTh;
#if BAG_ALGO_USE_CORNER_FEATURE
// 设置角点特征参数
m_algoParam.cornerParam.cornerTh = xmlParams.cornerParam.cornerTh;
m_algoParam.cornerParam.scale = xmlParams.cornerParam.scale;
m_algoParam.cornerParam.minEndingGap = xmlParams.cornerParam.minEndingGap;
m_algoParam.cornerParam.jumpCornerTh_1 = xmlParams.cornerParam.jumpCornerTh_1;
m_algoParam.cornerParam.jumpCornerTh_2 = xmlParams.cornerParam.jumpCornerTh_2;
#else
// 设置斜率参数
m_algoParam.slopeParam.LSlopeZWin = xmlParams.slopeParam.LSlopeZWin;
m_algoParam.slopeParam.validSlopeH = xmlParams.slopeParam.validSlopeH;
m_algoParam.slopeParam.minLJumpH = xmlParams.slopeParam.minLJumpH;
m_algoParam.slopeParam.minEndingGap = xmlParams.slopeParam.minEndingGap;
// 设置山谷参数
m_algoParam.valleyPara.valleyMinH = xmlParams.valleyParam.valleyMinH;
m_algoParam.valleyPara.valleyMaxW = xmlParams.valleyParam.valleyMaxW;
#endif
m_algoParam.cornerParam.cornerTh = xmlParams.cornerParam.cornerTh;
m_algoParam.cornerParam.scale = xmlParams.cornerParam.scale;
m_algoParam.cornerParam.minEndingGap = xmlParams.cornerParam.minEndingGap;
m_algoParam.cornerParam.jumpCornerTh_1 = xmlParams.cornerParam.jumpCornerTh_1;
m_algoParam.cornerParam.jumpCornerTh_2 = xmlParams.cornerParam.jumpCornerTh_2;
// 设置增长参数
m_algoParam.growParam.maxLineSkipNum = xmlParams.growParam.maxLineSkipNum;
@ -288,7 +310,7 @@ int GrabBagPresenter::InitAlgorithmParams()
m_planeCalibParam.planeHeight = xmlParams.planeCalibParam.planeHeight;
LOG_INFO("Algorithm parameters initialized successfully:\n");
LOG_INFO(" Bag: L=%d, W=%d, H=%d\n", m_algoParam.bagParam.bagL, m_algoParam.bagParam.bagW, m_algoParam.bagParam.bagH);
LOG_INFO(" Bag: L=%.1f, W=%.1f, H=%.1f\n", m_algoParam.bagParam.bagL, m_algoParam.bagParam.bagW, m_algoParam.bagParam.bagH);
LOG_INFO(" Filter: continuityTh=%.1f, outlierTh=%d\n", m_algoParam.filterParam.continuityTh, m_algoParam.filterParam.outlierTh);
LOG_INFO(" Plane calibration: height=%.1f\n", m_planeCalibParam.planeHeight);
@ -545,7 +567,6 @@ bool GrabBagPresenter::IsCameraConnected(int index)
}
// 静态回调函数实现
void GrabBagPresenter::_StaticCameraNotify(EVzDeviceWorkStatus eStatus, void* pExtData, unsigned int nDataLength, void* pInfoParam)
{
@ -681,7 +702,7 @@ void GrabBagPresenter::_AlgoDetectThread()
int GrabBagPresenter::_DetectTask()
{
LOG_INFO("----- Start real detection task using algorithm\n");
LOG_INFO("[Algo Thread] Start real detection task using algorithm\n");
// 1. 获取缓存的点云数据
if (m_detectionDataCache.empty()) {
@ -698,8 +719,8 @@ int GrabBagPresenter::_DetectTask()
}
// 3. 使用成员变量算法参数（已在初始化时从XML读取）
LOG_INFO("Using algorithm parameters from XML configuration\n");
LOG_INFO(" Bag: L=%d, W=%d, H=%d\n", m_algoParam.bagParam.bagL, m_algoParam.bagParam.bagW, m_algoParam.bagParam.bagH);
LOG_INFO("[Algo Thread] Using algorithm parameters from XML configuration\n");
LOG_INFO(" Bag: L=%.1f, W=%.1f, H=%.1f\n",m_algoParam.bagParam.bagL, m_algoParam.bagParam.bagW, m_algoParam.bagParam.bagH);
LOG_INFO(" Filter: continuityTh=%.1f, outlierTh=%d\n", m_algoParam.filterParam.continuityTh, m_algoParam.filterParam.outlierTh);
CVrTimeUtils oTimeUtils;
@ -707,14 +728,12 @@ int GrabBagPresenter::_DetectTask()
for (size_t i = 0; i < m_detectionDataCache.size(); i++) {
sg_lineDataR(&m_detectionDataCache[i], m_planeCalibParam.planeCalib, m_planeCalibParam.planeHeight);
}
LOG_DEBUG("sg_lineDataR time: %.2f ms\n", oTimeUtils.GetElapsedTimeInMilliSec());
// 5. 调用算法检测接口（使用成员变量参数）
oTimeUtils.Update();
std::vector<SSG_peakRgnInfo> objOps;
sg_getBagPosition(static_cast<SVzNL3DLaserLine*>(m_detectionDataCache.data()), m_detectionDataCache.size(), m_algoParam, m_planeCalibParam, objOps);
LOG_INFO("sg_getBagPosition detected %zu objects time : %.2f ms\n", objOps.size(), oTimeUtils.GetElapsedTimeInMilliSec());
LOG_INFO("[Algo Thread] sg_getBagPosition detected %zu objects time : %.2f ms\n", objOps.size(), oTimeUtils.GetElapsedTimeInMilliSec());
// 6. 转换检测结果为UI显示格式
DetectionResult detectionResult;
@ -722,7 +741,7 @@ int GrabBagPresenter::_DetectTask()
detectionResult.image = _GeneratePointCloudImage(static_cast<SVzNL3DLaserLine*>(m_detectionDataCache.data()),
m_detectionDataCache.size(), objOps);
// 转换检测结果
// 转换检测结果为UI显示格式（使用机械臂坐标系数据）
for (size_t i = 0; i < objOps.size(); i++) {
const SSG_peakRgnInfo& obj = objOps[i];
@ -733,18 +752,29 @@ int GrabBagPresenter::_DetectTask()
detectionResult.positionLayout.push_back(layer);
}
// 创建位置数据
// 进行坐标转换：从算法坐标系转换到机械臂坐标系
SVzNL3DPoint targetObj;
targetObj.x = obj.centerPos.x;
targetObj.y = obj.centerPos.y;
targetObj.z = obj.centerPos.z;
SVzNL3DPoint robotObj;
CVrConvert::EyeToRobot(targetObj, robotObj, m_clibMatrix);
// 创建位置数据（使用转换后的机械臂坐标）
GrabBagPosition pos;
pos.x = obj.centerPos.x;
pos.y = obj.centerPos.y;
pos.z = obj.centerPos.z;
pos.roll = obj.centerPos.x_roll;
pos.pitch = obj.centerPos.y_pitch;
pos.yaw = obj.centerPos.z_yaw;
pos.x = robotObj.x; // 机械臂坐标X
pos.y = robotObj.y; // 机械臂坐标Y
pos.z = robotObj.z; // 机械臂坐标Z
pos.roll = obj.centerPos.x_roll; // 保持原有姿态角
pos.pitch = obj.centerPos.y_pitch; // 保持原有姿态角
pos.yaw = obj.centerPos.z_yaw; // 保持原有偏航角
detectionResult.positionLayout[0].position.push_back(pos);
LOG_INFO("Object %zu: X=%.2f, Y=%.2f, Z=%.2f, Roll=%.2f, Pitch=%.2f, Yaw=%.2f\n",
LOG_INFO("[Algo Thread] Object %zu Eye Coords: X=%.2f, Y=%.2f, Z=%.2f\n",
i, obj.centerPos.x, obj.centerPos.y, obj.centerPos.z);
LOG_INFO("[Algo Thread] Object %zu Robot Coords: X=%.2f, Y=%.2f, Z=%.2f, Roll=%.2f, Pitch=%.2f, Yaw=%.2f\n",
i, pos.x, pos.y, pos.z, pos.roll, pos.pitch, pos.yaw);
}
@ -756,8 +786,8 @@ int GrabBagPresenter::_DetectTask()
QString statusMsg = QString("检测完成，发现%1个目标").arg(objOps.size());
m_pStatus->OnStatusUpdate(statusMsg.toStdString());
// 更新机械臂协议状态（发送第一个检测到的目标位置）
_SendDetectionResultToRobot(objOps, m_currentCameraIndex);
// 更新机械臂协议状态（发送转换后的目标位置数据）
_SendDetectionResultToRobot(detectionResult, m_currentCameraIndex);
// 9. 检测完成后，将工作状态更新为"完成"
if (m_pStatus) {
@ -801,19 +831,19 @@ void GrabBagPresenter::_ClearDetectionDataCache()
}
// 发送检测结果给机械臂
void GrabBagPresenter::_SendDetectionResultToRobot(const std::vector<SSG_peakRgnInfo>& objOps, int cameraIndex)
void GrabBagPresenter::_SendDetectionResultToRobot(const DetectionResult& detectionResult, int cameraIndex)
{
if (!m_pRobotProtocol) {
LOG_WARNING("Robot protocol not initialized, cannot send detection result\n");
LOG_WARNING("[Robot Task] Robot protocol not initialized, cannot send detection result\n");
return;
}
// 准备多目标数据结构
RobotProtocol::MultiTargetData multiTargetData;
multiTargetData.count = static_cast<uint16_t>(objOps.size());
if (objOps.empty()) {
LOG_INFO("No objects detected, sending empty result to robot\n");
// 检查是否有检测结果
if (detectionResult.positionLayout.empty() || detectionResult.positionLayout[0].position.empty()) {
LOG_INFO("[Robot Task] No objects detected, sending empty result to robot\n");
// 发送空的多目标数据
multiTargetData.count = 0;
multiTargetData.targets.clear();
@ -824,25 +854,20 @@ void GrabBagPresenter::_SendDetectionResultToRobot(const std::vector<SSG_peakRgn
return;
}
// 获取检测到的目标位置（已经是机械臂坐标系）
const auto& positions = detectionResult.positionLayout[0].position;
multiTargetData.count = static_cast<uint16_t>(positions.size());
// 转换所有检测结果为机械臂坐标
for (size_t i = 0; i < objOps.size(); i++) {
const SSG_peakRgnInfo& targetObj = objOps[i];
// 直接使用已经转换好的机械臂坐标
for (size_t i = 0; i < positions.size(); i++) {
const GrabBagPosition& pos = positions[i];
LOG_INFO("Target %zu: X=%f, Y=%f, Z=%f, Yaw=%f\n",
i, targetObj.centerPos.x, targetObj.centerPos.y, targetObj.centerPos.z,
targetObj.centerPos.z_yaw);
// 坐标转换：从算法坐标系转换到机械臂坐标系
// 直接使用已转换的坐标数据
RobotProtocol::TargetPosition robotTarget;
// 位置坐标转换 (单位转换：mm -> mm，坐标系转换等)
robotTarget.x = targetObj.centerPos.x; // X轴坐标
robotTarget.y = targetObj.centerPos.y; // Y轴坐标
robotTarget.z = targetObj.centerPos.z; // Z轴坐标
// 姿态角度转换 (弧度转角度，只保留Z轴旋转角)
robotTarget.rz = targetObj.centerPos.z_yaw; // Yaw角 (弧度转角度)
robotTarget.x = pos.x; // 已转换的X轴坐标
robotTarget.y = pos.y; // 已转换的Y轴坐标
robotTarget.z = pos.z; // 已转换的Z轴坐标
robotTarget.rz = pos.yaw; // Yaw角
// 添加到多目标数据
multiTargetData.targets.push_back(robotTarget);
@ -850,7 +875,7 @@ void GrabBagPresenter::_SendDetectionResultToRobot(const std::vector<SSG_peakRgn
// 发送多目标数据给机械臂（使用相机ID，从1开始编号）
int result = m_pRobotProtocol->SetMultiTargetData(multiTargetData, cameraIndex);
LOG_INFO("SetMultiTargetData result: %d for camera ID: %d\n", result, cameraIndex);
LOG_INFO("[Robot Task] SetMultiTargetData result: %d for camera ID: %d\n", result, cameraIndex);
if (m_pStatus) {
if (result != SUCCESS) {
m_pStatus->OnStatusUpdate(QString("发送多目标坐标给机械臂失败，相机ID: %1").arg(cameraIndex).toStdString());
@ -1002,3 +1027,5 @@ void GrabBagPresenter::OnConfigChanged(const ConfigResult& configResult)

65
GrabBagApp/Presenter/Src/PathManager.cpp Normal file
View File

@ -0,0 +1,65 @@
#include "PathManager.h"
#include <QtCore/QCoreApplication>
#include <QtCore/QFileInfo>
#include <QtCore/QDir>
#include <QtCore/QStandardPaths>
#include <QtCore/QFile>
#include "VrLog.h"
QString PathManager::GetConfigFilePath()
{
// 确保目标目录存在
EnsureConfigDirectoryExists();
return GetAppConfigDirectory() + "/config.xml";
}
QString PathManager::GetCalibrationFilePath()
{
// 确保目标目录存在
EnsureConfigDirectoryExists();
return GetAppConfigDirectory() + "/clib.ini";
}
QString PathManager::GetAppConfigDirectory()
{
#ifdef _WIN32
// Windows系统：使用程序目录
return GetProgramDirectory();
#else
// Linux系统：使用用户配置目录
return GetUserConfigDirectory() + "/GrabBag";
#endif
}
bool PathManager::EnsureConfigDirectoryExists()
{
QString configDir = GetAppConfigDirectory();
if (QDir().exists(configDir)) {
LOG_DEBUG("Configuration directory already exists: %s\n", configDir.toStdString().c_str());
return true;
}
LOG_INFO("Creating configuration directory: %s\n", configDir.toStdString().c_str());
bool success = QDir().mkpath(configDir);
if (success) {
LOG_INFO("Configuration directory created successfully\n");
} else {
LOG_ERROR("Failed to create configuration directory: %s\n", configDir.toStdString().c_str());
}
return success;
}
QString PathManager::GetProgramDirectory()
{
QString exePath = QCoreApplication::applicationFilePath();
return QFileInfo(exePath).absoluteDir().path();
}
QString PathManager::GetUserConfigDirectory()
{
return QStandardPaths::writableLocation(QStandardPaths::ConfigLocation);
}

4
GrabBagApp/Version.h
View File

@ -3,8 +3,8 @@
#define GRABBAG_VERSION_STRING "1.0.0"
#define GRABBAG_BUILD_STRING "1"
#define GRABBAG_FULL_VERSION_STRING "V1.0.0_1"
#define GRABBAG_BUILD_STRING "2"
#define GRABBAG_FULL_VERSION_STRING "V1.0.0_2"
// 获取版本信息的便捷函数
inline const char* GetGrabBagVersion() {

18
GrabBagApp/dialogcamera.ui
View File

@ -103,6 +103,9 @@
<string notr="true">color: rgb(221, 225, 233);
background-color: rgb(47, 48, 52);</string>
</property>
<property name="inputMethodHints">
<set>Qt::InputMethodHint::ImhDigitsOnly</set>
</property>
</widget>
</item>
</layout>
@ -135,6 +138,9 @@ background-color: rgb(47, 48, 52);</string>
<string notr="true">color: rgb(221, 225, 233);
background-color: rgb(47, 48, 52);</string>
</property>
<property name="inputMethodHints">
<set>Qt::InputMethodHint::ImhDigitsOnly</set>
</property>
</widget>
</item>
</layout>
@ -167,6 +173,9 @@ background-color: rgb(47, 48, 52);</string>
<string notr="true">color: rgb(221, 225, 233);
background-color: rgb(47, 48, 52);</string>
</property>
<property name="inputMethodHints">
<set>Qt::InputMethodHint::ImhDigitsOnly</set>
</property>
</widget>
</item>
</layout>
@ -237,6 +246,9 @@ background-color: rgb(47, 48, 52);</string>
<string notr="true">color: rgb(221, 225, 233);
background-color: rgb(47, 48, 52);</string>
</property>
<property name="inputMethodHints">
<set>Qt::InputMethodHint::ImhDigitsOnly</set>
</property>
</widget>
</item>
</layout>
@ -269,6 +281,9 @@ background-color: rgb(47, 48, 52);</string>
<string notr="true">color: rgb(221, 225, 233);
background-color: rgb(47, 48, 52);</string>
</property>
<property name="inputMethodHints">
<set>Qt::InputMethodHint::ImhDigitsOnly</set>
</property>
</widget>
</item>
</layout>
@ -301,6 +316,9 @@ background-color: rgb(47, 48, 52);</string>
<string notr="true">color: rgb(221, 225, 233);
background-color: rgb(47, 48, 52);</string>
</property>
<property name="inputMethodHints">
<set>Qt::InputMethodHint::ImhDigitsOnly</set>
</property>
</widget>
</item>
</layout>

9
GrabBagApp/dialogcameralevel.ui
View File

@ -124,6 +124,9 @@
<string notr="true">color: rgb(221, 225, 233);
background-color: rgb(47, 48, 52);</string>
</property>
<property name="inputMethodHints">
<set>Qt::InputMethodHint::ImhDigitsOnly</set>
</property>
</widget>
</item>
<item>
@ -137,6 +140,9 @@ background-color: rgb(47, 48, 52);</string>
<string notr="true">color: rgb(221, 225, 233);
background-color: rgb(47, 48, 52);</string>
</property>
<property name="inputMethodHints">
<set>Qt::InputMethodHint::ImhDigitsOnly</set>
</property>
</widget>
</item>
<item>
@ -150,6 +156,9 @@ background-color: rgb(47, 48, 52);</string>
<string notr="true">color: rgb(221, 225, 233);
background-color: rgb(47, 48, 52);</string>
</property>
<property name="inputMethodHints">
<set>Qt::InputMethodHint::ImhDigitsOnly</set>
</property>
</widget>
</item>
</layout>

56
GrabBagApp/dialogconfig.cpp
View File

@ -5,7 +5,10 @@
#include <QtCore/QCoreApplication>
#include <QtCore/QFileInfo>
#include <QtCore/QDir>
#include <QtCore/QStandardPaths>
#include <QtCore/QFile>
#include "VrLog.h"
#include "PathManager.h"
DialogConfig::DialogConfig(IVrConfig* devConfig, QWidget *parent) :
QDialog(parent),
@ -18,8 +21,7 @@ DialogConfig::DialogConfig(IVrConfig* devConfig, QWidget *parent) :
setWindowFlags(Qt::FramelessWindowHint);
// 获取配置文件路径
QString exePath = QCoreApplication::applicationFilePath();
m_configFilePath = QFileInfo(exePath).absoluteDir().path() + "/config.xml";
m_configFilePath = PathManager::GetConfigFilePath();
// 从配置文件加载数据到界面
LoadConfigToUI();
@ -44,20 +46,24 @@ void DialogConfig::LoadConfigToUI()
// 将算法参数加载到界面控件
const VrAlgorithmParams& params = m_configData.algorithmParams;
// 垛型参数（假设这些对应UI中的垛长、垛宽、垛高）
ui->lineEdit->setText(QString::number(params.bagParam.bagL)); // 垛长
ui->lineEdit_2->setText(QString::number(params.bagParam.bagW)); // 垛宽
ui->lineEdit_3->setText(QString::number(params.bagParam.bagH)); // 垛高
LOG_INFO("LoadConfigToUI: pileL:%.1f, pileW:%.1f, pileH:%.1f, bagL:%.1f, bagW:%.1f, bagH:%.1f\n",
params.pileParam.pileL, params.pileParam.pileW, params.pileParam.pileH,
params.bagParam.bagL, params.bagParam.bagW, params.bagParam.bagH);
// 垛型参数
ui->lineEdit_pile_length->setText(QString::number(params.pileParam.pileL)); // 垛长
ui->lineEdit_pile_width->setText(QString::number(params.pileParam.pileW)); // 垛宽
ui->lineEdit_pile_height->setText(QString::number(params.pileParam.pileH)); // 垛高
// 袋型参数
ui->lineEdit_4->setText(QString::number(params.bagParam.bagL)); // 袋长
ui->lineEdit_5->setText(QString::number(params.bagParam.bagW)); // 袋宽
ui->lineEdit_6->setText(QString::number(params.bagParam.bagH)); // 袋高
ui->lineEdit_bag_length->setText(QString::number(params.bagParam.bagL)); // 袋长
ui->lineEdit_bag_width->setText(QString::number(params.bagParam.bagW)); // 袋宽
ui->lineEdit_bag_height->setText(QString::number(params.bagParam.bagH)); // 袋高
// 过滤参数
ui->lineEdit_7->setText(QString::number(params.filterParam.continuityTh)); // X过滤
ui->lineEdit_8->setText(QString::number(params.filterParam.outlierTh)); // Y过滤
ui->lineEdit_9->setText(QString::number(params.planeCalibParam.planeHeight)); // Z过滤
ui->lineEdit_filter_x->setText(QString::number(params.filterParam.continuityTh)); // X过滤
ui->lineEdit_filter_y->setText(QString::number(params.filterParam.outlierTh)); // Y过滤
ui->lineEdit_filter_z->setText(QString::number(params.planeCalibParam.planeHeight)); // Z过滤
LOG_INFO("Configuration loaded to UI successfully\n");
@ -86,18 +92,26 @@ bool DialogConfig::SaveConfigFromUI()
VrAlgorithmParams& params = m_configData.algorithmParams;
// 垛型参数
params.bagParam.bagL = ui->lineEdit->text().toDouble(); // 垛长
params.bagParam.bagW = ui->lineEdit_2->text().toDouble(); // 垛宽
params.bagParam.bagH = ui->lineEdit_3->text().toDouble(); // 垛高
params.pileParam.pileL = ui->lineEdit_pile_length->text().toDouble(); // 垛长
params.pileParam.pileW = ui->lineEdit_pile_width->text().toDouble(); // 垛宽
params.pileParam.pileH = ui->lineEdit_pile_height->text().toDouble(); // 垛高
// 袋型参数（这里简化处理，实际可能需要分开）
// ui->lineEdit_4, ui->lineEdit_5, ui->lineEdit_6 可以作为袋的单独参数
// 袋型参数
params.bagParam.bagL = ui->lineEdit_bag_length->text().toDouble(); // 袋长
params.bagParam.bagW = ui->lineEdit_bag_width->text().toDouble(); // 袋宽
params.bagParam.bagH = ui->lineEdit_bag_height->text().toDouble(); // 袋高
// 过滤参数
params.filterParam.continuityTh = ui->lineEdit_7->text().toDouble(); // X过滤
params.filterParam.outlierTh = ui->lineEdit_8->text().toInt(); // Y过滤
params.planeCalibParam.planeHeight = ui->lineEdit_9->text().toDouble(); // Z过滤
params.filterParam.continuityTh = ui->lineEdit_filter_x->text().toDouble(); // X过滤
params.filterParam.outlierTh = ui->lineEdit_filter_y->text().toInt(); // Y过滤
params.planeCalibParam.planeHeight = ui->lineEdit_filter_z->text().toDouble(); // Z过滤
LOG_INFO("SaveConfigFromUI: pileL:%.1f, pileW:%.1f, pileH:%.1f, bagL:%.1f, bagW:%.1f, bagH:%.1f, continuityTh:%.1f, outlierTh:%d, planeHeight:%.1f\n",
params.pileParam.pileL, params.pileParam.pileW, params.pileParam.pileH,
params.bagParam.bagL, params.bagParam.bagW, params.bagParam.bagH,
params.filterParam.continuityTh, params.filterParam.outlierTh, params.planeCalibParam.planeHeight);
LOG_DEBUG("Starting to save configuration to file: %s\n", m_configFilePath.toStdString().c_str());
// 保存配置文件，SaveConfig会自动触发通知回调
bool success = m_vrConfig->SaveConfig(m_configFilePath.toStdString(), m_configData);
@ -143,3 +157,5 @@ void DialogConfig::on_btn_cancel_clicked()
// 直接关闭窗口，不保存任何更改
reject();
}

60
GrabBagApp/dialogconfig.ui
View File

@ -93,7 +93,7 @@
</widget>
</item>
<item>
<widget class="QLineEdit" name="lineEdit">
<widget class="QLineEdit" name="lineEdit_pile_length">
<property name="font">
<font>
<pointsize>18</pointsize>
@ -103,6 +103,9 @@
<string notr="true">color: rgb(221, 225, 233);
background-color: rgb(47, 48, 52);</string>
</property>
<property name="inputMethodHints">
<set>Qt::InputMethodHint::ImhDigitsOnly</set>
</property>
</widget>
</item>
</layout>
@ -125,7 +128,7 @@ background-color: rgb(47, 48, 52);</string>
</widget>
</item>
<item>
<widget class="QLineEdit" name="lineEdit_2">
<widget class="QLineEdit" name="lineEdit_pile_width">
<property name="font">
<font>
<pointsize>18</pointsize>
@ -135,6 +138,9 @@ background-color: rgb(47, 48, 52);</string>
<string notr="true">color: rgb(221, 225, 233);
background-color: rgb(47, 48, 52);</string>
</property>
<property name="inputMethodHints">
<set>Qt::InputMethodHint::ImhDigitsOnly</set>
</property>
</widget>
</item>
</layout>
@ -157,7 +163,7 @@ background-color: rgb(47, 48, 52);</string>
</widget>
</item>
<item>
<widget class="QLineEdit" name="lineEdit_3">
<widget class="QLineEdit" name="lineEdit_pile_height">
<property name="font">
<font>
<pointsize>18</pointsize>
@ -167,6 +173,9 @@ background-color: rgb(47, 48, 52);</string>
<string notr="true">color: rgb(221, 225, 233);
background-color: rgb(47, 48, 52);</string>
</property>
<property name="inputMethodHints">
<set>Qt::InputMethodHint::ImhDigitsOnly</set>
</property>
</widget>
</item>
</layout>
@ -218,7 +227,7 @@ background-color: rgb(47, 48, 52);</string>
</widget>
</item>
<item>
<widget class="QLineEdit" name="lineEdit_4">
<widget class="QLineEdit" name="lineEdit_bag_length">
<property name="font">
<font>
<pointsize>18</pointsize>
@ -228,6 +237,9 @@ background-color: rgb(47, 48, 52);</string>
<string notr="true">color: rgb(221, 225, 233);
background-color: rgb(47, 48, 52);</string>
</property>
<property name="inputMethodHints">
<set>Qt::InputMethodHint::ImhDigitsOnly</set>
</property>
</widget>
</item>
</layout>
@ -250,7 +262,7 @@ background-color: rgb(47, 48, 52);</string>
</widget>
</item>
<item>
<widget class="QLineEdit" name="lineEdit_5">
<widget class="QLineEdit" name="lineEdit_bag_width">
<property name="font">
<font>
<pointsize>18</pointsize>
@ -260,6 +272,9 @@ background-color: rgb(47, 48, 52);</string>
<string notr="true">color: rgb(221, 225, 233);
background-color: rgb(47, 48, 52);</string>
</property>
<property name="inputMethodHints">
<set>Qt::InputMethodHint::ImhDigitsOnly</set>
</property>
</widget>
</item>
</layout>
@ -282,7 +297,7 @@ background-color: rgb(47, 48, 52);</string>
</widget>
</item>
<item>
<widget class="QLineEdit" name="lineEdit_6">
<widget class="QLineEdit" name="lineEdit_bag_height">
<property name="font">
<font>
<pointsize>18</pointsize>
@ -292,6 +307,9 @@ background-color: rgb(47, 48, 52);</string>
<string notr="true">color: rgb(221, 225, 233);
background-color: rgb(47, 48, 52);</string>
</property>
<property name="inputMethodHints">
<set>Qt::InputMethodHint::ImhDigitsOnly</set>
</property>
</widget>
</item>
</layout>
@ -343,7 +361,7 @@ background-color: rgb(47, 48, 52);</string>
</widget>
</item>
<item>
<widget class="QLineEdit" name="lineEdit_7">
<widget class="QLineEdit" name="lineEdit_filter_x">
<property name="font">
<font>
<pointsize>18</pointsize>
@ -353,6 +371,9 @@ background-color: rgb(47, 48, 52);</string>
<string notr="true">color: rgb(221, 225, 233);
background-color: rgb(47, 48, 52);</string>
</property>
<property name="inputMethodHints">
<set>Qt::InputMethodHint::ImhDigitsOnly</set>
</property>
</widget>
</item>
</layout>
@ -375,7 +396,7 @@ background-color: rgb(47, 48, 52);</string>
</widget>
</item>
<item>
<widget class="QLineEdit" name="lineEdit_8">
<widget class="QLineEdit" name="lineEdit_filter_y">
<property name="font">
<font>
<pointsize>18</pointsize>
@ -385,6 +406,9 @@ background-color: rgb(47, 48, 52);</string>
<string notr="true">color: rgb(221, 225, 233);
background-color: rgb(47, 48, 52);</string>
</property>
<property name="inputMethodHints">
<set>Qt::InputMethodHint::ImhDigitsOnly</set>
</property>
</widget>
</item>
</layout>
@ -407,7 +431,7 @@ background-color: rgb(47, 48, 52);</string>
</widget>
</item>
<item>
<widget class="QLineEdit" name="lineEdit_9">
<widget class="QLineEdit" name="lineEdit_filter_z">
<property name="font">
<font>
<pointsize>18</pointsize>
@ -417,6 +441,9 @@ background-color: rgb(47, 48, 52);</string>
<string notr="true">color: rgb(221, 225, 233);
background-color: rgb(47, 48, 52);</string>
</property>
<property name="inputMethodHints">
<set>Qt::InputMethodHint::ImhDigitsOnly</set>
</property>
</widget>
</item>
</layout>
@ -471,12 +498,15 @@ background-color: rgb(47, 48, 52);</string>
</widget>
</widget>
<tabstops>
<tabstop>lineEdit</tabstop>
<tabstop>lineEdit_3</tabstop>
<tabstop>lineEdit_4</tabstop>
<tabstop>lineEdit_6</tabstop>
<tabstop>lineEdit_7</tabstop>
<tabstop>lineEdit_9</tabstop>
<tabstop>lineEdit_pile_length</tabstop>
<tabstop>lineEdit_pile_width</tabstop>
<tabstop>lineEdit_pile_height</tabstop>
<tabstop>lineEdit_bag_length</tabstop>
<tabstop>lineEdit_bag_width</tabstop>
<tabstop>lineEdit_bag_height</tabstop>
<tabstop>lineEdit_filter_x</tabstop>
<tabstop>lineEdit_filter_y</tabstop>
<tabstop>lineEdit_filter_z</tabstop>
<tabstop>btn_apply</tabstop>
<tabstop>btn_cancel</tabstop>
</tabstops>

4
GrabBagApp/main.cpp
View File

@ -2,6 +2,10 @@
#include <QApplication>
#include <QIcon>
#include <QSharedMemory>
#include <QSystemSemaphore>
#include <QMessageBox>
#include <QDebug>
int main(int argc, char *argv[])
{

7
GrabBagApp/mainwindow.cpp
View File

@ -53,8 +53,11 @@ MainWindow::MainWindow(QWidget *parent)
buildLabel->setStyleSheet("color: rgb(239, 241, 245); font-size: 20px; margin-right: 16px;");
statusBar()->addPermanentWidget(buildLabel);
// 启动后自动全屏显示
this->showFullScreen();
// 隐藏标题栏
setWindowFlags(Qt::FramelessWindowHint);
// 启动后自动最大化显示
this->showMaximized();
// 初始化GraphicsScene
QGraphicsScene* scene = new QGraphicsScene(this);

55
GrabBagApp/resultitem.ui
View File

@ -21,9 +21,9 @@
<property name="geometry">
<rect>
<x>50</x>
<y>50</y>
<width>181</width>
<height>131</height>
<y>40</y>
<width>191</width>
<height>151</height>
</rect>
</property>
<layout class="QHBoxLayout" name="horizontalLayout">
@ -31,6 +31,11 @@
<layout class="QVBoxLayout" name="verticalLayout">
<item>
<widget class="QLabel" name="label">
<property name="font">
<font>
<pointsize>12</pointsize>
</font>
</property>
<property name="layoutDirection">
<enum>Qt::LayoutDirection::RightToLeft</enum>
</property>
@ -48,6 +53,11 @@ background-color: rgb(37, 38, 42);</string>
</item>
<item>
<widget class="QLabel" name="label_2">
<property name="font">
<font>
<pointsize>12</pointsize>
</font>
</property>
<property name="layoutDirection">
<enum>Qt::LayoutDirection::RightToLeft</enum>
</property>
@ -65,6 +75,11 @@ background-color: rgb(37, 38, 42);</string>
</item>
<item>
<widget class="QLabel" name="label_3">
<property name="font">
<font>
<pointsize>12</pointsize>
</font>
</property>
<property name="layoutDirection">
<enum>Qt::LayoutDirection::RightToLeft</enum>
</property>
@ -82,6 +97,11 @@ background-color: rgb(37, 38, 42);</string>
</item>
<item>
<widget class="QLabel" name="label_4">
<property name="font">
<font>
<pointsize>12</pointsize>
</font>
</property>
<property name="layoutDirection">
<enum>Qt::LayoutDirection::RightToLeft</enum>
</property>
@ -103,6 +123,11 @@ background-color: rgb(37, 38, 42);</string>
<layout class="QVBoxLayout" name="verticalLayout_2">
<item>
<widget class="QLineEdit" name="result_x">
<property name="font">
<font>
<pointsize>-1</pointsize>
</font>
</property>
<property name="styleSheet">
<string notr="true">background-color: rgb(59, 61, 71);
color: rgb(239, 241, 245);
@ -114,6 +139,11 @@ font-size: 12px;</string>
</item>
<item>
<widget class="QLineEdit" name="result_y">
<property name="font">
<font>
<pointsize>-1</pointsize>
</font>
</property>
<property name="styleSheet">
<string notr="true">background-color: rgb(59, 61, 71);
color: rgb(239, 241, 245);
@ -125,6 +155,11 @@ font-size: 12px;</string>
</item>
<item>
<widget class="QLineEdit" name="result_z">
<property name="font">
<font>
<pointsize>-1</pointsize>
</font>
</property>
<property name="styleSheet">
<string notr="true">background-color: rgb(59, 61, 71);
color: rgb(239, 241, 245);
@ -136,6 +171,11 @@ font-size: 12px;</string>
</item>
<item>
<widget class="QLineEdit" name="result_rz">
<property name="font">
<font>
<pointsize>-1</pointsize>
</font>
</property>
<property name="styleSheet">
<string notr="true">background-color: rgb(59, 61, 71);
color: rgb(239, 241, 245);
@ -153,11 +193,16 @@ font-size: 12px;</string>
<property name="geometry">
<rect>
<x>52</x>
<y>17</y>
<width>63</width>
<y>10</y>
<width>191</width>
<height>30</height>
</rect>
</property>
<property name="font">
<font>
<pointsize>12</pointsize>
</font>
</property>
<property name="layoutDirection">
<enum>Qt::LayoutDirection::LeftToRight</enum>
</property>

1
GrabBagPrj/pkg_app.sh
View File

@ -213,7 +213,6 @@ echo "Icon=/usr/share/pixmaps/grabbag.png" >> ${DESKTOP_PATH}
echo "Categories=Development;" >> ${DESKTOP_PATH}
echo "GenericName=GrabBag App" >> ${DESKTOP_PATH}
echo "Keywords=grabbag;app;" >> ${DESKTOP_PATH}
echo "SingleMainWindow=true" >> ${DESKTOP_PATH}
echo "StartupNotify=true" >> ${DESKTOP_PATH}
echo "设置文件权限..."

11
VrConfig/Inc/IVrConfig.h
View File

@ -21,6 +21,16 @@ struct VrBagParam
double bagH = 160.0; // 高
};
/**
* @brief 垛参数
*/
struct VrPileParam
{
double pileL = 1300.0; // 垛长
double pileW = 900.0; // 垛宽
double pileH = 800.0; // 垛高
};
/**
* @brief 离群点滤波参数
*/
@ -91,6 +101,7 @@ struct VrPlaneCalibParam
struct VrAlgorithmParams
{
VrBagParam bagParam; // 编织袋参数
VrPileParam pileParam; // 垛参数
VrOutlierFilterParam filterParam; // 滤波参数
VrCornerParam cornerParam; // 角点特征参数
VrSlopeParam slopeParam; // 斜率参数

19
VrConfig/Src/VrConfig.cpp
View File

@ -90,6 +90,18 @@ ConfigResult CVrConfig::LoadConfig(const std::string& filePath)
result.algorithmParams.bagParam.bagH = bagParamElement->DoubleAttribute("bagH");
}
// 解析垛参数
XMLElement* pileParamElement = algoParamsElement->FirstChildElement("PileParam");
if (pileParamElement)
{
if (pileParamElement->Attribute("pileL"))
result.algorithmParams.pileParam.pileL = pileParamElement->DoubleAttribute("pileL");
if (pileParamElement->Attribute("pileW"))
result.algorithmParams.pileParam.pileW = pileParamElement->DoubleAttribute("pileW");
if (pileParamElement->Attribute("pileH"))
result.algorithmParams.pileParam.pileH = pileParamElement->DoubleAttribute("pileH");
}
// 解析滤波参数
XMLElement* filterParamElement = algoParamsElement->FirstChildElement("FilterParam");
if (filterParamElement)
@ -218,6 +230,13 @@ bool CVrConfig::SaveConfig(const std::string& filePath, ConfigResult& configResu
bagParamElement->SetAttribute("bagH", configResult.algorithmParams.bagParam.bagH);
algoParamsElement->InsertEndChild(bagParamElement);
// 添加垛参数
XMLElement* pileParamElement = doc.NewElement("PileParam");
pileParamElement->SetAttribute("pileL", configResult.algorithmParams.pileParam.pileL);
pileParamElement->SetAttribute("pileW", configResult.algorithmParams.pileParam.pileW);
pileParamElement->SetAttribute("pileH", configResult.algorithmParams.pileParam.pileH);
algoParamsElement->InsertEndChild(pileParamElement);
// 添加滤波参数
XMLElement* filterParamElement = doc.NewElement("FilterParam");
filterParamElement->SetAttribute("continuityTh", configResult.algorithmParams.filterParam.continuityTh);

3
VrConfig/config/config.xml
View File

@ -12,6 +12,9 @@
<!-- 编织袋参数 -->
<BagParam bagL="650.0" bagW="450.0" bagH="160.0" />
<!-- 垛参数 -->
<PileParam pileL="1300.0" pileW="900.0" pileH="800.0" />
<!-- 滤波参数 -->
<FilterParam continuityTh="20.0" outlierTh="5" />

22
VrEyeDevice/Inc/VrConvert.h Normal file
View File

@ -0,0 +1,22 @@
#pragma once
#include <iostream>
#include <ostream>
#include "VZNL_Types.h"
std::ostream& operator<<(std::ostream& os, const SVzNL3DPoint& sPoint);
namespace CVrConvert
{
// 加载矩阵
bool LoadClibMatrix(const char* sFileName, const char* sIdent, double dMatrix[16]);
// 存储矩阵
bool SaveClibMatrix(const char* sFileName, const char* sIdent, double dMatrix[16]);
// 手眼标定
bool CalibEyeToRobot(SVzNL3DPoint* pEye3DPoint, SVzNL3DPoint* pRobot3DPoint, const int nNum);
// 手眼坐标转换
bool EyeToRobot(const SVzNL3DPoint sEyePoint, SVzNL3DPoint& sRobotPoint, const double clib[16]);
};

120
VrEyeDevice/Src/VrConvert.cpp Normal file
View File

@ -0,0 +1,120 @@
#include "VrConvert.h"
#include <vector>
#include <Eigen/Dense>
#include <Eigen/Core>
#include <Eigen/Geometry>
#include "VrSimpleLog.h"
#include "SimpleIni.h"
std::ostream& operator<<(std::ostream& os, const SVzNL3DPoint& sPoint)
{
os << "3DPoint {x: " << sPoint.x << ", y: " << sPoint.y << ", z: " << sPoint.z << "}";
return os;
}
bool CVrConvert::LoadClibMatrix(const char* sFileName, const char* sIdent, double dMatrix[16])
{
// 读取配置文件
CSimpleIniA ini;
// 加载文件并解析
SI_Error rc = ini.LoadFile(sFileName);
if(SI_OK != rc) return false;
for(int i = 0 ; i < 16; i++)
{
char sKey[64] = {0};
#ifdef _WIN32
sprintf_s(sKey, "%s_%d", sIdent, i);
#else
sprintf(sKey, "%s_%d", sIdent, i);
#endif
dMatrix[i] = ini.GetDoubleValue(sIdent, sKey, i % 5 == 0 ? 1 : 0);
}
return true;
}
// 存储矩阵
bool CVrConvert::SaveClibMatrix(const char* sFileName, const char* sIdent, double dMatrix[16])
{
CSimpleIniA ini;
SI_Error rc = ini.LoadFile(sFileName);
if (rc != SI_OK) return false;
for(int i = 0 ; i < 16; i++)
{
char sKey[64] = {0};
#ifdef _WIN32
sprintf_s(sKey, "%s_%d", sIdent, i);
#else
sprintf(sKey, "%s_%d", sIdent, i);
#endif
ini.SetDoubleValue(sIdent, sKey, dMatrix[i]);
}
ini.SaveFile(sFileName);
return true;
}
// 手眼标定
bool CVrConvert::CalibEyeToRobot(SVzNL3DPoint* pEye3DPoint, SVzNL3DPoint* pRobot3DPoint, const int nNum)
{
if(nNum <= 0) return false;
#if 0
Eigen::Matrix4d calibration_matrix = Eigen::Matrix4d::Identity();
for(int i = 0; i < nNum; i++)
{
Eigen::Matrix<double, 4, 1> eye, robot;
eye << pEye3DPoint[i].x << pEye3DPoint[i].y << pEye3DPoint[i].z << 1;
robot << pRobot3DPoint[i].x << pRobot3DPoint[i].y << pRobot3DPoint[i].z << 1;
calibration_matrix += (eye.inverse() * robot);
}
// 求取平均值作为最终的手眼标定矩阵
calibration_matrix /= nNum;
std::cout << "Hand-eye calibration matrix:\n" << calibration_matrix << std::endl;
#endif
return true;
}
bool CVrConvert::EyeToRobot(const SVzNL3DPoint sEyePoint, SVzNL3DPoint& sRobotPoint, const double clib[16])
{
bool bRet = true;
// Define the original arm coordinates (1x6)
Eigen::Matrix<double, 4, 1> eyecoordinates;
eyecoordinates << sEyePoint.x, sEyePoint.y, sEyePoint.z, 1;
#if 0
Eigen::Matrix<double, 3, 1> eyeposture;
eyeposture << sEyePoint.roll, sEyePoint.pitch, sEyePoint.yaw;
#endif
// Define the transformation matrix (4x4)
Eigen::Matrix4d transformation_matrix;
// Fill in the values of the transformation matrix (example values)
transformation_matrix << clib[0], clib[1], clib[2], clib[3],
clib[4], clib[5], clib[6], clib[7],
clib[8], clib[9], clib[10], clib[11],
clib[12], clib[13], clib[14], clib[15];
Eigen::Matrix<double, 4, 1> robotcoordinates;
robotcoordinates = transformation_matrix * eyecoordinates;
sRobotPoint.x = robotcoordinates(0, 0);
sRobotPoint.y = robotcoordinates(1, 0);
sRobotPoint.z = robotcoordinates(2, 0);
#if 0
Eigen::Matrix3d oriMatrix = transformation_matrix.block<3, 3>(0, 0);
Eigen::Matrix<double, 3, 1> robotposture;
robotposture = oriMatrix * eyeposture;
sRobotPoint.roll = robotposture(0, 0);
sRobotPoint.pitch = robotposture(1, 0);
sRobotPoint.yaw = robotposture(2, 0);
#endif
return bRet;
}

14
VrEyeDevice/VrEyeDevice.pro
View File

@ -13,24 +13,28 @@ CONFIG += c++17
INCLUDEPATH += ./Inc
INCLUDEPATH += ./_Inc
INCLUDEPATH += ../VrCommon/Inc
INCLUDEPATH += ../VrUtils/Inc
SOURCES += \
Src/VrConvert.cpp \
Src/VrEyeDevice.cpp \
Src/VrEyeCommon.cpp
HEADERS += \
Inc/IVrEyeDevice.h \
Inc/VrConvert.h \
Inc/VrEyeDevice_global.h \
VrEyeDevice_global.h \
_Inc/VrEyeCommon.h \
_Inc/VrEyeDevice.h
#linux下的为unix ，windows下用的win32
INCLUDEPATH += ../SDK/VzNLSDK/_Inc
INCLUDEPATH += ../SDK/VzNLSDK/Inc
INCLUDEPATH += $$PWD/../SDK/eigen-3.3.9
#linux下的为unix ，windows下用的win32
INCLUDEPATH += $$PWD/../SDK/VzNLSDK/_Inc
INCLUDEPATH += $$PWD/../SDK/VzNLSDK/Inc
win32:CONFIG(release, debug|release): {
LIBS += -L$$PWD/../SDK/VzNLSDK/Windows/x64/Release
LIBS += -lVzKernel -lVzNLDetect -lVzNLGraphics
@ -49,6 +53,8 @@ else:unix:!macx: {
LIBS += -ldl -lpthread -lrt
}
INCLUDEPATH += $$PWD/../VrUtils/Inc
INCLUDEPATH += $$PWD/../VrUtils/ini
win32:CONFIG(debug, debug|release) {
LIBS += -L../VrUtils/debug -lVrUtils
}else:win32:CONFIG(release, debug|release){

3
VrUtils/VrUtils.pro
View File

@ -28,6 +28,8 @@ HEADERS += \
Inc/VrNetUtils.h \
Inc/VrSimpleLog.h \
Inc/VrTimeUtils.h \
ini/ConvertUTF.h \
ini/SimpleIni.h \
log4cpp/include/log4cpp/AbortAppender.hh \
log4cpp/include/log4cpp/Appender.hh \
log4cpp/include/log4cpp/AppenderSkeleton.hh \
@ -94,6 +96,7 @@ SOURCES += \
Src/VrLog.cpp \
Src/VrNetUtils.cpp \
Src/VrTimeUtils.cpp \
ini/ConvertUTF.c \
log4cpp/src/AbortAppender.cpp \
log4cpp/src/Appender.cpp \
log4cpp/src/AppenderSkeleton.cpp \