Files
VisionGuide/source/MeasurementOptionsPanel.cpp
T

546 lines
60 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 "YoloDetector.h"
#include <QScrollArea>
#include <QVBoxLayout>
#include <QFormLayout>
#include <QGridLayout>
#include <QGroupBox>
#include <QComboBox>
#include <QDoubleSpinBox>
#include <QSpinBox>
#include <QLineEdit>
#include <QLabel>
#include <QPushButton>
#include <QTableWidget>
#include <QHeaderView>
#include <QFileDialog>
#include <QMessageBox>
#include <QJsonDocument>
#include <QFileInfo>
#include <QSaveFile>
#include <QCryptographicHash>
#include <QDir>
#include <QDateTime>
#include <QUuid>
#include <QThread>
#include <QPointer>
#include <QTimer>
#include <QCoreApplication>
#include <QSignalBlocker>
#include <cmath>
#include <algorithm>
#include <stdexcept>
#include <limits>
#include <VisionGuideApi.h>
namespace {
cv::Matx44d cameraTransform(const VisionPoseResult& p) {
VisionGuideApi::HTuple pose, matrix;
pose.Append(p.tx);pose.Append(p.ty);pose.Append(p.tz);pose.Append(p.rx);pose.Append(p.ry);pose.Append(p.rz);pose.Append(p.poseType);
VisionGuideApi::PoseToHomMat3d(pose,&matrix);
cv::Matx44d out=cv::Matx44d::eye();
for(int r=0;r<3;++r)for(int c=0;c<4;++c)out(r,c)=matrix[r*4+c].D()*(c==3?1000.0:1.0);
return out;
}
cv::Matx44d jsonMatrix(const QJsonValue& value) {
auto rows=value.toArray();if(rows.size()!=4)throw std::runtime_error("Calibration matrix must be 4x4.");
cv::Matx44d out;
for(int r=0;r<4;++r){auto row=rows[r].toArray();if(row.size()!=4)throw std::runtime_error("Invalid calibration matrix.");for(int c=0;c<4;++c){if(!row[c].isDouble()||!std::isfinite(row[c].toDouble()))throw std::runtime_error("Non-finite calibration matrix.");out(r,c)=row[c].toDouble();}}
cv::Matx33d R;for(int r=0;r<3;++r)for(int c=0;c<3;++c)R(r,c)=out(r,c);
if(cv::norm(cv::Mat(R.t()*R-cv::Matx33d::eye()))>1e-5||std::abs(cv::determinant(cv::Mat(R))-1)>1e-5||cv::norm(cv::Mat(out.row(3)-cv::Matx<double,1,4>(0,0,0,1)))>1e-8)throw std::runtime_error("Calibration rotation is not rigid/right-handed.");
return out;
}
cv::Matx44d robotTransform(const QJsonArray& xyzabc) {
if(xyzabc.size()!=6)throw std::runtime_error("Photograph robot XYZABC required.");
double v[6];for(int i=0;i<6;++i){if(!xyzabc[i].isDouble()||!std::isfinite(xyzabc[i].toDouble()))throw std::runtime_error("Invalid photograph robot pose.");v[i]=xyzabc[i].toDouble();}
const double k=std::acos(-1.0)/180;double a=v[3]*k,b=v[4]*k,c=v[5]*k;
cv::Matx33d rz(std::cos(a),-std::sin(a),0,std::sin(a),std::cos(a),0,0,0,1),ry(std::cos(b),0,std::sin(b),0,1,0,-std::sin(b),0,std::cos(b)),rx(1,0,0,0,std::cos(c),-std::sin(c),0,std::sin(c),std::cos(c));
auto R=rz*ry*rx;auto T=cv::Matx44d::eye();for(int i=0;i<3;++i){for(int j=0;j<3;++j)T(i,j)=R(i,j);T(i,3)=v[i];}return T;
}
QJsonArray array6(const VisionPoseResult& p){return {p.tx*1000,p.ty*1000,p.tz*1000,p.rx,p.ry,p.rz};}
QJsonArray rectJson(const QRect& r){return {r.x(),r.y(),r.width(),r.height()};}
QRect readRect(const QJsonValue& value){auto a=value.toArray();return a.size()==4?QRect(a[0].toInt(),a[1].toInt(),a[2].toInt(),a[3].toInt()):QRect();}
QString shaFile(const QString& path){QFile f(path);if(!f.open(QIODevice::ReadOnly))return {};QCryptographicHash hash(QCryptographicHash::Sha256);hash.addData(&f);return QString::fromLatin1(hash.result().toHex());}
QJsonObject paramsJson(const VisionGuideMatcher::Params& p){return {{"min_score",p.minScore},{"greediness",p.greediness},{"pose_refinement",p.poseRefinement},{"filter_enabled",p.useFilter},{"filter_type",p.filterType},{"filter_kernel",p.filterKernelSize},{"filter_sigma",p.filterSigma},{"auto_segmentation",p.useAutoSeg},{"generate_display_contours",p.generateDisplayContours},{"cov_pose_mode","standard_deviations"}};}
}
MeasurementSelection MeasurementOptionsPanel::selectPose(int mode,const VisionPoseResult& left,const VisionPoseResult& right,const StereoPoseFusion::FusionResult& f) {
MeasurementSelection out;
if(mode==0){out.pose=left;out.source=left.valid?"LEFT":"NONE";out.error=left.errorMsg;return out;}
if(mode==1){
if(left.valid&&right.valid&&f.valid&&f.consistent){out.pose=f.fused;out.source="FUSED";}
else if(left.valid&&right.valid){out.pose=left;out.source="LEFT_FALLBACK";out.error=QString::fromUtf8("双目一致性未通过,按设置保留左路位姿。");}
else out.error=QString::fromUtf8("严格双目未通过:需要两路有效、同目标且一致性合格。");
return out;
}
if(left.valid&&right.valid&&!f.consistent){out.pose=left;out.source="LEFT_FALLBACK";out.error=QString::fromUtf8("双目一致性未通过,按设置保留左路位姿。");return out;}
if(!f.valid){out.error=f.message;return out;}
out.pose=f.fused;
out.source=left.valid&&right.valid&&f.consistent?"FUSED":(f.weightRight>f.weightLeft?"RIGHT_FALLBACK":"LEFT_FALLBACK");
return out;
}
QRect MeasurementOptionsPanel::selectTargetRoi(const QVector<YoloResult>& parts,const QSize& imageSize,int selectionMode,const QRect& hint){
if(imageSize.isEmpty())return {};
QVector<YoloResult> candidates;
const QRectF bounds(QPointF(0,0),QSizeF(imageSize));
for(auto part:parts){
if(!std::isfinite(part.box.x())||!std::isfinite(part.box.y())||!std::isfinite(part.box.width())||!std::isfinite(part.box.height())||!std::isfinite(part.conf))continue;
part.box=part.box.intersected(bounds);
if(part.box.width()>=10&&part.box.height()>=10)candidates.append(part);
}
std::stable_sort(candidates.begin(),candidates.end(),[](const auto& a,const auto& b){return a.conf>b.conf;});
int index=0;
if(hint.isValid()){
double best=0;index=-1;
for(int i=0;i<candidates.size();++i){auto overlap=candidates[i].box.intersected(QRectF(hint));double area=overlap.isEmpty()?0:overlap.width()*overlap.height();if(area>best){best=area;index=i;}}
}else if(selectionMode==0){
const QPointF center(imageSize.width()*.5,imageSize.height()*.5);double best=std::numeric_limits<double>::max();
for(int i=0;i<candidates.size();++i){auto delta=candidates[i].box.center()-center;double distance=delta.x()*delta.x()+delta.y()*delta.y();if(distance<best){best=distance;index=i;}}
}
if(index<0||index>=candidates.size())return {};
auto box=candidates[index].box;const auto padding=QSizeF(box.width()*.10,box.height()*.10);
box.adjust(-padding.width(),-padding.height(),padding.width(),padding.height());
return box.toAlignedRect().intersected(QRect(QPoint(0,0),imageSize));
}
QJsonObject MeasurementOptionsPanel::poseJson(const VisionPoseResult& p) {
QJsonObject out{{"valid",p.valid},{"XYZRXRYRZ_mm_deg",array6(p)},{"pose_type",p.poseType},{"score",p.score},{"core_ms",p.matchTimeMs},{"preprocess_ms",p.preprocessTimeMs},{"parse_projection_ms",p.parseTimeMs},{"std_xyz_mm",QJsonArray{p.stdTx,p.stdTy,p.stdTz}},{"error",p.errorMsg}};
if(p.valid){auto T=cameraTransform(p);QJsonArray matrix;for(int r=0;r<3;++r)for(int c=0;c<4;++c)matrix.append(T(r,c)/(c==3?1000.0:1.0));out["T_camera_object_row_major_3x4_m"]=matrix;out["pose7_m_deg"]=QJsonArray{p.tx,p.ty,p.tz,p.rx,p.ry,p.rz,p.poseType};}
return out;
}
MeasurementSelection MeasurementOptionsPanel::enforceDeadline(MeasurementSelection selection,qint64 elapsedMs,qint64 deadlineMs){if(elapsedMs>=deadlineMs){selection.pose.valid=false;selection.source="NONE";selection.error=QString::fromUtf8("超过截止时间,迟到结果不发布。");}return selection;}
VisionPoseResult MeasurementOptionsPanel::offsetPose(const VisionPoseResult& pose,double offsetMm){
auto out=pose;if(!pose.valid)return out;if(!std::isfinite(offsetMm))throw std::runtime_error("Invalid CAD offset");
auto transform=cameraTransform(pose);out.tx+=offsetMm*transform(0,2)/1000;out.ty+=offsetMm*transform(1,2)/1000;out.tz+=offsetMm*transform(2,2)/1000;return out;
}
QJsonObject MeasurementOptionsPanel::graspTarget(const VisionPoseResult& pose,const QJsonArray& robot,const QJsonObject& calibration,double offsetMm) {
if(!pose.valid)throw std::runtime_error("No valid camera pose.");
if(calibration["robot_rotation_rule"].toString()!="Rz(A) Ry(B) Rx(C)"||calibration["robot_tool"].toString()!="TOOL1"||calibration["robot_reference_frame_human_confirmed"].toString()!="world")throw std::runtime_error("Only verified world/TOOL1 Rz(A)Ry(B)Rx(C) calibration is accepted.");
auto H=jsonMatrix(calibration["T_TOOL1_camera_mm"]),D=jsonMatrix(calibration["T_grasp_TOOL1_from_one_teach_mm"]);
auto P=robotTransform(robot)*H*cameraTransform(pose);
auto n=calibration["tray_normal_world"].toArray();if(n.size()!=3)throw std::runtime_error("Tray normal missing.");
cv::Vec3d z(n[0].toDouble(),n[1].toDouble(),n[2].toDouble());if(cv::norm(z)<.9||!std::isfinite(cv::norm(z)))throw std::runtime_error("Invalid normal.");z/=cv::norm(z);
auto teach=robotTransform(calibration["teach_reference_XYZABC_mm_deg"].toArray());cv::Vec3d tz(teach(0,2),teach(1,2),teach(2,2));if(z.dot(tz)<0)z=-z;
cv::Vec3d a(P(0,2),P(1,2),P(2,2));auto x=a-z*z.dot(a);if(cv::norm(x)<.05)throw std::runtime_error("Part axis and normal are degenerate.");x/=cv::norm(x);cv::Vec3d tx(teach(0,0),teach(1,0),teach(2,0));if(x.dot(tx)<0)x=-x;auto y=z.cross(x);y/=cv::norm(y);x=y.cross(z);
if(!std::isfinite(offsetMm))throw std::runtime_error("Invalid CAD offset");
auto G=cv::Matx44d::eye();for(int i=0;i<3;++i){G(i,0)=x[i];G(i,1)=y[i];G(i,2)=z[i];G(i,3)=P(i,3)+offsetMm*a[i];}auto target=G*D;
const double k=180/std::acos(-1.0);double abc[3]={std::atan2(target(1,0),target(0,0))*k,std::atan2(-target(2,0),std::hypot(target(0,0),target(1,0)))*k,std::atan2(target(2,1),target(2,2))*k};auto tv=calibration["teach_reference_XYZABC_mm_deg"].toArray();
for(int i=0;i<3;++i)abc[i]+=360*std::round((tv[i+3].toDouble()-abc[i])/360);
return {{"XYZABC_mm_deg",QJsonArray{target(0,3),target(1,3),target(2,3),abc[0],abc[1],abc[2]}},{"frame","world"},{"tool","TOOL1"},{"CAD_axial_offset_mm",offsetMm},{"fixed_grasp_relationship_D_applied",true},{"additional_height_compensation_mm",0},{"teach_is_ground_truth",false},{"robot_rotation_rule","Rz(A) Ry(B) Rx(C)"},{"Z_source","visual_3d"},{"height_constraint_applied",false}};
}
QJsonObject MeasurementOptionsPanel::graspTargets(const MeasurementCaptureSnapshot& s,const QJsonArray& robot,const QJsonObject& cal,double offsetMm){
QJsonObject out;
if(s.leftPose.valid)out["left"]=graspTarget(s.leftPose,robot,cal,offsetMm);
if(s.fusion.rightInLeft.valid)out["right_in_left"]=graspTarget(s.fusion.rightInLeft,robot,cal,offsetMm);
if(s.leftPose.valid&&s.rightPose.valid&&s.fusion.valid&&s.fusion.consistent)out["fused"]=graspTarget(s.fusion.fused,robot,cal,offsetMm);
return out;
}
QJsonObject MeasurementOptionsPanel::graspTargetOnPlane(const VisionPoseResult& pose,const QJsonArray& robot,const QJsonObject& cal,const QJsonArray& reference,double offsetMm){
if(reference.size()!=3)throw std::runtime_error("Fill validated world/TOOL1 reference XYZ");
cv::Vec3d ref;for(int i=0;i<3;++i){if(!reference[i].isDouble()||!std::isfinite(reference[i].toDouble()))throw std::runtime_error("Invalid plane reference XYZ");ref[i]=reference[i].toDouble();}
auto out=graspTarget(pose,robot,cal,offsetMm);auto nominal=out["XYZABC_mm_deg"].toArray();
auto worldCamera=robotTransform(robot)*jsonMatrix(cal["T_TOOL1_camera_mm"]);auto object=worldCamera*cameraTransform(pose);
cv::Vec3d camera(worldCamera(0,3),worldCamera(1,3),worldCamera(2,3)),observed(object(0,3),object(1,3),object(2,3));
cv::Vec3d ray=observed-camera,toolOffset(nominal[0].toDouble()-observed[0],nominal[1].toDouble()-observed[1],nominal[2].toDouble()-observed[2]);
auto nv=cal["tray_normal_world"].toArray();cv::Vec3d normal(nv[0].toDouble(),nv[1].toDouble(),nv[2].toDouble());normal/=cv::norm(normal);
const double denominator=normal.dot(ray);if(std::abs(denominator)<1e-4*cv::norm(ray))throw std::runtime_error("Camera ray parallel to grasp plane");
const double scale=normal.dot(ref-camera-toolOffset)/denominator;
if(!std::isfinite(scale)||scale<=0)throw std::runtime_error("Reference grasp plane is behind camera");
auto target=camera+scale*ray+toolOffset;QJsonArray corrected=nominal,delta;
for(int i=0;i<3;++i){corrected[i]=target[i];delta.append(target[i]-nominal[i].toDouble());}
out["XYZABC_mm_deg"]=corrected;out["unconstrained_visual_XYZABC_mm_deg"]=nominal;
out["Z_source"]="validated_equal_height_grasp_plane";out["height_constraint_applied"]=true;
out["validated_plane_reference_world_TOOL1_XYZ_mm"]=reference;out["plane_normal_world"]=nv;
out["plane_correction_world_XYZ_mm"]=delta;out["camera_ray_scale"]=scale;out["height_method"]="camera_ray_plane_intersection";
return out;
}
QJsonObject MeasurementOptionsPanel::graspTargetKeepingXY(const VisionPoseResult& pose,const QJsonArray& robot,const QJsonObject& cal,const QJsonArray& reference,double offsetMm){
if(reference.size()!=3)throw std::runtime_error("请填写示教基准XYZ。");
double ref[3];for(int i=0;i<3;++i){if(!reference[i].isDouble()||!std::isfinite(reference[i].toDouble()))throw std::runtime_error("示教基准XYZ无效。");ref[i]=reference[i].toDouble();}
auto out=graspTarget(pose,robot,cal,offsetMm);auto nominal=out["XYZABC_mm_deg"].toArray(),corrected=nominal;
auto n=cal["tray_normal_world"].toArray();const double nz=n[2].toDouble();
if(std::abs(nz)<1e-4)throw std::runtime_error("当前平面接近竖直,无法保持XY求Z。");
double z=ref[2]-(n[0].toDouble()*(nominal[0].toDouble()-ref[0])+n[1].toDouble()*(nominal[1].toDouble()-ref[1]))/nz;
if(!std::isfinite(z))throw std::runtime_error("平面高度计算结果无效。");corrected[2]=z;
out["XYZABC_mm_deg"]=corrected;out["unconstrained_visual_XYZABC_mm_deg"]=nominal;
out["Z_source"]="validated_equal_height_grasp_plane";out["height_constraint_applied"]=true;out["height_method"]="keep_visual_xy_solve_z";
out["validated_plane_reference_world_TOOL1_XYZ_mm"]=reference;out["plane_normal_world"]=n;
out["plane_correction_world_XYZ_mm"]=QJsonArray{0,0,z-nominal[2].toDouble()};return out;
}
QJsonObject MeasurementOptionsPanel::boardNormalWorld(const QJsonObject& board,const QJsonArray& robot,const QJsonObject& cal){
if(cal["robot_rotation_rule"].toString()!="Rz(A) Ry(B) Rx(C)"||cal["robot_tool"].toString()!="TOOL1"||cal["robot_reference_frame_human_confirmed"].toString()!="world")throw std::runtime_error("标定板转换需要已验证的世界系/TOOL1转换关系。");
auto values=board.value("pose7_m_deg").toArray();if(values.size()!=7)throw std::runtime_error("缺少标定板原始位姿。");
for(auto v:values)if(!v.isDouble()||!std::isfinite(v.toDouble()))throw std::runtime_error("标定板位姿不是有限数值。");
VisionPoseResult pose;pose.valid=true;pose.tx=values[0].toDouble();pose.ty=values[1].toDouble();pose.tz=values[2].toDouble();pose.rx=values[3].toDouble();pose.ry=values[4].toDouble();pose.rz=values[5].toDouble();pose.poseType=values[6].toInt();
auto transform=robotTransform(robot)*jsonMatrix(cal["T_TOOL1_camera_mm"])*cameraTransform(pose);cv::Vec3d n(transform(0,2),transform(1,2),transform(2,2));n/=cv::norm(n);
auto previous=cal["tray_normal_world"].toArray();if(previous.size()==3&&n.dot(cv::Vec3d(previous[0].toDouble(),previous[1].toDouble(),previous[2].toDouble()))<0)n=-n;
return {{"normal_world",QJsonArray{n[0],n[1],n[2]}},{"photo_robot_XYZABC_mm_deg",robot},{"normal_source","HG75_board_Z_axis"},{"tray_height_used",false},{"frame","world"},{"tool","TOOL1"}};
}
MeasurementOptionsPanel::MeasurementOptionsPanel(CameraPanel* camera,YoloDetector* yolo,VisionGuideMatcher* matcher,StereoPoseFusion* fusion,MeasurePanel* legacy,QWidget* parent,const QString& setupFile)
:QWidget(parent),m_camera(camera),m_yolo(yolo),m_matcher(matcher),m_fusion(fusion),m_legacy(legacy) {
m_setupFile=setupFile.isEmpty()?QDir(rootPath()).filePath("user_data/MeasurementGuidance/tray_setup.json"):setupFile;
setObjectName("measurementOptionsPanel");auto outer=new QVBoxLayout(this);auto scroll=new QScrollArea(this);scroll->setWidgetResizable(true);auto body=new QWidget;auto layout=new QVBoxLayout(body);outer->addWidget(scroll);scroll->setWidget(body);
m_calibrationPath=QDir(rootPath()).filePath("analysis/20261013_capture_analysis/grasp_results.json");
auto trayRow=new QFormLayout;
m_traySelector=new QComboBox;m_traySelector->setObjectName("measurementTrayId");
for(int id=1;id<=99;++id)m_traySelector->addItem(QString::number(id),id);
trayRow->addRow(QString::fromUtf8("料盘编号"),m_traySelector);layout->addLayout(trayRow);
createTraySetupControls(layout);
createTeachReferenceControls(layout);
auto options=new QGroupBox(QString::fromUtf8("测量设置"));auto form=new QFormLayout(options);
m_mode=new QComboBox;m_mode->setObjectName("measurementCameraMode");m_mode->addItems({QString::fromUtf8("左相机"),QString::fromUtf8("双目(两路有效,不一致保留左路)"),QString::fromUtf8("双目(允许单侧,不一致保留左路)")});
m_refinement=new QComboBox;m_refinement->setObjectName("measurementRefinement");m_refinement->addItem(QString::fromUtf8("标准"),"least_squares");m_refinement->addItem(QString::fromUtf8("高"),"least_squares_high");m_refinement->addItem(QString::fromUtf8("很高"),"least_squares_very_high");m_refinement->addItem(QString::fromUtf8("无精细化(粗定位)"),"none");
m_deadline=new QSpinBox;m_deadline->setRange(1,120);m_deadline->setValue(5);m_deadline->setSuffix(QString::fromUtf8(" 秒"));
m_minScore=new QDoubleSpinBox;m_minScore->setRange(.1,.99);m_minScore->setDecimals(2);m_minScore->setSingleStep(.05);m_minScore->setValue(.60);
m_greediness=new QDoubleSpinBox;m_greediness->setRange(0,1);m_greediness->setDecimals(2);m_greediness->setSingleStep(.05);m_greediness->setValue(.85);
m_yoloSelection=new QComboBox;m_yoloSelection->setObjectName("measurementYoloSelection");m_yoloSelection->addItems({QString::fromUtf8("中心选择"),QString::fromUtf8("置信度选择")});
m_yoloSelection->setToolTip(QString::fromUtf8("中心选择:零件框中心最靠近图像中心;置信度选择:零件置信度最高。双目右路优先选左路对应目标。"));
m_posePoint=new QComboBox;m_posePoint->setObjectName("measurementPosePoint");m_posePoint->addItems({QString::fromUtf8("模型原点"),QString::fromUtf8("偏移抓取点")});
m_offset=new QDoubleSpinBox;m_offset->setObjectName("measurementCadOffset");m_offset->setRange(-100,100);m_offset->setDecimals(3);m_offset->setSingleStep(.1);m_offset->setValue(m_legacy?m_legacy->currentGraspOffsetMm():4.4);m_offset->setSuffix(" mm");m_offset->setToolTip(QString::fromUtf8("沿零件模型+Z轴移动抓取点;世界抓取计算应用一次该偏移,与相机位姿显示选项无关。"));
form->addRow(QString::fromUtf8("相机模式"),m_mode);form->addRow(QString::fromUtf8("精细化档位"),m_refinement);form->addRow(QString::fromUtf8("截止时间"),m_deadline);form->addRow("MinScore",m_minScore);form->addRow("Greediness",m_greediness);form->addRow(QString::fromUtf8("YOLO选择模式"),m_yoloSelection);layout->addWidget(options);
form->addRow(QString::fromUtf8("相机位姿显示"),m_posePoint);form->addRow(QString::fromUtf8("抓取点偏移(模型+Z)"),m_offset);
m_start=new QPushButton(QString::fromUtf8("开始测量"));m_start->setObjectName("measurementStart");layout->addWidget(m_start);m_status=new QLabel(QString::fromUtf8("就绪"));m_status->setWordWrap(true);m_source=new QLabel("来源:—");m_source->setWordWrap(true);layout->addWidget(m_status);layout->addWidget(m_source);
m_values=new QTableWidget(6,7);m_values->setObjectName("measurementValues");m_values->setHorizontalHeaderLabels({"X(mm)","Y(mm)","Z(mm)","RX/A(°)","RY/B(°)","RZ/C(°)","Score"});m_values->setVerticalHeaderLabels({QString::fromUtf8("左路"),QString::fromUtf8("右路→左系"),QString::fromUtf8("最终相机位姿"),QString::fromUtf8("左路抓取XYZABC"),QString::fromUtf8("右路抓取XYZABC"),QString::fromUtf8("融合抓取XYZABC")});m_values->setEditTriggers(QAbstractItemView::NoEditTriggers);m_values->horizontalHeader()->setSectionResizeMode(QHeaderView::ResizeToContents);m_values->setMinimumHeight(225);layout->addWidget(m_values);
auto robotBox=new QGroupBox(QString::fromUtf8("本次拍照位姿(世界系 / TOOL1)"));
auto grid=new QGridLayout(robotBox);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":"°")),0,i*2);
m_robot[i]=new QLineEdit;m_robot[i]->setMinimumWidth(55);
m_robot[i]->setObjectName(QString("measurementRobot%1").arg(labels[i]));
grid->addWidget(m_robot[i],0,i*2+1);grid->setColumnStretch(i*2+1,1);
}
m_grasp=new QPushButton(QString::fromUtf8("计算抓取 XYZABC"));layout->addWidget(robotBox);
auto resultBox=new QGroupBox(QString::fromUtf8("抓取结果"));
resultBox->setObjectName("measurementGraspResultGroup");
auto resultLayout=new QVBoxLayout(resultBox);
auto heightRow=new QFormLayout;
m_heightMethod=new QComboBox;
m_heightMethod->setObjectName("measurementHeightMethod");
m_heightMethod->addItem(QString::fromUtf8("XY推导Z"),"keep_visual_xy_solve_z");
m_heightMethod->addItem(QString::fromUtf8("视觉测量Z"),"visual_3d");
m_heightMethod->addItem(QString::fromUtf8("视线与平面求交(同时调整XYZ)"),"camera_ray_plane_intersection");
m_heightMethod->setToolTip(QString::fromUtf8("示教平面由抓取示教XYZ和料盘法向确定。视线求交还会调整XY。"));
heightRow->addRow(QString::fromUtf8("Z计算方式"),m_heightMethod);
resultLayout->addLayout(heightRow);
resultLayout->addWidget(m_grasp);
layout->addWidget(resultBox);
auto buttons=new QHBoxLayout;m_draw=new QPushButton(QString::fromUtf8("显示本次结果图"));m_draw->setObjectName("measurementDraw");m_save=new QPushButton(QString::fromUtf8("保存数据与图像"));m_save->setObjectName("measurementSave");buttons->addWidget(m_draw);buttons->addWidget(m_save);layout->addLayout(buttons);m_saved=new QLabel;m_saved->setWordWrap(true);layout->addWidget(m_saved);layout->addStretch();
connect(m_start,&QPushButton::clicked,this,&MeasurementOptionsPanel::start);connect(m_draw,&QPushButton::clicked,this,&MeasurementOptionsPanel::renderResult);connect(m_save,&QPushButton::clicked,this,&MeasurementOptionsPanel::saveData);connect(m_grasp,&QPushButton::clicked,this,&MeasurementOptionsPanel::calculateGrasp);
for(auto field:m_robot)connect(field,&QLineEdit::textChanged,this,[this]{invalidateGraspTargets();});
connect(m_posePoint,qOverload<int>(&QComboBox::currentIndexChanged),this,[this]{refreshCameraPoses();if(!m_busy&&(m_snapshot.leftPose.valid||m_snapshot.rightPose.valid))renderResult();});
connect(m_offset,qOverload<double>(&QDoubleSpinBox::valueChanged),this,[this]{if(m_loadingSetup)return;invalidateGraspTargets();refreshCameraPoses();if(!m_busy&&(m_snapshot.leftPose.valid||m_snapshot.rightPose.valid))renderResult();});
connect(m_heightMethod,qOverload<int>(&QComboBox::currentIndexChanged),this,[this]{
if(m_loadingSetup)return;
invalidateGraspTargets();updateSetupStatus();
try{saveSetup();}catch(const std::exception& e){m_status->setText(sanitizeUserVisibleLog(QString::fromUtf8(e.what())));}
});
for(auto field:m_planePoint)connect(field,&QLineEdit::textChanged,this,[this]{invalidateGraspTargets();updateSetupStatus();});
for(auto field:m_planeAngles)connect(field,&QLineEdit::textChanged,this,[this]{invalidateGraspTargets();});
connect(m_importTeach,&QPushButton::clicked,this,&MeasurementOptionsPanel::importTeachReference);connect(m_saveTeach,&QPushButton::clicked,this,&MeasurementOptionsPanel::saveTeachReference);
loadSetup();updateSetupStatus();
connect(m_traySelector,qOverload<int>(&QComboBox::currentIndexChanged),this,[this]{selectTray();});
setBusy(false);
}
MeasurementOptionsPanel::~MeasurementOptionsPanel(){if(m_worker){m_worker->wait();delete m_worker;m_worker=nullptr;}for(auto t:m_archiveWorkers){t->wait();delete t;}m_archiveWorkers.clear();if(m_busy&&m_legacy)m_legacy->endExternalMeasurement();}
QString MeasurementOptionsPanel::rootPath() const {QDir dir(QCoreApplication::applicationDirPath());if(dir.dirName()=="bin")dir.cdUp();if(QDir::current().exists("StereoCalib"))return QDir::currentPath();return dir.absolutePath();}
void MeasurementOptionsPanel::setBusy(bool busy){
m_busy=busy;for(auto widget:QList<QWidget*>{m_start,m_traySelector,m_mode,m_refinement,m_deadline,m_minScore,m_greediness,m_yoloSelection,m_posePoint,m_offset,m_heightMethod,m_importTeach,m_measureBoard,m_importBoard,m_resetSetup,m_boardDescription,m_browseDescription})widget->setEnabled(!busy);
for(auto field:m_robot)field->setEnabled(!busy);for(auto field:m_boardRobot)field->setEnabled(!busy);for(auto field:m_planePoint)field->setEnabled(!busy);for(auto field:m_planeAngles)field->setEnabled(!busy);
m_saveTeach->setEnabled(!busy);m_applyNormal->setEnabled(!busy&&m_boardRecord["status"].toString()=="success");
const bool any=m_snapshot.leftPose.valid||m_snapshot.fusion.rightInLeft.valid;m_draw->setEnabled(!busy&&any);m_save->setEnabled(!busy&&!m_saveBusy&&!m_record.isEmpty());m_grasp->setEnabled(!busy&&any&&!m_timedOut);emit busyChanged(busy);
}
void MeasurementOptionsPanel::fillPose(int row,const VisionPoseResult& p){auto values=array6(p);for(int c=0;c<6;++c)m_values->setItem(row,c,new QTableWidgetItem(p.valid?QString::number(values[c].toDouble(),'f',3):"—"));m_values->setItem(row,6,new QTableWidgetItem(p.valid?QString::number(p.score,'f',4):"—"));}
QJsonArray MeasurementOptionsPanel::robotInput() const {QJsonArray values;for(auto field:m_robot){bool ok=false;double v=field->text().trimmed().toDouble(&ok);if(!ok||!std::isfinite(v))throw std::runtime_error("Fill all six photograph robot values.");values.append(v);}bool allZero=true;for(auto v:values)if(std::abs(v.toDouble())>1e-9)allZero=false;if(allZero)throw std::runtime_error("All-zero photograph robot pose is invalid.");return values;}
void MeasurementOptionsPanel::refreshCameraPoses(){
const bool offset=m_posePoint->currentIndex()==1;const double mm=m_offset->value();
const VisionPoseResult raw[]={m_snapshot.leftPose,m_snapshot.fusion.rightInLeft,m_snapshot.finalPose};
for(int row=0;row<3;++row)fillPose(row,offset?offsetPose(raw[row],mm):raw[row]);
const QString point=offset?QString::fromUtf8("偏移点"):QString::fromUtf8("模型原点");
const QString names[]={QString::fromUtf8("左路"),QString::fromUtf8("右路→左系"),QString::fromUtf8("最终相机位姿")};
for(int row=0;row<3;++row)m_values->setVerticalHeaderItem(row,new QTableWidgetItem(names[row]+"·"+point));
if(!m_record.isEmpty()){
m_record["camera_pose_display_point"]=offset?"offset_grasp_point":"model_origin";m_record["CAD_axial_offset_mm"]=mm;
QJsonObject poses;const QString keys[]={"left","right_in_left","final"};for(int row=0;row<3;++row)if(raw[row].valid)poses[keys[row]]=poseJson(offsetPose(raw[row],mm));m_record["offset_camera_poses"]=poses;
}
}
void MeasurementOptionsPanel::start() {
if(m_busy||!m_matcher||!m_legacy)return;
if(m_legacy->isBusy()){m_status->setText(QString::fromUtf8("旧流程仍在运行,请等待结束。"));return;}
const int mode=m_mode->currentIndex();
if(!m_matcher->isReady(CAM_LEFT)||(mode!=0&&(!m_matcher->isReady(CAM_RIGHT)||!m_fusion||!m_fusion->isReady()))){m_status->setText(QString::fromUtf8("所选相机模式的模板/标定未就绪,请先在原页加载。"));return;}
if(!m_camera||(mode==0&&!m_camera->leftConnected())||(mode==1&&(!m_camera->leftConnected()||!m_camera->rightConnected()))||(mode==2&&!m_camera->leftConnected()&&!m_camera->rightConnected())){m_status->setText(QString::fromUtf8("请先连接所选相机。"));return;}
if(!m_yolo||!m_yolo->isLoaded()){m_status->setText(QString::fromUtf8("YOLO模型未加载。"));return;}
const quint64 generation=++m_generation;auto clock=std::make_shared<QElapsedTimer>();clock->start();
MeasurementCaptureSnapshot s;s.runId=static_cast<quint64>(QDateTime::currentMSecsSinceEpoch());s.parameters=m_legacy->currentVisionParameters();s.parameters.minScore=m_minScore->value();s.parameters.greediness=m_greediness->value();s.parameters.poseRefinement=m_refinement->currentData().toString();s.parameters.generateDisplayContours=true;s.leftCal=m_matcher->cameraParamFile(CAM_LEFT);s.rightCal=m_matcher->cameraParamFile(CAM_RIGHT);s.leftModel=m_matcher->shapeModelFile(CAM_LEFT);s.rightModel=m_matcher->shapeModelFile(CAM_RIGHT);s.stereoPoseFile=m_fusion?m_fusion->relPoseFile():QString();s.captureBeginMs=QDateTime::currentMSecsSinceEpoch();
if(!m_legacy->beginExternalMeasurement(s))return;
m_timedOut=false;m_record={};m_snapshot={};m_resultImage={};m_values->clearContents();m_status->setText(QString::fromUtf8("采图/读取原图中…"));m_source->setText("来源:—");setBusy(true);
const int deadlineMs=m_deadline->value()*1000;
const QString id=QDateTime::currentDateTime().toString("yyyyMMdd_HHmmss_zzz")+"_"+QUuid::createUuid().toString(QUuid::WithoutBraces).left(8);
QJsonObject record{{"schema","measurement_capture_v2"},{"run_id",id},{"generation",static_cast<double>(generation)},{"input_source","fresh_camera_images"},{"flow","standard"},{"yolo_selection",m_yoloSelection->currentIndex()==0?"center":"confidence"},{"camera_mode",mode==0?"left_only":(mode==1?"strict_stereo":"stereo_with_fallback")},{"parameters",paramsJson(s.parameters)},{"deadline_ms",deadlineMs},{"left_camera_sha256",m_matcher->cameraParamSha256(CAM_LEFT)},{"right_camera_sha256",m_matcher->cameraParamSha256(CAM_RIGHT)},{"left_camera_file",s.leftCal},{"right_camera_file",s.rightCal},{"left_model_file",s.leftModel},{"right_model_file",s.rightModel},{"stereo_pose_file",s.stereoPoseFile},{"model_origin_on_axis",true},{"additional_height_compensation_mm",0},{"photo_robot_pair_confirmed",false}};
QElapsedTimer capture;capture.start();
record["pipeline_revision"]="20261015_multi_tray";record["tray_id"]=m_currentTrayId;
if(m_camera->leftConnected())s.leftImage=m_camera->captureLeft();if(mode!=0&&m_camera->rightConnected())s.rightImage=m_camera->captureRight();s.captureEndMs=QDateTime::currentMSecsSinceEpoch();record["source_object_id"]="same_model_as_calibrated_part";
QJsonObject timings{{"capture_ms",static_cast<double>(capture.elapsed())}};record["capture_begin_utc"]=QDateTime::fromMSecsSinceEpoch(s.captureBeginMs,Qt::UTC).toString(Qt::ISODateWithMs);record["capture_end_utc"]=QDateTime::fromMSecsSinceEpoch(s.captureEndMs,Qt::UTC).toString(Qt::ISODateWithMs);
if((mode==0&&s.leftImage.isNull())||(mode==1&&(s.leftImage.isNull()||s.rightImage.isNull()))||(s.leftImage.isNull()&&s.rightImage.isNull())){record["timings_ms"]=timings;MeasurementSelection fail;fail.error=QString::fromUtf8("所选模式需要的原图采集/读取失败。");complete(s,record,fail);m_legacy->endExternalMeasurement();setBusy(false);return;}
const int selectionMode=m_yoloSelection->currentIndex();QPointer<MeasurementOptionsPanel> self(this);auto matcher=m_matcher;auto detector=m_yolo;auto fusion=m_fusion;
m_status->setText(QString::fromUtf8("后台计算中;精细化=%1").arg(s.parameters.poseRefinement));
emit logMessage(QString::fromUtf8("[测量][版本] 20261015_multi_tray"));
emit logMessage(QString::fromUtf8("[测量][预处理] 继承主页面设置 filter=%1 type=%2 kernel=%3 segmentation=%4").arg(s.parameters.useFilter).arg(s.parameters.filterType).arg(s.parameters.filterKernelSize).arg(s.parameters.useAutoSeg));
emit logMessage(QString::fromUtf8("[测量] run=%1 flow=%2 camera=%3 refinement=%4 minScore=%5 greediness=%6 deadline=%7ms").arg(id,record["flow"].toString(),record["camera_mode"].toString(),s.parameters.poseRefinement).arg(s.parameters.minScore).arg(s.parameters.greediness).arg(deadlineMs));
auto worker=QThread::create([self,s,record,timings,clock,mode,selectionMode,deadlineMs,matcher,detector,fusion,generation]()mutable{
MeasurementSelection selected;QElapsedTimer stage;
if(mode==0)s.rightPose.errorMsg="Not requested: fixed LEFT mode";
else if(s.rightImage.isNull())s.rightPose.errorMsg="RIGHT image unavailable";
try{
auto chooseRoi=[&](const QImage& image,const QRect& hint,bool right){
stage.restart();
// Match the established YOLO page's 640-pixel input path.
auto thumbnail=image.scaled(640,640,Qt::IgnoreAspectRatio,Qt::SmoothTransformation);
auto detections=detector->detect(thumbnail,.25f,.45f,-1);
QVector<YoloResult> parts;QJsonArray audit;
const double sx=double(image.width())/640,sy=double(image.height())/640;
for(auto d:detections){d.box=QRectF(d.box.x()*sx,d.box.y()*sy,d.box.width()*sx,d.box.height()*sy);
const bool isPart=detector->numClasses()!=3||d.classId==1||d.classId==2;
audit.append(QJsonObject{{"class_id",d.classId},{"label",d.label},{"confidence",d.conf},{"box_xywh",QJsonArray{d.box.x(),d.box.y(),d.box.width(),d.box.height()}},{"part_candidate",isPart}});
if(isPart)parts.append(d);
}
timings[right?"right_yolo_ms":"left_yolo_ms"]=static_cast<double>(stage.elapsed());
const QString side=right?"right":"left";record[side+"_detections"]=audit;
auto roi=MeasurementOptionsPanel::selectTargetRoi(parts,image.size(),selectionMode,right?hint:QRect());
QString error;if(roi.isEmpty())error=QString::fromUtf8("%1:YOLO检测%2个框、零件候选%3个%4,未获得有效ROI。").arg(right?"右路":"左路").arg(detections.size()).arg(parts.size()).arg(right&&hint.isValid()?QString::fromUtf8(",未找到与左路对应的目标"):QString());
if(right)s.rightPose.errorMsg=error;else s.leftPose.errorMsg=error;
const QString diagnostic=QString::fromUtf8("[测量][ROI] %1 检测=%2 零件=%3 ROI=(%4,%5,%6,%7) %8").arg(right?"右路":"左路").arg(detections.size()).arg(parts.size()).arg(roi.x()).arg(roi.y()).arg(roi.width()).arg(roi.height()).arg(error);
if(self)QMetaObject::invokeMethod(self.data(),[self,diagnostic]{if(self)emit self->logMessage(diagnostic);},Qt::QueuedConnection);
return roi;
};
if(!s.leftImage.isNull())s.leftRoi=chooseRoi(s.leftImage,QRect(),false);
if(!s.leftImage.isNull()&&s.leftRoi.isValid()&&QRect(QPoint(0,0),s.leftImage.size()).contains(s.leftRoi)){
stage.restart();auto gray=VisionGuideMatcher::qImageToMat(s.leftImage);if(gray.channels()!=1)cv::cvtColor(gray,gray,cv::COLOR_BGR2GRAY);timings["left_gray_ms"]=static_cast<double>(stage.elapsed());
stage.restart();s.leftPose=matcher->match(gray,s.leftRoi,CAM_LEFT,false,&s.parameters);timings["left_match_wall_ms"]=static_cast<double>(stage.elapsed());timings["left_core_ms"]=s.leftPose.matchTimeMs;timings["left_preprocess_ms"]=s.leftPose.preprocessTimeMs;timings["left_parse_projection_ms"]=s.leftPose.parseTimeMs;
}else{if(s.leftPose.errorMsg.isEmpty())s.leftPose.errorMsg=QString::fromUtf8("左路原图或ROI无效");if(mode!=2)throw std::runtime_error(s.leftPose.errorMsg.toUtf8().constData());}
if(mode!=0&&!s.rightImage.isNull()&&clock->elapsed()<deadlineMs){
QRect hint;if(s.leftRoi.isValid()&&!matcher->predictRightRoi(s.leftRoi,s.stereoPoseFile,s.rightImage.size(),hint,s.leftPose.valid?s.leftPose.tz:.32,.10))throw std::runtime_error("RIGHT ROI projection failed.");s.rightRoi=chooseRoi(s.rightImage,hint,true);
if(s.rightRoi.isValid()&&QRect(QPoint(0,0),s.rightImage.size()).contains(s.rightRoi)){
stage.restart();auto grayR=VisionGuideMatcher::qImageToMat(s.rightImage);if(grayR.channels()!=1)cv::cvtColor(grayR,grayR,cv::COLOR_BGR2GRAY);timings["right_gray_ms"]=static_cast<double>(stage.elapsed());stage.restart();s.rightPose=matcher->match(grayR,s.rightRoi,CAM_RIGHT,false,&s.parameters);timings["right_match_wall_ms"]=static_cast<double>(stage.elapsed());timings["right_core_ms"]=s.rightPose.matchTimeMs;timings["right_preprocess_ms"]=s.rightPose.preprocessTimeMs;timings["right_parse_projection_ms"]=s.rightPose.parseTimeMs;
}else if(s.rightPose.errorMsg.isEmpty())s.rightPose.errorMsg=QString::fromUtf8("右路ROI无效");
}
stage.restart();if(mode!=0)s.fusion=fusion->fuse(s.leftPose,s.rightPose,s.parameters.poseRefinement=="none"?StereoPoseFusion::FM_WeightedScore:StereoPoseFusion::FM_InverseVariance,1.0,10.0,s.leftCal,s.rightCal);selected=MeasurementOptionsPanel::selectPose(mode,s.leftPose,s.rightPose,s.fusion);timings["fusion_selection_ms"]=static_cast<double>(stage.elapsed());
}catch(const VisionGuideApi::HException& e){selected.error=sanitizeUserVisibleLog(QString::fromLocal8Bit(e.ErrorMessage().TextA()));}catch(const std::exception& e){selected.error=sanitizeUserVisibleLog(QString::fromUtf8(e.what()));}catch(...){selected.error="Unhandled measurement error";}
if(!selected.pose.valid&&mode==2&&s.leftPose.valid&&clock->elapsed()<deadlineMs){
s.rightPose.errorMsg=selected.error;record["fallback_reason"]=selected.error;
s.fusion=fusion->fuse(s.leftPose,s.rightPose,s.parameters.poseRefinement=="none"?StereoPoseFusion::FM_WeightedScore:StereoPoseFusion::FM_InverseVariance,1.0,10.0,s.leftCal,s.rightCal);
selected=MeasurementOptionsPanel::selectPose(mode,s.leftPose,s.rightPose,s.fusion);
}
const double elapsed=clock->elapsed();timings["worker_end_ms"]=elapsed;
selected=MeasurementOptionsPanel::enforceDeadline(selected,static_cast<qint64>(elapsed),deadlineMs);if(elapsed>=deadlineMs)record["timed_out"]=true;
if(selected.pose.valid)timings["pose_ready_ms"]=elapsed;else timings["pose_ready_ms"]=QJsonValue();
s.finalPose=selected.pose;s.finalSide=CAM_LEFT;s.valid=selected.pose.valid;record["timings_ms"]=timings;
if(self)QMetaObject::invokeMethod(self.data(),[self,s,record,selected,generation,clock,deadlineMs]()mutable{if(!self||self->m_generation!=generation)return;auto times=record["timings_ms"].toObject();if(clock->elapsed()>=deadlineMs){s.valid=false;s.finalPose.valid=false;selected.pose.valid=false;selected.source="NONE";selected.error=QString::fromUtf8("位姿发布前已超时。");record["timed_out"]=true;times["pose_ready_ms"]=QJsonValue();}else if(s.valid)times["pose_ready_ms"]=static_cast<double>(clock->elapsed());record["timings_ms"]=times;self->complete(s,record,selected);},Qt::QueuedConnection);
});
m_worker=worker;connect(worker,&QThread::finished,this,[this,worker]{if(m_worker==worker)m_worker=nullptr;m_legacy->endExternalMeasurement();setBusy(false);m_remoteRequest={};worker->deleteLater();});worker->start();
QTimer::singleShot(std::max(0,deadlineMs-int(clock->elapsed())),Qt::PreciseTimer,this,[this,generation,deadlineMs]{if(!m_busy||m_generation!=generation||m_record["generation"].toDouble()==static_cast<double>(generation))return;m_timedOut=true;m_status->setText(QString::fromUtf8("达到%1秒截止,返回失败;等待视觉引导算子退出…").arg(deadlineMs/1000));m_snapshot.valid=false;m_draw->setEnabled(false);m_grasp->setEnabled(false);remoteFailure("DEADLINE","Measurement deadline reached; waiting for the operator to exit");emit logMessage(QString::fromUtf8("[测量][超时] 迟到结果将丢弃;算子退出前保持资源锁。"));});
}
void MeasurementOptionsPanel::complete(MeasurementCaptureSnapshot snapshot,QJsonObject record,const MeasurementSelection& selected){
if(m_timedOut){snapshot.valid=false;snapshot.finalPose.valid=false;record["timed_out"]=true;auto times=record["timings_ms"].toObject();times["pose_ready_ms"]=QJsonValue();record["timings_ms"]=times;}
m_snapshot=snapshot;record["status"]=snapshot.valid?"success":"failed";record["output_source"]=snapshot.valid?selected.source:"NONE";record["error"]=snapshot.valid?QString():selected.error;record["left_raw"]=poseJson(snapshot.leftPose);record["right_raw"]=poseJson(snapshot.rightPose);record["right_in_left"]=poseJson(snapshot.fusion.rightInLeft);record["final_pose"]=poseJson(snapshot.finalPose);record["left_roi_xywh"]=rectJson(snapshot.leftRoi);record["right_roi_xywh"]=rectJson(snapshot.rightRoi);record["fusion"]=QJsonObject{{"consistent",snapshot.fusion.consistent},{"position_disparity_mm",snapshot.fusion.positionDisparityMm},{"axis_disparity_deg",snapshot.fusion.orientationDisparityDeg},{"weight_left",snapshot.fusion.weightLeft},{"weight_right",snapshot.fusion.weightRight},{"uncertainty_model","rotated_diagonal_translation_approximation"}};m_record=record;m_legacy->acceptExternalMeasurement(snapshot);
refreshCameraPoses();if(snapshot.valid&&!selected.error.isEmpty())m_record["fallback_reason"]=selected.error;
m_source->setText(QString::fromUtf8("输出来源:%1;最终位姿在左相机系").arg(record["output_source"].toString()));m_status->setText(snapshot.valid?(selected.error.isEmpty()?QString::fromUtf8("位姿已就绪。可核对拍照示教,计算抓取XYZABC;原图按需保存。"):selected.error):QString::fromUtf8("测量失败:%1").arg(selected.error));
emit logMessage(QString::fromUtf8("[测量][结果] run=%1 status=%2 source=%3").arg(record["run_id"].toString(),record["status"].toString(),record["output_source"].toString()));
auto metrics=m_record["fusion"].toObject();metrics["position_limit_mm"]=1.0;metrics["reprojection_checked"]=snapshot.fusion.reprojectionChecked;metrics["reprojection_passed"]=snapshot.fusion.reprojectionPassed;metrics["left_reprojection_error_px"]=snapshot.fusion.leftReprojectionErrorPx;metrics["right_reprojection_error_px"]=snapshot.fusion.rightReprojectionErrorPx;metrics["reprojection_limit_px"]=snapshot.fusion.reprojectionLimitPx;m_record["fusion"]=metrics;
if(!snapshot.rightImage.isNull())emit logMessage(QString::fromUtf8("[测量][双目] %1").arg(snapshot.fusion.message));
if(!snapshot.valid)emit logMessage(QString::fromUtf8("[测量][失败原因] %1;左路:%2;右路:%3").arg(record["error"].toString(),snapshot.leftPose.errorMsg,snapshot.rightPose.errorMsg));
// Publish structured values before drawing/archiving; neither may trigger another capture.
if(!m_remoteRequest.isEmpty())completeRemote(snapshot,m_record);
if(snapshot.leftPose.valid||snapshot.rightPose.valid)renderResult();
writeArchive(m_record,m_snapshot,QImage(),false);
}
bool MeasurementOptionsPanel::remoteReady(QString* error) const
{
try {
if(!m_camera||!m_camera->leftConnected()||!m_camera->rightConnected())throw std::runtime_error("Both cameras must be connected");
if(!m_matcher||!m_matcher->isReady(CAM_LEFT)||!m_yolo||!m_yolo->isLoaded())throw std::runtime_error("LEFT calibration/model or YOLO is not loaded");
const auto cal=effectiveCalibration();
if(cal["robot_rotation_rule"].toString()!="Rz(A) Ry(B) Rx(C)"||cal["robot_tool"].toString()!="TOOL1"||cal["robot_reference_frame_human_confirmed"].toString()!="world")throw std::runtime_error("Verified world/TOOL1 Rz(A)Ry(B)Rx(C) calibration required");
jsonMatrix(cal["T_TOOL1_camera_mm"]);jsonMatrix(cal["T_grasp_TOOL1_from_one_teach_mm"]);
robotTransform(cal["teach_reference_XYZABC_mm_deg"].toArray());
const auto normal=cal["tray_normal_world"].toArray();
if(normal.size()!=3)throw std::runtime_error("Tray normal missing");
double norm=0;for(auto v:normal){if(!v.isDouble()||!std::isfinite(v.toDouble()))throw std::runtime_error("Invalid tray normal");norm+=v.toDouble()*v.toDouble();}
if(norm<.81)throw std::runtime_error("Invalid tray normal");
if(usesTeachPlane()){robotTransform(m_activeTeach);}
return true;
} catch(const std::exception& e){if(error)*error=QString::fromUtf8(e.what());return false;}
}
void MeasurementOptionsPanel::remoteFailure(const QString& code,const QString& message)
{
if(m_remoteRequest.isEmpty()||m_remoteTerminal)return;
m_remoteTerminal=true;
QJsonObject result{{"type","measure"},{"request_id",m_remoteRequest["request_id"]},
{"context",m_remoteRequest["context"]},{"status",m_remoteCancelled?"cancelled":(code=="DEADLINE"?"timeout":"error")},
{"camera_valid",false},{"robot_valid",false},{"error_code",code},{"message",message}};
if(!m_remoteCancelled&&code=="GRASP")for(auto it=m_remoteCameraResult.begin();it!=m_remoteCameraResult.end();++it)result[it.key()]=it.value();
emit remoteFinished(result);
}
void MeasurementOptionsPanel::startRemote(const QJsonObject& request)
{
if(!m_legacy||m_busy||m_legacy->isBusy()){
emit remoteFinished(QJsonObject{{"type","measure"},{"request_id",request["request_id"]},{"context",request["context"]},{"status","error"},{"error_code","BUSY"},{"message","A measurement is already running"}});return;
}
m_remoteRequest=request;m_remoteTerminal=false;m_remoteCancelled=false;m_remoteCameraResult={};
try {
const auto parameters=request["parameters"].toObject();
if(parameters["camera_mode"].toString()!="left_only"||parameters["refinement"].toString()!="least_squares_high"||parameters["deadline_ms"].toInt()!=5000||
parameters["min_score"].toDouble()!=.70||parameters["greediness"].toDouble()!=.80||parameters["yolo_selection"].toString()!="center"||
parameters["pose_display"].toString()!="model_origin"||parameters["grasp_offset_mm"].toDouble()!=4.40)throw std::runtime_error("Unsupported measurement parameters");
const auto convention=request["convention"].toObject();
if(convention["frame"].toString()!="world"||convention["tool"].toString()!="TOOL1"||convention["rotation"].toString()!="Rz(A)Ry(B)Rx(C)"||convention["linear_unit"].toString()!="mm"||convention["angular_unit"].toString()!="deg")throw std::runtime_error("Robot pose convention mismatch");
if(!request["require_robot_pose"].toBool()||(request["pose_source"].toString()!="plc_r40"&&request["pose_source"].toString()!="diagnostic"))throw std::runtime_error("Robot pose source required");
m_remoteRobot=request["robot_pose"].toArray();robotTransform(m_remoteRobot);
const auto context=request["context"].toObject();
if(context["workpiece_id"].toString().isEmpty()||context["task_id"].toString().isEmpty()||context["generation"].toString().isEmpty())throw std::runtime_error("Complete measurement context required");
const int tray=context["tray_no"].toInt();
if(tray<1||tray>99||context["tray_no"].toDouble()!=tray)throw std::runtime_error("Invalid tray number");
if(tray!=m_currentTrayId){
const auto profile=m_trayProfiles[QString::number(tray)];
if(!profile.isObject())throw std::runtime_error("Requested tray has no saved setup");
m_trayProfiles[QString::number(m_currentTrayId)]=traySetupState();
restoreTraySetup(profile.toObject());m_currentTrayId=tray;
QSignalBlocker blocker(m_traySelector);m_traySelector->setCurrentIndex(m_traySelector->findData(tray));
}
QString error;if(!remoteReady(&error))throw std::runtime_error(error.toUtf8().constData());
// Freeze the exact robot pose and effective tray setup before capture. Display rounding is irrelevant.
m_remoteCalibration=effectiveCalibration();m_remoteTeach=m_activeTeach;m_remoteHeightMethod=selectedHeightMethod();
m_mode->setCurrentIndex(0);m_refinement->setCurrentIndex(m_refinement->findData("least_squares_high"));m_deadline->setValue(5);
m_minScore->setValue(.70);m_greediness->setValue(.80);m_yoloSelection->setCurrentIndex(0);m_posePoint->setCurrentIndex(0);m_offset->setValue(4.4);
m_remoteFingerprint=QString::fromLatin1(QCryptographicHash::hash(QJsonDocument(QJsonObject{{"calibration",m_remoteCalibration},{"tray",traySetupState()},{"camera_sha256",m_matcher->cameraParamSha256(CAM_LEFT)},{"model_sha256",shaFile(m_matcher->shapeModelFile(CAM_LEFT))},{"offset_mm",4.4}}).toJson(QJsonDocument::Compact),QCryptographicHash::Sha256).toHex());
for(int i=0;i<6;++i)m_robot[i]->setText(QString::number(m_remoteRobot[i].toDouble(),'g',17));
start();
if(!m_busy&&!m_remoteTerminal)remoteFailure("START_FAILED","Measurement did not start");
if(!m_busy)m_remoteRequest={};
}catch(const VisionGuideApi::HException& e){remoteFailure("MEASURE",QString::fromLocal8Bit(e.ErrorMessage().TextA()));if(!m_worker){m_legacy->endExternalMeasurement();setBusy(false);m_remoteRequest={};}}
catch(const std::exception& e){remoteFailure("VALIDATION",QString::fromUtf8(e.what()));if(!m_worker){m_legacy->endExternalMeasurement();setBusy(false);m_remoteRequest={};}}
}
void MeasurementOptionsPanel::cancelRemote()
{
if(m_remoteRequest.isEmpty())return;
m_remoteCancelled=true;remoteFailure("CANCELLED","Measurement result invalidated");
}
void MeasurementOptionsPanel::completeRemote(const MeasurementCaptureSnapshot& snapshot,const QJsonObject& record)
{
if(m_remoteTerminal)return;
if(m_remoteCancelled){remoteFailure("CANCELLED","Measurement result invalidated");return;}
if(!snapshot.valid||!snapshot.leftPose.valid||record["output_source"].toString()!="LEFT"){
remoteFailure(record["timed_out"].toBool()?"DEADLINE":"MEASURE",record["error"].toString("LEFT measurement failed"));return;
}
QElapsedTimer clock;clock.start();
try {
const auto camera=array6(snapshot.leftPose);for(auto v:camera)if(!v.isDouble()||!std::isfinite(v.toDouble()))throw std::runtime_error("Non-finite camera pose");
if(!std::isfinite(snapshot.leftPose.score)||snapshot.leftPose.score<.70)throw std::runtime_error("LEFT score is below MinScore");
m_remoteCameraResult={{"camera_valid",true},{"camera_pose",camera},{"source","LEFT"},{"pose_type","model_origin"},{"camera_rotation_type",snapshot.leftPose.poseType},{"score",snapshot.leftPose.score}};
QJsonObject grasp;
const QJsonArray reference=m_remoteTeach.size()==6?QJsonArray{m_remoteTeach[0],m_remoteTeach[1],m_remoteTeach[2]}:QJsonArray();
if(m_remoteHeightMethod=="keep_visual_xy_solve_z")grasp=graspTargetKeepingXY(snapshot.leftPose,m_remoteRobot,m_remoteCalibration,reference,4.4);
else if(m_remoteHeightMethod=="camera_ray_plane_intersection")grasp=graspTargetOnPlane(snapshot.leftPose,m_remoteRobot,m_remoteCalibration,reference,4.4);
else if(m_remoteHeightMethod=="visual_3d")grasp=graspTarget(snapshot.leftPose,m_remoteRobot,m_remoteCalibration,4.4);
else throw std::runtime_error("Unsupported tray height method");
const auto robot=grasp["XYZABC_mm_deg"].toArray();robotTransform(robot);
m_record["photo_robot_XYZABC_mm_deg"]=m_remoteRobot;m_record["photo_robot_pair_confirmed"]=true;m_record["photo_robot_pairing_method"]="ipc_request_snapshot";
m_record["grasp_targets"]=QJsonObject{{"left",grasp}};
for(int i=0;i<6;++i)m_values->setItem(3,i,new QTableWidgetItem(QString::number(robot[i].toDouble(),'f',3)));
m_remoteTerminal=true;
emit remoteFinished(QJsonObject{{"type","measure"},{"request_id",m_remoteRequest["request_id"]},{"context",m_remoteRequest["context"]},{"status","ok"},
{"camera_pose",camera},{"robot_pose",robot},{"camera_valid",true},{"robot_valid",true},{"source","LEFT"},{"pose_type","model_origin"},{"camera_rotation_type",snapshot.leftPose.poseType},
{"score",snapshot.leftPose.score},{"parameters",m_remoteRequest["parameters"]},{"convention",m_remoteRequest["convention"]},{"calibration_fingerprint",m_remoteFingerprint},{"run_id",record["run_id"]},
{"capture_begin",record["capture_begin_utc"]},{"capture_end",record["capture_end_utc"]},{"measure_elapsed_ms",record["timings_ms"].toObject()["pose_ready_ms"]},{"grasp_elapsed_ms",double(clock.elapsed())}});
}catch(const VisionGuideApi::HException& e){remoteFailure("GRASP",QString::fromLocal8Bit(e.ErrorMessage().TextA()));}
catch(const std::exception& e){remoteFailure("GRASP",QString::fromUtf8(e.what()));}
}
void MeasurementOptionsPanel::renderResult(){
if(!m_matcher)return;
auto draw=[&](VisionPoseResult pose,const QImage& input,CameraSide side,const QString& label){
if(!pose.valid||input.isNull()||!m_matcher->updatePoseProjection(pose,side,true))return QImage();
if(m_posePoint->currentIndex()==1){pose=offsetPose(pose,m_offset->value());if(!m_matcher->updatePoseProjection(pose,side,false))return QImage();}
auto image=VisionGuideMatcher::qImageToMat(input);if(image.empty())return QImage();m_matcher->drawPoseResult(image,pose);auto output=VisionGuideMatcher::matToQImage(image);emit resultImage(QString::fromUtf8("测量_%1_%2").arg(label,m_record["run_id"].toString()),output);return output;
};
auto right=draw(m_snapshot.rightPose,m_snapshot.rightImage,CAM_RIGHT,QString::fromUtf8("右路原始位姿"));
auto left=draw(m_snapshot.leftPose,m_snapshot.leftImage,CAM_LEFT,QString::fromUtf8("左路原始位姿"));
m_resultImage=!left.isNull()?left:right;
if(m_snapshot.valid){
const bool rightFallback=m_record["output_source"].toString()=="RIGHT_FALLBACK";
auto final=draw(rightFallback?m_snapshot.rightPose:m_snapshot.finalPose,rightFallback?m_snapshot.rightImage:m_snapshot.leftImage,rightFallback?CAM_RIGHT:CAM_LEFT,m_record["output_source"].toString());
if(!final.isNull())m_resultImage=final;
}
}
void MeasurementOptionsPanel::calculateGrasp(){
if(m_busy||m_timedOut||m_record.value("timed_out").toBool())return;
try{
invalidateGraspTargets();
if(m_record["tray_id"].toInt(m_currentTrayId)!=m_currentTrayId)throw std::runtime_error("测量结果与当前料盘不一致,请重新测量。");
if(m_snapshot.parameters.poseRefinement=="none")throw std::runtime_error("Coarse-only mode is diagnostic; select a refined mode for grasp output.");
auto robot=robotInput();auto cal=effectiveCalibration();
auto targets=graspTargets(m_snapshot,robot,cal,m_offset->value());if(targets.isEmpty())throw std::runtime_error("No valid camera pose.");
const QString keys[]={"left","right_in_left","fused"};
if(usesTeachPlane()){
if(m_activeTeach.size()!=6)throw std::runtime_error("请先保存抓取示教位姿。");
QJsonArray reference{m_activeTeach[0],m_activeTeach[1],m_activeTeach[2]};
const VisionPoseResult sources[]={m_snapshot.leftPose,m_snapshot.fusion.rightInLeft,m_snapshot.fusion.fused};
for(int i=0;i<3;++i)if(targets.contains(keys[i]))targets[keys[i]]=selectedHeightMethod()=="keep_visual_xy_solve_z"?graspTargetKeepingXY(sources[i],robot,cal,reference,m_offset->value()):graspTargetOnPlane(sources[i],robot,cal,reference,m_offset->value());
m_record["validated_plane_reference_world_TOOL1_XYZ_mm"]=reference;m_record["equal_height_grasp_plane_assumed"]=true;
QJsonArray teaching=m_activeTeach;
m_record["height_teach_XYZABC_mm_deg"]=teaching;
m_record["height_teach_is_ground_truth"]=false;
saveSetup();
}
m_record["grasp_Z_source"]=usesTeachPlane()?"validated_equal_height_grasp_plane":"visual_3d";m_record["grasp_height_method"]=selectedHeightMethod();
for(int i=0;i<3;++i){auto result=targets.value(keys[i]).toObject();auto values=result.value("XYZABC_mm_deg").toArray();
for(int c=0;c<6;++c)m_values->setItem(i+3,c,new QTableWidgetItem(values.size()==6?QString::number(values[c].toDouble(),'f',3):"—"));
m_values->setItem(i+3,6,new QTableWidgetItem(values.size()==6?"D":"—"));
if(!result.isEmpty()){result["calibration_sha256"]=shaFile(m_calibrationPath);result["source"]=keys[i];targets[keys[i]]=result;}
}
m_record["tray_id"]=m_currentTrayId;m_record["grasp_targets"]=targets;m_record["photo_robot_XYZABC_mm_deg"]=robot;m_record["photo_robot_pair_confirmed"]=true;m_record["photo_robot_pairing_method"]="manual_entry_at_calculation";m_record["tray_normal_world_used"]=cal["tray_normal_world"];m_record["tray_normal_evidence"]=m_activeNormal.isEmpty()?QJsonObject{{"source","original_calibration"}}:m_activeNormal;
updateSetupStatus();
m_status->setText(QString::fromUtf8("已计算%1组抓取结果 · %2").arg(targets.size()).arg(m_heightMethod->currentText()));
QJsonObject displayed;for(const auto& key:keys)if(targets.contains(key))displayed[key]=targets[key].toObject()["XYZABC_mm_deg"];
emit logMessage(QString::fromUtf8("[测量][抓取] %1").arg(QString::fromUtf8(QJsonDocument(displayed).toJson(QJsonDocument::Compact))));writeArchive(m_record,m_snapshot,QImage(),false);
}catch(const VisionGuideApi::HException& e){m_status->setText(sanitizeUserVisibleLog(QString::fromLocal8Bit(e.ErrorMessage().TextA())));}catch(const std::exception& e){m_status->setText(sanitizeUserVisibleLog(QString::fromUtf8(e.what())));}
}
void MeasurementOptionsPanel::saveData(){
if(m_busy||m_saveBusy||m_record.isEmpty())return;
try{QJsonObject record=m_record;try{record["photo_robot_XYZABC_mm_deg"]=robotInput();record["photo_robot_pair_confirmed"]=true;record["photo_robot_pairing_method"]="manual_entry_at_save";}catch(const std::exception&){record.remove("photo_robot_XYZABC_mm_deg");record["photo_robot_pair_confirmed"]=false;}record["result_image_format"]="JPEG";record["result_image_file"]=m_resultImage.isNull()?QString():"result.jpg";m_saveBusy=true;m_save->setEnabled(false);writeArchive(record,m_snapshot,m_resultImage,true);
}catch(const std::exception& e){m_status->setText(sanitizeUserVisibleLog(QString::fromUtf8(e.what())));}
}
void MeasurementOptionsPanel::writeArchive(QJsonObject record,const MeasurementCaptureSnapshot& snapshot,const QImage& result,bool images){
if(m_archiveWorkers.size()>=4){m_saved->setText(QString::fromUtf8("归档队列已满,请稍后保存。"));m_saveBusy=false;m_save->setEnabled(!m_busy&&!m_record.isEmpty());return;}
const QString dir=QDir(rootPath()).filePath("user_data/MeasurementTests/"+record["run_id"].toString());const QString run=record["run_id"].toString();QPointer<MeasurementOptionsPanel> self(this);
// A same-run metadata update is serialized behind its preceding archive jobs.
auto preceding=m_archiveTail;auto completion=std::make_shared<std::promise<void>>();m_archiveTail=completion->get_future().share();
auto archiveSnapshot=images?snapshot:MeasurementCaptureSnapshot();
auto job=QThread::create([self,record,snapshot=archiveSnapshot,result,images,dir,run,preceding,completion]()mutable{
if(preceding.valid())preceding.wait();QElapsedTimer t;t.start();QString failure;try{
if(!QDir().mkpath(dir))throw std::runtime_error("Archive directory creation failed.");
QFile prior(QDir(dir).filePath("record.json"));if(prior.open(QIODevice::ReadOnly)){auto old=QJsonDocument::fromJson(prior.readAll()).object();prior.close();for(auto key:{"left_image_file","right_image_file","left_image_sha256","right_image_sha256","image_archive_ms","result_image_file","result_image_format"})if(old.contains(key)&&!record.contains(key))record[key]=old[key];}
if(images){auto save=[&](const QImage& image,const QString& file,const QString& key){if(image.isNull())return;if(!image.save(QDir(dir).filePath(file),"PNG"))throw std::runtime_error("PNG save failed.");record[key+"_file"]=file;record[key+"_sha256"]=shaFile(QDir(dir).filePath(file));};save(snapshot.leftImage,"left_raw.png","left_image");save(snapshot.rightImage,"right_raw.png","right_image");if(!result.isNull()&&!result.save(QDir(dir).filePath(record["result_image_file"].toString()),record["result_image_format"].toString()=="PNG"?"PNG":"JPG",record["result_image_format"].toString()=="PNG"?-1:92))throw std::runtime_error("Result image save failed.");record["image_archive_ms"]=static_cast<double>(t.elapsed());}
QSaveFile file(QDir(dir).filePath("record.json"));if(!file.open(QIODevice::WriteOnly)||file.write(QJsonDocument(sanitizeUserVisibleRecord(record).toObject()).toJson())<0||!file.commit())throw std::runtime_error("Atomic metadata save failed.");
}catch(const std::exception& e){failure=sanitizeUserVisibleLog(QString::fromUtf8(e.what()));}
completion->set_value();
if(self)QMetaObject::invokeMethod(self.data(),[self,run,dir,failure,images]{if(!self)return;if(images){self->m_saveBusy=false;self->m_save->setEnabled(!self->m_busy&&!self->m_record.isEmpty());}if(failure.isEmpty()){self->m_saved->setText(QString::fromUtf8("已保存:%1").arg(dir));emit self->logMessage(QString::fromUtf8("[测量][归档] run=%1 dir=%2").arg(run,dir));}else{self->m_saved->setText(QString::fromUtf8("保存失败:%1").arg(failure));emit self->logMessage(QString::fromUtf8("[测量][归档失败] %1").arg(failure));}},Qt::QueuedConnection);
});
m_archiveWorkers.insert(job);connect(job,&QThread::finished,this,[this,job]{m_archiveWorkers.remove(job);job->deleteLater();});job->start();
}