Files
VisionGuide/source/MeasurementOptionsPanel.cpp
T

546 lines
60 KiB
C++
Raw Normal View History

2026-10-26 10:56:56 +08:00
#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>
2026-10-27 22:14:53 +08:00
#include <QSignalBlocker>
2026-10-26 10:56:56 +08:00
#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);
});
2026-10-27 22:14:53 +08:00
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("[测量][超时] 迟到结果将丢弃;算子退出前保持资源锁。"));});
2026-10-26 10:56:56 +08:00
}
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));
2026-10-27 22:14:53 +08:00
// Publish structured values before drawing/archiving; neither may trigger another capture.
if(!m_remoteRequest.isEmpty())completeRemote(snapshot,m_record);
2026-10-26 10:56:56 +08:00
if(snapshot.leftPose.valid||snapshot.rightPose.valid)renderResult();
writeArchive(m_record,m_snapshot,QImage(),false);
}
2026-10-27 22:14:53 +08:00
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()));}
}
2026-10-26 10:56:56 +08:00
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();
}