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 ();
}