first Commit

This commit is contained in:
lidg
2026-10-26 10:56:56 +08:00
commit c8cd533d9b
81 changed files with 128540 additions and 0 deletions
+317
View File
@@ -0,0 +1,317 @@
#ifndef NOMINMAX
#define NOMINMAX
#endif
#include "UserVisibleLog.h"
#include "MeasurementOptionsPanel.h"
#include "CameraPanel.h"
#include <QVBoxLayout>
#include <QGridLayout>
#include <QGroupBox>
#include <QLabel>
#include <QLineEdit>
#include <QPushButton>
#include <QComboBox>
#include <QDoubleSpinBox>
#include <QTableWidget>
#include <QHeaderView>
#include <QFileDialog>
#include <QJsonDocument>
#include <QSaveFile>
#include <QFile>
#include <QFileInfo>
#include <QDir>
#include <QDateTime>
#include <QCryptographicHash>
#include <QTemporaryDir>
#include <QPointer>
#include <QSignalBlocker>
#include <QThread>
#include <QUuid>
#include <VisionGuideApi.h>
#include <cmath>
#include <algorithm>
#include <stdexcept>
namespace {
QByteArray bytes(const QString& path){QFile f(path);if(!f.open(QIODevice::ReadOnly))throw std::runtime_error(QString("无法读取:%1").arg(path).toUtf8().constData());return f.readAll();}
QString sha(const QByteArray& value){return QString::fromLatin1(QCryptographicHash::hash(value,QCryptographicHash::Sha256).toHex());}
QJsonObject readJson(const QString& path){QJsonParseError error;auto document=QJsonDocument::fromJson(bytes(path),&error);if(error.error!=QJsonParseError::NoError||!document.isObject())throw std::runtime_error("配置JSON无效。");return document.object();}
QJsonArray fields(QLineEdit* const* inputs,int count){QJsonArray out;for(int i=0;i<count;++i){bool ok=false;double value=inputs[i]->text().trimmed().toDouble(&ok);if(!ok||!std::isfinite(value))throw std::runtime_error("请完整填写有限数值,XYZ单位mm,ABC单位deg。");out.append(value);}return out;}
cv::Vec3d normal(const QJsonArray& a){if(a.size()!=3)throw std::runtime_error("缺少料盘法向。");cv::Vec3d out;for(int i=0;i<3;++i){if(!a[i].isDouble()||!std::isfinite(a[i].toDouble()))throw std::runtime_error("料盘法向无效。");out[i]=a[i].toDouble();}double length=cv::norm(out);if(length<.9||length>1.1)throw std::runtime_error("料盘法向不是单位向量。");return out/length;}
QString uniqueId(){return QDateTime::currentDateTime().toString("yyyyMMdd_HHmmss_zzz")+"_board_"+QUuid::createUuid().toString(QUuid::WithoutBraces).left(8);}
}
void MeasurementOptionsPanel::createTraySetupControls(QVBoxLayout* layout){
auto box=new QGroupBox(QString::fromUtf8("料盘法向(HG75)"));box->setObjectName("measurementTrayNormalGroup");auto grid=new QGridLayout(box);
m_normalStatus=new QLabel;m_normalStatus->setObjectName("measurementActiveNormal");m_normalStatus->setStyleSheet("color:#16803c;");m_normalStatus->setWordWrap(true);grid->addWidget(m_normalStatus,0,0,1,3);
m_boardDescription=new QLineEdit(QDir(rootPath()).filePath("VisionGuideData/LD-HG75-13x15.cpd"));m_browseDescription=new QPushButton(QString::fromUtf8("选择描述文件"));
grid->addWidget(m_boardDescription,1,0,1,2);grid->addWidget(m_browseDescription,1,2);
m_measureBoard=new QPushButton(QString::fromUtf8("拍摄并测量标定板"));m_measureBoard->setObjectName("measurementBoardCapture");m_importBoard=new QPushButton(QString::fromUtf8("导入标定板图像"));grid->addWidget(m_measureBoard,2,0,1,2);grid->addWidget(m_importBoard,2,2);
m_boardValues=new QTableWidget(1,6);m_boardValues->setObjectName("measurementBoardPose");m_boardValues->setHorizontalHeaderLabels({"X(mm)","Y(mm)","Z(mm)","RX(°)","RY(°)","RZ(°)"});m_boardValues->setVerticalHeaderLabels({QString::fromUtf8("板→左相机")});m_boardValues->setEditTriggers(QAbstractItemView::NoEditTriggers);m_boardValues->horizontalHeader()->setSectionResizeMode(QHeaderView::ResizeToContents);m_boardValues->setFixedHeight(66);grid->addWidget(m_boardValues,3,0,1,3);
m_boardStatus=new QLabel(QString::fromUtf8(""));m_boardStatus->setWordWrap(true);grid->addWidget(m_boardStatus,4,0,1,3);
grid->addWidget(new QLabel(QString::fromUtf8("标定板拍照位姿(世界系 / TOOL1)")),5,0,1,3);
auto poseRow=new QWidget;auto poseGrid=new QGridLayout(poseRow);poseGrid->setContentsMargins(0,0,0,0);
const char* names[]={"X","Y","Z","A","B","C"};
for(int i=0;i<6;++i){
poseGrid->addWidget(new QLabel(QString::fromUtf8("%1(%2)").arg(names[i],i<3?"mm":"°")),0,i*2);
m_boardRobot[i]=new QLineEdit;m_boardRobot[i]->setMinimumWidth(55);
m_boardRobot[i]->setObjectName(QString("measurementBoardRobot%1").arg(names[i]));
poseGrid->addWidget(m_boardRobot[i],0,i*2+1);poseGrid->setColumnStretch(i*2+1,1);
}
grid->addWidget(poseRow,6,0,1,3);
m_applyNormal=new QPushButton(QString::fromUtf8("保存料盘法向"));m_applyNormal->setObjectName("measurementApplyNormal");m_resetSetup=new QPushButton(QString::fromUtf8("清空当前料盘"));grid->addWidget(m_applyNormal,7,0);grid->addWidget(m_resetSetup,7,1,1,2);layout->addWidget(box);
connect(m_measureBoard,&QPushButton::clicked,this,[this]{measureBoard(false);});connect(m_importBoard,&QPushButton::clicked,this,[this]{measureBoard(true);});connect(m_applyNormal,&QPushButton::clicked,this,&MeasurementOptionsPanel::applyBoardNormal);connect(m_resetSetup,&QPushButton::clicked,this,&MeasurementOptionsPanel::resetTraySetup);
connect(m_browseDescription,&QPushButton::clicked,this,[this]{auto path=QFileDialog::getOpenFileName(this,QString::fromUtf8("选择描述文件"),m_boardDescription->text(),"Board (*.cpd *.descr)");if(!path.isEmpty())m_boardDescription->setText(path);});
}
QString MeasurementOptionsPanel::selectedHeightMethod() const {
return m_heightMethod->currentData().toString();
}
bool MeasurementOptionsPanel::usesTeachPlane() const {
return selectedHeightMethod()!="visual_3d";
}
void MeasurementOptionsPanel::createTeachReferenceControls(QVBoxLayout* layout){
auto box=new QGroupBox(QString::fromUtf8("抓取示教位姿(世界系 / TOOL1)"));box->setObjectName("measurementTeachGroup");
auto grid=new QGridLayout(box);
m_teachStatus=new QLabel;m_teachStatus->setObjectName("measurementActiveTeach");m_teachStatus->setStyleSheet("color:#16803c;");m_teachStatus->setWordWrap(true);grid->addWidget(m_teachStatus,0,0,1,12);
const char* labels[]={"X","Y","Z","A","B","C"};
for(int i=0;i<6;++i){
grid->addWidget(new QLabel(QString::fromUtf8("%1(%2)").arg(labels[i],i<3?"mm":"°")),1,i*2);
auto field=new QLineEdit;field->setMinimumWidth(55);
if(i<3){m_planePoint[i]=field;field->setObjectName(QString("measurementPlane%1").arg(labels[i]));}
else{m_planeAngles[i-3]=field;field->setObjectName(QString("measurementTeach%1").arg(labels[i]));}
grid->addWidget(field,1,i*2+1);grid->setColumnStretch(i*2+1,1);
}
m_importTeach=new QPushButton(QString::fromUtf8("导入示教位姿"));m_saveTeach=new QPushButton(QString::fromUtf8("保存示教位姿"));m_saveTeach->setObjectName("measurementSaveTeach");
grid->addWidget(m_importTeach,2,0,1,6);grid->addWidget(m_saveTeach,2,6,1,6);layout->addWidget(box);
}
void MeasurementOptionsPanel::invalidateGraspTargets(){
if(m_loadingSetup)return;
for(auto key:{"grasp_target","grasp_targets","grasp_Z_source","validated_plane_reference_world_TOOL1_XYZ_mm","equal_height_grasp_plane_confirmed","equal_height_grasp_plane_assumed","grasp_height_method","tray_normal_world_used","tray_normal_evidence","height_teach_XYZABC_mm_deg","height_teach_is_ground_truth"})m_record.remove(key);
for(int row=3;row<6;++row)for(int c=0;c<7;++c)m_values->setItem(row,c,new QTableWidgetItem("—"));
}
QJsonObject MeasurementOptionsPanel::effectiveCalibration() const {
auto cal=readJson(m_calibrationPath);
if(m_normalRequired)throw std::runtime_error("料盘设置已清空,请先测量并应用当前料盘法向。");
if(!m_activeNormal.isEmpty()){
if(m_activeNormal["grasp_calibration_sha256"].toString()!=sha(bytes(m_calibrationPath)))throw std::runtime_error("手眼/抓取标定已改变,请重新建立料盘法向。");
const auto expected=m_activeNormal["left_camera_sha256"].toString();
if(m_matcher&&m_matcher->isCamLoaded(CAM_LEFT)&&!expected.isEmpty()&&expected!=m_matcher->cameraParamSha256(CAM_LEFT))throw std::runtime_error("左内参与已保存料盘法向不一致,请重新测量标定板。");
auto n=m_activeNormal.value("normal_world").toArray();normal(n);cal["tray_normal_world"]=n;
}
return cal;
}
void MeasurementOptionsPanel::updateSetupStatus(){
if(m_loadingSetup)return;
const char* axes[]={"X","Y","Z","A","B","C"};
if(m_activeTeach.size()==6){
QStringList values;for(int i=0;i<6;++i)values<<QString::fromUtf8("%1 %2 %3").arg(axes[i]).arg(m_activeTeach[i].toDouble(),0,'f',3).arg(i<3?"mm":"°");
m_teachStatus->setText(QString::fromUtf8("料盘 %1 · 当前示教:%2").arg(m_currentTrayId).arg(values.join(" ")));
}else m_teachStatus->setText(QString::fromUtf8("料盘 %1 · 当前示教:未保存").arg(m_currentTrayId));
try{
auto cal=effectiveCalibration();auto n=normal(cal["tray_normal_world"].toArray());
QString source=m_activeNormal.isEmpty()?QFileInfo(m_calibrationPath).fileName()+" · "+sha(bytes(m_calibrationPath)).left(8):m_activeNormal["board_record_id"].toString()+QString::fromUtf8(" · %1次观测").arg(m_normalSamples.size());
m_normalStatus->setText(QString::fromUtf8("当前法向 [%1, %2, %3] · 倾角 %4° · %5").arg(n[0],0,'f',6).arg(n[1],0,'f',6).arg(n[2],0,'f',6).arg(std::acos(std::min(1.0,std::abs(n[2])))*180/std::acos(-1.),0,'f',3).arg(source));
m_normalStatus->setToolTip(m_activeNormal.isEmpty()?m_calibrationPath:QJsonDocument(m_activeNormal).toJson(QJsonDocument::Indented));
}catch(const std::exception& e){m_normalStatus->setText(sanitizeUserVisibleLog(QString::fromUtf8(e.what())));}
}
QJsonObject MeasurementOptionsPanel::traySetupState() const {
QJsonObject state{{"schema","measurement_guidance_setup_v1"},{"ui_revision","multi_tray_v1"},{"tray_id",m_currentTrayId},{"normal_required",m_normalRequired},{"active_normal",m_activeNormal},{"normal_samples",m_normalSamples},{"height_method",selectedHeightMethod()},{"height_source",usesTeachPlane()?1:0},{"CAD_axial_offset_mm",m_offset->value()},{"board_description",m_boardDescription->text()}};
if(m_activeTeach.size()==6){
state["active_teach_XYZABC_mm_deg"]=m_activeTeach;
state["teach_reference"]=QJsonObject{{"XYZ_mm",QJsonArray{m_activeTeach[0],m_activeTeach[1],m_activeTeach[2]}},{"XYZABC_mm_deg",m_activeTeach},{"active",true},{"activation_method","save_teach_button"},{"operator_verified_grasp_height",false},{"is_ground_truth",false},{"frame","world"},{"tool","TOOL1"}};
}
try{auto draft=fields(m_planePoint,3);auto abc=fields(m_planeAngles,3);for(auto angle:abc)draft.append(angle);state["teach_input_XYZABC_mm_deg"]=draft;}catch(...){}
try{state["last_board_photo_robot_XYZABC_mm_deg"]=fields(m_boardRobot,6);}catch(...){}
return state;
}
void MeasurementOptionsPanel::saveSetup(){
if(m_loadingSetup)return;
const auto profile=traySetupState();
auto profiles=m_trayProfiles;profiles[QString::number(m_currentTrayId)]=profile;
// Keep the selected profile at the top level for existing measurement consumers.
auto document=profile;document["trays"]=profiles;document["selected_tray_id"]=m_currentTrayId;
document["saved_utc"]=QDateTime::currentDateTimeUtc().toString(Qt::ISODateWithMs);
if(!QDir().mkpath(QFileInfo(m_setupFile).absolutePath()))throw std::runtime_error("无法创建料盘设置目录。");
const auto data=QJsonDocument(document).toJson();QSaveFile file(m_setupFile);
if(!file.open(QIODevice::WriteOnly)||file.write(data)!=data.size()||!file.commit())throw std::runtime_error("料盘设置保存失败。");
m_trayProfiles=profiles;
}
void MeasurementOptionsPanel::restoreTraySetup(const QJsonObject& state){
auto active=state["active_normal"].toObject();if(!active.isEmpty())normal(active["normal_world"].toArray());
auto validate=[](const QJsonArray& a){for(auto v:a)if(!v.isDouble()||!std::isfinite(v.toDouble()))throw std::runtime_error("已保存的位姿数值无效。");};
auto reference=state["teach_reference"].toObject();auto teaching=state["active_teach_XYZABC_mm_deg"].toArray();
if(teaching.isEmpty()&&reference["operator_verified_grasp_height"].toBool()&&reference["frame"].toString()=="world"&&reference["tool"].toString()=="TOOL1"){
teaching=reference["XYZABC_mm_deg"].toArray();if(teaching.isEmpty()){teaching=reference["XYZ_mm"].toArray();for(auto angle:state["teach_ABC_deg"].toArray())teaching.append(angle);}
}
if(!teaching.isEmpty()&&teaching.size()!=6)throw std::runtime_error("示教位姿必须包含XYZABC。");validate(teaching);
auto draft=state["teach_input_XYZABC_mm_deg"].toArray();
if(draft.isEmpty()){draft=reference["XYZABC_mm_deg"].toArray();if(draft.isEmpty())draft=teaching;}
validate(draft);if(!draft.isEmpty()&&draft.size()!=6)throw std::runtime_error("示教输入必须包含XYZABC。");
auto photo=state["last_board_photo_robot_XYZABC_mm_deg"].toArray();validate(photo);
QString method=state["height_method"].toString("keep_visual_xy_solve_z");
const auto revision=state["ui_revision"].toString();
if(!state.isEmpty()&&revision!="multi_tray_v1"&&revision!="one_teach_plane_v2"&&revision!="one_teach_plane_v3"&&state["height_source"].toInt()!=1)method="visual_3d";
const int methodIndex=m_heightMethod->findData(method);if(methodIndex<0)throw std::runtime_error("已保存的Z计算方式无效。");
m_loadingSetup=true;
m_activeNormal=active;m_normalSamples=state["normal_samples"].toArray();m_normalRequired=state["normal_required"].toBool(true);m_activeTeach=teaching;
for(int i=0;i<3;++i){m_planePoint[i]->setText(draft.size()==6?QString::number(draft[i].toDouble(),'f',3):QString());m_planeAngles[i]->setText(draft.size()==6?QString::number(draft[i+3].toDouble(),'f',3):QString());}
for(int i=0;i<6;++i)m_boardRobot[i]->setText(photo.size()==6?QString::number(photo[i].toDouble(),'f',3):QString());
m_heightMethod->setCurrentIndex(methodIndex);m_offset->setValue(state["CAD_axial_offset_mm"].toDouble(4.4));
auto description=state["board_description"].toString();if(!QFileInfo::exists(description))description=QDir(rootPath()).filePath("VisionGuideData/LD-HG75-13x15.cpd");m_boardDescription->setText(description);
m_loadingSetup=false;
}
void MeasurementOptionsPanel::loadSetup(){
if(!QFile::exists(m_setupFile))return;
try{
auto document=readJson(m_setupFile);if(document["schema"].toString()!="measurement_guidance_setup_v1")throw std::runtime_error("料盘设置格式不支持。");
const bool legacy=!document.contains("trays");auto profiles=document["trays"].toObject();
int id=legacy?1:document["selected_tray_id"].toInt(1);
if(id<1||id>99)throw std::runtime_error("料盘编号无效。");
if(legacy){document["normal_required"]=document["normal_required"].toBool(false);profiles["1"]=document;}
if(!profiles[QString::number(id)].isObject())throw std::runtime_error("当前料盘设置不存在。");
restoreTraySetup(profiles[QString::number(id)].toObject());m_trayProfiles=profiles;m_currentTrayId=id;
{QSignalBlocker blocker(m_traySelector);m_traySelector->setCurrentIndex(m_traySelector->findData(id));}
if(legacy)saveSetup();
}catch(const std::exception& e){m_loadingSetup=false;m_status->setText(QString::fromUtf8("料盘设置未恢复:%1").arg(sanitizeUserVisibleLog(QString::fromUtf8(e.what()))));}
}
void MeasurementOptionsPanel::selectTray(){
if(m_loadingSetup)return;
const int next=m_traySelector->currentData().toInt();if(next==m_currentTrayId)return;
const auto oldId=m_currentTrayId;const auto oldProfiles=m_trayProfiles;const auto oldState=traySetupState();
if(m_busy){QSignalBlocker blocker(m_traySelector);m_traySelector->setCurrentIndex(m_traySelector->findData(oldId));return;}
try{
auto profiles=m_trayProfiles;profiles[QString::number(oldId)]=oldState;
const auto state=profiles[QString::number(next)].toObject();restoreTraySetup(state);
m_currentTrayId=next;m_trayProfiles=profiles;saveSetup();
// An image/board observation belongs to the tray selected when captured.
m_record={};m_snapshot={};m_resultImage={};m_boardRecord={};m_boardCapture={};m_values->clearContents();m_boardValues->clearContents();m_boardStatus->clear();m_saved->clear();m_source->setText(QString::fromUtf8("来源:—"));
invalidateGraspTargets();updateSetupStatus();setBusy(false);
m_status->setText(QString::fromUtf8("当前料盘:%1").arg(next));
}catch(const std::exception& e){
m_trayProfiles=oldProfiles;m_currentTrayId=oldId;restoreTraySetup(oldState);
{QSignalBlocker blocker(m_traySelector);m_traySelector->setCurrentIndex(m_traySelector->findData(oldId));}
updateSetupStatus();m_status->setText(sanitizeUserVisibleLog(QString::fromUtf8(e.what())));
}
}
void MeasurementOptionsPanel::importTeachReference(){
if(m_busy)return;
const QString path=QFileDialog::getOpenFileName(this,QString::fromUtf8("读取抓取示教记录"),QDir(rootPath()).filePath("user_data/VisionGuidanceCapture"),"JSON (*.json)");if(path.isEmpty())return;
try{
auto record=readJson(path);QJsonArray pose;
if(record["type"].toString()=="desired_grasp_teach")pose=record["teach_robot"].toObject()["XYZABC_mm_deg"].toArray();
else if(record["schema"].toString()=="offline_grasp_analysis_v1")pose=record["teach_reference_XYZABC_mm_deg"].toArray();
if(pose.size()!=6)throw std::runtime_error("请选择抓取示教记录,拍照机器人位姿不能作为抓取示教基准。");
for(auto v:pose)if(!v.isDouble()||!std::isfinite(v.toDouble()))throw std::runtime_error("示教位姿包含无效数值。");
for(int i=0;i<3;++i){m_planePoint[i]->setText(QString::number(pose[i].toDouble(),'f',3));m_planeAngles[i]->setText(QString::number(pose[i+3].toDouble(),'f',3));}
m_status->setText(QString::fromUtf8("示教位姿已导入,请核对后保存。"));updateSetupStatus();
}catch(const std::exception& e){m_status->setText(sanitizeUserVisibleLog(QString::fromUtf8(e.what())));}
}
void MeasurementOptionsPanel::saveTeachReference(){
if(m_busy)return;
const auto previous=m_activeTeach;
try{
auto pose=fields(m_planePoint,3);auto abc=fields(m_planeAngles,3);for(auto angle:abc)pose.append(angle);
bool nonzero=false;for(auto v:pose)nonzero|=std::abs(v.toDouble())>1e-9;if(!nonzero)throw std::runtime_error("示教位姿不能全为0。");
auto cal=effectiveCalibration();normal(cal["tray_normal_world"].toArray());
m_activeTeach=pose;saveSetup();invalidateGraspTargets();updateSetupStatus();m_status->setText(QString::fromUtf8("料盘 %1 的示教位姿已保存。").arg(m_currentTrayId));
emit logMessage(QString::fromUtf8("[测量][示教基准] %1").arg(QString::fromUtf8(QJsonDocument(QJsonObject{{"tray_id",m_currentTrayId},{"XYZABC_mm_deg",pose},{"normal_world",cal["tray_normal_world"]},{"height_method",selectedHeightMethod()},{"is_ground_truth",false}}).toJson(QJsonDocument::Compact))));
}catch(const std::exception& e){m_activeTeach=previous;updateSetupStatus();m_status->setText(sanitizeUserVisibleLog(QString::fromUtf8(e.what())));}
}
void MeasurementOptionsPanel::measureBoard(bool offline){
if(m_busy)return;
if(m_legacy&&m_legacy->isBusy()){m_boardStatus->setText(QString::fromUtf8("另一个测量入口仍在运行,请等待结束。"));return;}
if(offline){auto path=QFileDialog::getOpenFileName(this,QString::fromUtf8("选择左相机标定板原图"),QDir(rootPath()).filePath("bin/calib_images"),"Images (*.png *.jpg *.jpeg *.bmp *.tif)");if(path.isEmpty())return;measureBoardImage(QImage(path),path);}
else{
if(!m_camera||!m_camera->leftConnected()){m_boardStatus->setText(QString::fromUtf8("请先连接左相机。"));return;}
if(!m_matcher||!m_matcher->isCamLoaded(CAM_LEFT)){m_boardStatus->setText(QString::fromUtf8("请先加载左相机内参。"));return;}
if(m_legacy&&!m_legacy->beginExternalMeasurement(MeasurementCaptureSnapshot()))return;
setBusy(true);const auto begin=QDateTime::currentMSecsSinceEpoch();
try{auto image=m_camera->captureLeft();measureBoardImage(image,"left_camera",begin,QDateTime::currentMSecsSinceEpoch(),true);}
catch(const std::exception& e){if(m_legacy)m_legacy->endExternalMeasurement();setBusy(false);m_boardStatus->setText(sanitizeUserVisibleLog(QString::fromUtf8(e.what())));}
}
}
void MeasurementOptionsPanel::measureBoardImage(const QImage& image,const QString& source,qint64 captureBeginMs,qint64 captureEndMs,bool measurementLocked){
if(m_busy&&!measurementLocked)return;
bool ownsLock=measurementLocked;
m_boardRecord={};m_boardCapture={};m_boardValues->clearContents();m_applyNormal->setEnabled(false);
try{
if(image.isNull())throw std::runtime_error("标定板原图为空。");
if(!m_matcher||!m_matcher->isCamLoaded(CAM_LEFT))throw std::runtime_error("请先加载左相机内参;测板不需要零件轮廓库或YOLO。");
const QString cameraFile=m_matcher->cameraParamFile(CAM_LEFT),description=m_boardDescription->text().trimmed();
const auto cameraBytes=bytes(cameraFile),descriptionBytes=bytes(description);
MeasurementCaptureSnapshot capture;capture.leftImage=image;capture.leftCal=cameraFile;capture.captureBeginMs=captureBeginMs;capture.captureEndMs=captureEndMs;
if(!measurementLocked&&m_legacy&&!m_legacy->beginExternalMeasurement(capture))return;ownsLock=true;
m_snapshot={};m_record={};m_resultImage={};m_values->clearContents();m_boardRecord={};m_boardCapture={};m_boardValues->clearContents();invalidateGraspTargets();
const auto generation=++m_generation;m_timedOut=false;setBusy(true);m_boardStatus->setText(QString::fromUtf8("正在测量HG75…"));m_status->setText(QString::fromUtf8("正在测量标定板…"));m_source->setText(QString::fromUtf8("来源:左相机HG75标定板"));
QJsonObject record{{"schema","measurement_board_normal_v1"},{"type","board_normal"},{"run_id",uniqueId()},{"image_source",source},{"left_camera_sha256",sha(cameraBytes)},{"left_camera_file",cameraFile},{"board_description_file",description},{"board_description_sha256",sha(descriptionBytes)},{"tray_height_used",false},{"capture_operation_utc",QDateTime::currentDateTimeUtc().toString(Qt::ISODateWithMs)},{"assets",QJsonObject{{"left_camera_base64",QString::fromLatin1(cameraBytes.toBase64())},{"board_description_base64",QString::fromLatin1(descriptionBytes.toBase64())}}}};
record["tray_id"]=m_currentTrayId;
record["capture_begin_epoch_ms"]=captureBeginMs>0?QJsonValue(double(captureBeginMs)):QJsonValue(QJsonValue::Null);record["capture_end_epoch_ms"]=captureEndMs>0?QJsonValue(double(captureEndMs)):QJsonValue(QJsonValue::Null);record["capture_timestamp_source"]=captureBeginMs>0?"software_capture_interval":"offline_image_original_exposure_time_not_recorded";
QPointer<MeasurementOptionsPanel> self(this);const auto tempRoot=QDir(rootPath()).filePath("user_data/MeasurementTests");QDir().mkpath(tempRoot);
auto worker=QThread::create([self,capture,record,cameraBytes,descriptionBytes,tempRoot,generation]{
BoardPoseEstimate result;QString failure;
try{
QTemporaryDir temporary(QDir(tempRoot).filePath(".board_XXXXXX"));if(!temporary.isValid())throw std::runtime_error("无法创建标定板计算目录。");
auto save=[&](const QString& name,const QByteArray& data){auto path=temporary.filePath(name);QSaveFile file(path);if(!file.open(QIODevice::WriteOnly)||file.write(data)!=data.size()||!file.commit())throw std::runtime_error("标定文件快照保存失败。");return path;};
result=estimateBoardPose(capture.leftImage,save("left.cal",cameraBytes),save("board.cpd",descriptionBytes));
}catch(const VisionGuideApi::HException& e){failure=sanitizeUserVisibleLog(QString::fromLocal8Bit(e.ErrorMessage().TextA()));}catch(const std::exception& e){failure=sanitizeUserVisibleLog(QString::fromUtf8(e.what()));}
if(self)QMetaObject::invokeMethod(self.data(),[self,result,capture,record,failure,generation]{if(self&&self->m_generation==generation)self->completeBoard(result,capture,record,failure);},Qt::QueuedConnection);
});
m_worker=worker;connect(worker,&QThread::finished,this,[this,worker]{if(m_worker==worker)m_worker=nullptr;if(m_legacy)m_legacy->endExternalMeasurement();setBusy(false);worker->deleteLater();});worker->start();
}catch(const std::exception& e){if(ownsLock){if(m_legacy)m_legacy->endExternalMeasurement();setBusy(false);}m_boardStatus->setText(sanitizeUserVisibleLog(QString::fromUtf8(e.what())));}
}
void MeasurementOptionsPanel::completeBoard(const BoardPoseEstimate& result,const MeasurementCaptureSnapshot& capture,QJsonObject record,const QString& failure){
record["status"]=failure.isEmpty()?"success":"failed";record["error"]=failure;record["board"]=result.data;record["result_image_format"]="JPEG";record["result_image_file"]=result.axesImage.isNull()?QString():"result.jpg";m_boardRecord=record;m_boardCapture=capture;
if(failure.isEmpty()){
auto pose=result.data["pose7_m_deg"].toArray();for(int i=0;i<6;++i)m_boardValues->setItem(0,i,new QTableWidgetItem(QString::number(pose[i].toDouble()*(i<3?1000:1),'f',3)));
m_boardStatus->setText(QString::fromUtf8("标记 %1 · 重投影RMS %2 px" ).arg(result.data["detected_mark_count"].toInt()).arg(result.data["reprojection_rms_px"].toDouble(),0,'f',4));
emit resultImage(QString::fromUtf8("测量_HG75坐标系_%1").arg(record["run_id"].toString()),result.axesImage);
}else m_boardStatus->setText(QString::fromUtf8("标定板测量失败:%1").arg(failure));
emit logMessage(QString::fromUtf8("[测量][标定板] run=%1 status=%2 %3").arg(record["run_id"].toString(),record["status"].toString(),failure));writeArchive(record,capture,result.axesImage,true);
}
void MeasurementOptionsPanel::applyBoardNormal(){
if(m_busy)return;
const auto oldNormal=m_activeNormal;const auto oldSamples=m_normalSamples;const bool oldRequired=m_normalRequired;
try{
if(m_boardRecord["status"].toString()!="success")throw std::runtime_error("请先成功测量标定板。");
if(m_boardRecord["tray_id"].toInt(m_currentTrayId)!=m_currentTrayId)throw std::runtime_error("标定板结果与当前料盘不一致,请重新测量。");
auto board=m_boardRecord["board"].toObject();double rms=board["reprojection_rms_px"].toDouble(99);if(!std::isfinite(rms)||rms>1.0||board["detected_mark_count"].toInt()<10)throw std::runtime_error("标定板标记不足或重投影RMS超过1 px,请重新拍摄清晰原图。");
const auto cameraSha=m_boardRecord["left_camera_sha256"].toString();if(m_matcher&&cameraSha!=m_matcher->cameraParamSha256(CAM_LEFT))throw std::runtime_error("标定板拍摄后左内参发生改变,请重新测量。");
auto cal=readJson(m_calibrationPath);auto sample=boardNormalWorld(board,fields(m_boardRobot,6),cal);auto n=normal(sample["normal_world"].toArray());
sample["tray_id"]=m_currentTrayId;sample["board_record_id"]=m_boardRecord["run_id"];sample["grasp_calibration_sha256"]=sha(bytes(m_calibrationPath));sample["left_camera_sha256"]=cameraSha;
if(!m_activeNormal.isEmpty()){
auto previous=normal(m_activeNormal["normal_world"].toArray());if(n.dot(previous)<0)n=-n;
if(std::acos(std::max(-1.,std::min(1.,n.dot(previous))))*180/std::acos(-1.)>1.)throw std::runtime_error("本次法向与当前料盘差超过1°。核对拍照位姿;若料盘已移动,请先清空设置。");
if(m_activeNormal["grasp_calibration_sha256"]!=sample["grasp_calibration_sha256"])throw std::runtime_error("手眼标定版本改变,请清空并重新建立料盘设置。");
if(m_activeNormal["left_camera_sha256"]!=sample["left_camera_sha256"])throw std::runtime_error("左相机内参版本改变,请清空并重新建立料盘设置。");
}
// Reapplying one observation updates its robot pairing; it does not increase weight.
QJsonArray samples;for(auto value:m_normalSamples)if(value.toObject()["board_record_id"]!=sample["board_record_id"])samples.append(value);
sample["normal_world"]=QJsonArray{n[0],n[1],n[2]};samples.append(sample);cv::Vec3d mean(0,0,0);for(auto value:samples)mean+=normal(value.toObject()["normal_world"].toArray());mean/=cv::norm(mean);
m_normalSamples=samples;m_activeNormal=sample;m_activeNormal["normal_world"]=QJsonArray{mean[0],mean[1],mean[2]};m_activeNormal["observation_count"]=samples.size();m_normalRequired=false;saveSetup();invalidateGraspTargets();updateSetupStatus();
m_boardRecord["photo_robot_XYZABC_mm_deg"]=sample["photo_robot_XYZABC_mm_deg"];m_boardRecord["photo_robot_pair_confirmed"]=true;m_boardRecord["photo_robot_pairing_method"]="manual_entry_at_save_normal";m_boardRecord["board_parallel_to_tray_assumed"]=true;m_boardRecord["normal_solution"]=sample;m_boardRecord["active_mean_normal_world"]=m_activeNormal["normal_world"];writeArchive(m_boardRecord,m_boardCapture,QImage(),false);
m_boardStatus->setText(QString::fromUtf8("料盘 %1 法向已保存(%2次观测)。").arg(m_currentTrayId).arg(samples.size()));emit logMessage(QString::fromUtf8("[测量][料盘法向] %1").arg(QString::fromUtf8(QJsonDocument(m_activeNormal).toJson(QJsonDocument::Compact))));
}catch(const std::exception& e){m_activeNormal=oldNormal;m_normalSamples=oldSamples;m_normalRequired=oldRequired;m_boardStatus->setText(sanitizeUserVisibleLog(QString::fromUtf8(e.what())));}
}
void MeasurementOptionsPanel::resetTraySetup(){
if(m_busy)return;
m_activeNormal={};m_normalSamples={};m_activeTeach={};m_normalRequired=true;m_boardRecord={};m_boardCapture={};for(auto field:m_planePoint)field->clear();for(auto field:m_planeAngles)field->clear();invalidateGraspTargets();
try{saveSetup();m_boardStatus->setText(QString::fromUtf8("料盘 %1 设置已清空。").arg(m_currentTrayId));}catch(const std::exception& e){m_boardStatus->setText(sanitizeUserVisibleLog(QString::fromUtf8(e.what())));}updateSetupStatus();setBusy(false);
}