Files
VisionGuide/source/MeasurementGuidanceSetup.cpp
T
2026-10-26 10:56:56 +08:00

318 lines
30 KiB
C++
Raw Blame History

This file contains ambiguous Unicode characters
This file contains Unicode characters that might be confused with other characters. If you think that this is intentional, you can safely ignore this warning. Use the Escape button to reveal them.
#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);
}