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

462 lines
37 KiB
C++
Raw Blame History

This file contains ambiguous Unicode characters
This file contains Unicode characters that might be confused with other characters. If you think that this is intentional, you can safely ignore this warning. Use the Escape button to reveal them.
#ifndef NOMINMAX
#define NOMINMAX
#endif
#include "UserVisibleLog.h"
#include "VisionGuidanceCapturePanel.h"
#include "CameraManager.h"
#include "MeasurePanel.h"
#include "BoardPoseEstimator.h"
#include "AppSettings.h"
#include <VisionGuideApi.h>
#include <QScrollArea>
#include <QVBoxLayout>
#include <QGridLayout>
#include <QGroupBox>
#include <QLineEdit>
#include <QLabel>
#include <QPushButton>
#include <QDoubleSpinBox>
#include <QCheckBox>
#include <QComboBox>
#include <QTableWidget>
#include <QHeaderView>
#include <QFileDialog>
#include <QMessageBox>
#include <QDesktopServices>
#include <QUrl>
#include <QDir>
#include <QFileInfo>
#include <QSaveFile>
#include <QFile>
#include <QTemporaryDir>
#include <QJsonDocument>
#include <QJsonArray>
#include <QUuid>
#include <QDateTime>
#include <QCryptographicHash>
#include <QApplication>
#include <cmath>
#include <stdexcept>
using namespace VisionGuideApi;
namespace {
QString now() { return QDateTime::currentDateTimeUtc().toString(Qt::ISODateWithMs); }
QJsonArray vec(double x, double y, double z) { return {x,y,z}; }
QString uid() { return QUuid::createUuid().toString(QUuid::WithoutBraces); }
QJsonObject fileInfo(const QString& path, bool contents = false) {
QJsonObject j; QFileInfo f(path);
j["path"] = f.absoluteFilePath(); j["exists"] = f.isFile();
j["size_bytes"] = double(f.size()); j["modified_utc"] = f.lastModified().toUTC().toString(Qt::ISODateWithMs);
if (contents && f.isFile()) {
if (f.size() > 4*1024*1024) throw std::runtime_error("标定文件过大,请确认选择了参数文件");
QFile in(path); if (!in.open(QIODevice::ReadOnly)) throw std::runtime_error("无法读取标定文件");
QByteArray bytes=in.readAll();
j["sha256"] = QString::fromLatin1(QCryptographicHash::hash(bytes,QCryptographicHash::Sha256).toHex());
j["content_base64"] = QString::fromLatin1(bytes.toBase64());
}
return j;
}
QJsonObject poseData(const HTuple& p) {
if(p.Length()!=7) throw std::runtime_error("视觉引导位姿必须包含7个分量");
HTuple h; PoseToHomMat3d(p,&h); QJsonArray raw, matrix;
for(int i=0;i<7;++i) raw.append(p[i].D());
for(int i=0;i<12;++i) matrix.append(h[i].D());
return {{"pose7_m_deg",raw},{"pose_type",p[6].I()},
{"T_camera_object_row_major_3x4_m",matrix},
{"origin_camera_mm",vec(p[0].D()*1000,p[1].D()*1000,p[2].D()*1000)},
{"z_axis_camera",vec(h[2].D(),h[6].D(),h[10].D())}};
}
QJsonObject matcherParams(const VisionGuideMatcher::Params& p) {
return {{"min_score",p.minScore},{"greediness",p.greediness},{"auto_segmentation",p.useAutoSeg},
{"seg_dilation",p.segDilation},{"pose_refinement",p.poseRefinement},
{"filter_enabled",p.useFilter},{"filter_type",p.filterType},{"filter_kernel",p.filterKernelSize},
{"filter_sigma",p.filterSigma},{"filter_sigma_color",p.filterSigmaColor},{"filter_sigma_space",p.filterSigmaSpace}};
}
struct Cursor {
Cursor() { QApplication::setOverrideCursor(Qt::WaitCursor); }
~Cursor() { QApplication::restoreOverrideCursor(); }
};
cv::Vec3d v3(const QJsonArray& a) { return {a[0].toDouble(),a[1].toDouble(),a[2].toDouble()}; }
}
VisionGuidanceCapturePanel::VisionGuidanceCapturePanel(CameraManager* camera, VisionGuideMatcher* matcher,
MeasurePanel* measure, std::function<void(const QString&)> log, QWidget* parent)
: QWidget(parent), m_camera(camera), m_matcher(matcher), m_measure(measure), m_log(std::move(log))
{
auto outer=new QVBoxLayout(this); outer->setContentsMargins(0,0,0,0);
auto scroll=new QScrollArea(this); scroll->setWidgetResizable(true); outer->addWidget(scroll);
auto page=new QWidget; auto layout=new QVBoxLayout(page); scroll->setWidget(page);
auto group=[&](const QString& title){auto box=new QGroupBox(title); layout->addWidget(box); return new QGridLayout(box);};
auto line=[](QGridLayout* g,int row,const QString& title,const QString& value=QString()) {
auto e=new QLineEdit(value);g->addWidget(new QLabel(title),row,0);g->addWidget(e,row,1);return e;
};
auto browse=[this](QGridLayout* g,int row,QLineEdit* edit,const QString& filter,bool directory=false){
auto b=new QPushButton(QString::fromUtf8("选择…"));g->addWidget(b,row,2);
connect(b,&QPushButton::clicked,this,[this,edit,filter,directory]{
QString p=directory?QFileDialog::getExistingDirectory(this,QString::fromUtf8("数据目录"),edit->text()):
QFileDialog::getOpenFileName(this,QString::fromUtf8("选择文件"),edit->text(),filter);
if(!p.isEmpty()){edit->setText(p);if(directory)refreshHistory();}
});
};
auto& s=AppSettings::instance();
auto g=group(QString::fromUtf8("1. 选择要记录的数据"));
m_recordType=new QComboBox; m_recordType->setObjectName("guidance_record_type");
m_recordType->addItems({QString::fromUtf8("标定板 / 料盘法向"),QString::fromUtf8("第一个零件(含抓取偏移)"),
QString::fromUtf8("第二个零件(含抓取偏移)"),QString::fromUtf8("固定零件多视角 / 重复测量"),
QString::fromUtf8("期望抓取示教参考(非真值)")});g->addWidget(m_recordType,0,0,1,3);
m_view=line(g,1,QString::fromUtf8("视角 / 测量编号"),"view_01");
m_object=line(g,2,QString::fromUtf8("零件编号"),"part_01");
g=group(QString::fromUtf8("2. 软件测量的视觉位姿"));
auto boardButton=new QPushButton(QString::fromUtf8("拍摄左相机并测量标定板位姿"));g->addWidget(boardButton,0,0,1,3);
boardButton->setObjectName("guidance_measure_board");
auto readButton=new QPushButton(QString::fromUtf8("读取最近一次“开始测量”的零件 / 抓取点位姿"));g->addWidget(readButton,1,0,1,3);
readButton->setObjectName("guidance_read_measurement");
const char* visionNames[]={"X","Y","Z","RX","RY","RZ"};
for(int i=0;i<6;++i){auto w=new QWidget;auto l=new QVBoxLayout(w);l->setContentsMargins(0,0,0,0);
l->addWidget(new QLabel(QString(visionNames[i])+(i<3?" (mm)":" (deg)")));
m_visionFields[i]=new QLineEdit;m_visionFields[i]->setReadOnly(true);m_visionFields[i]->setMinimumWidth(65);
m_visionFields[i]->setObjectName(QString("vision_")+visionNames[i]);l->addWidget(m_visionFields[i]);g->addWidget(w,2+i/3,i%3);
}
m_poseStatus=new QLabel(QString::fromUtf8("尚未获取视觉位姿。零件先在原页面点击开始测量,再返回此页读取。"));m_poseStatus->setWordWrap(true);g->addWidget(m_poseStatus,4,0,1,3);
m_captureStatus=new QLabel;m_captureStatus->setWordWrap(true);g->addWidget(m_captureStatus,5,0,1,3);
m_offset=new QDoubleSpinBox;m_offset->setRange(-1000,1000);m_offset->setDecimals(4);m_offset->setSuffix(" mm");m_offset->setReadOnly(true);m_offset->setButtonSymbols(QAbstractSpinBox::NoButtons);
g->addWidget(new QLabel(QString::fromUtf8("本次零件沿 +Z 偏移")),6,0);g->addWidget(m_offset,6,1,1,2);
auto offsetHelp=new QLabel(QString::fromUtf8("零件XYZ显示偏移后的点,RX/RY/RZ沿用视觉结果;保存时同时保留原点、原始位姿、旋转轴和偏移定义。RZ仅留档。"));offsetHelp->setWordWrap(true);g->addWidget(offsetHelp,7,0,1,3);
g=group(QString::fromUtf8("3. 手动输入对应拍照时的示教器位姿"));
const char* robotNames[]={"X","Y","Z","A","B","C"};
for(int i=0;i<6;++i){auto w=new QWidget;auto l=new QVBoxLayout(w);l->setContentsMargins(0,0,0,0);
l->addWidget(new QLabel(QString(robotNames[i])+(i<3?" (mm)":" (deg)")));
m_photoRobot[i]=new QLineEdit;m_photoRobot[i]->setMinimumWidth(65);m_photoRobot[i]->setPlaceholderText(QString::fromUtf8("手动填写"));
m_photoRobot[i]->setObjectName(QString("photo_")+robotNames[i]);m_teachRobot[i]=m_photoRobot[i];l->addWidget(m_photoRobot[i]);g->addWidget(w,i/3,i%3);
}
m_tool=line(g,2,QString::fromUtf8("工具号"),"TOOL1");
m_reference=line(g,3,QString::fromUtf8("示教器参考坐标系"),QString::fromUtf8("世界坐标系(请核对)"));
m_convention=line(g,4,QString::fromUtf8("ABC定义 / 角度顺序"),QString::fromUtf8("未确认;A、B、C原样记录"));
m_measurePairConfirmed=new QCheckBox(QString::fromUtf8("确认以上XYZABC对应该次视觉拍照时刻"));g->addWidget(m_measurePairConfirmed,5,0,1,3);
auto save=new QPushButton(QString::fromUtf8("同时保存:视觉位姿 + 手输示教器位姿"));save->setObjectName("guidance_save_selected");g->addWidget(save,6,0,1,3);
m_saveStatus=new QLabel;m_saveStatus->setWordWrap(true);m_saveStatus->setTextInteractionFlags(Qt::TextSelectableByMouse);g->addWidget(m_saveStatus,7,0,1,3);
g=group(QString::fromUtf8("4. 文件与标定板设置"));
m_output=line(g,0,QString::fromUtf8("数据目录"),s.get("guidance/output",QDir::current().absoluteFilePath("user_data/VisionGuidanceCapture")));
m_output->setObjectName("guidance_output");browse(g,0,m_output,{},true);
m_cal=line(g,1,QString::fromUtf8("左相机内参 .cal"),s.get(AppSettings::K_LeftCal,QDir::current().absoluteFilePath("user_data/StereoCalibration/cam_left.cal")));browse(g,1,m_cal,"Camera (*.cal *.dat);;All (*)");
m_cpd=line(g,2,QString::fromUtf8("HG75描述文件 .cpd"),s.get("guidance/cpd",QDir::current().absoluteFilePath("VisionGuideData/LD-HG75-13x15.cpd")));browse(g,2,m_cpd,"Board (*.cpd *.descr);;All (*)");
m_handeye=line(g,3,QString::fromUtf8("手眼文件(可选留档)"),s.get(AppSettings::K_HandEyeMatrix));browse(g,3,m_handeye,"All (*)");
m_boardParallel=new QCheckBox(QString::fromUtf8("确认标定板面与料盘平面平行且已固定"));g->addWidget(m_boardParallel,4,0,1,3);
auto offline=new QPushButton(QString::fromUtf8("导入标定板左图并测量(离线)"));g->addWidget(offline,5,0,1,3);
g=group(QString::fromUtf8("5. 法向诊断(原始数据始终保留)"));
m_equalHeight=new QCheckBox(QString::fromUtf8("双零件基准点等高"));g->addWidget(m_equalHeight,0,0,1,3);
m_axisParallel=new QCheckBox(QString::fromUtf8("两零件旋转轴近似平行,且平行于料盘平面"));g->addWidget(m_axisParallel,1,0,1,3);
auto compare=new QPushButton(QString::fromUtf8("对同一相机坐标系的两个零件计算法向"));g->addWidget(compare,2,0,1,3);
m_normalStatus=new QLabel(QString::fromUtf8("两个零件支持分别拍摄并保存。相机移动后的两张图,需先确认ABC定义与手眼关系,再在统一坐标系计算法向。"));m_normalStatus->setWordWrap(true);g->addWidget(m_normalStatus,3,0,1,3);
g=group(QString::fromUtf8("6. 已保存记录"));
m_history=new QTableWidget(0,3);m_history->setHorizontalHeaderLabels({QString::fromUtf8("时间"),QString::fromUtf8("类型"),QString::fromUtf8("零件 / 视角")});
m_history->setEditTriggers(QAbstractItemView::NoEditTriggers);m_history->setMinimumHeight(180);m_history->horizontalHeader()->setSectionResizeMode(QHeaderView::ResizeToContents);g->addWidget(m_history,0,0,1,3);
auto open=new QPushButton(QString::fromUtf8("打开数据目录"));g->addWidget(open,1,0);
auto refresh=new QPushButton(QString::fromUtf8("刷新记录"));g->addWidget(refresh,1,1);
connect(open,&QPushButton::clicked,this,[this]{QDir().mkpath(m_output->text().trimmed());QDesktopServices::openUrl(QUrl::fromLocalFile(m_output->text().trimmed()));});
connect(refresh,&QPushButton::clicked,this,[this]{refreshHistory();});
connect(m_output,&QLineEdit::editingFinished,this,[this]{refreshHistory();});
connect(compare,&QPushButton::clicked,this,[this]{compareNormals();});
connect(readButton,&QPushButton::clicked,this,[this]{prepareMeasurement();});
connect(save,&QPushButton::clicked,this,[this]{saveSelected();});
auto detect=[this](bool imported){
m_pendingRecord={};m_measurePairConfirmed->setChecked(false);for(auto e:m_visionFields)e->clear();
QString previous=m_capture["capture_id"].toString();capture(imported,false);
if(m_capture["capture_id"].toString()!=previous)board(false);
};
connect(boardButton,&QPushButton::clicked,this,[detect]{detect(false);});
connect(offline,&QPushButton::clicked,this,[detect]{detect(true);});
auto updateType=[this,boardButton,readButton,offline,save]{
int type=m_recordType->currentIndex();bool boardMode=type==0,teachMode=type==4;
boardButton->setEnabled(boardMode);offline->setEnabled(boardMode);readButton->setEnabled(!boardMode&&!teachMode);
m_boardParallel->setVisible(boardMode);m_offset->setEnabled(!boardMode&&!teachMode);
m_measurePairConfirmed->setText(teachMode?QString::fromUtf8("确认这是期望抓取的示教参考(非真值)"):QString::fromUtf8("确认以上XYZABC对应该次视觉拍照时刻"));
save->setText(teachMode?QString::fromUtf8("保存期望抓取示教参考(非真值)"):QString::fromUtf8("同时保存:视觉位姿 + 手输示教器位姿"));
m_pendingRecord={};m_pendingLeft={};m_pendingRight={};m_pendingAssets={};m_measurePairConfirmed->setChecked(false);
for(auto e:m_visionFields)e->clear();
m_poseStatus->setText(teachMode?QString::fromUtf8("此类记录不需要视觉位姿;填写下方期望抓取示教参数即可。"):QString::fromUtf8("请获取本类记录的视觉位姿,再填写对应示教器位姿。"));
if(type==1 && m_object->text()=="part_02")m_object->setText("part_01");
if(type==2 && m_object->text()=="part_01")m_object->setText("part_02");
};
connect(m_recordType,QOverload<int>::of(&QComboBox::currentIndexChanged),this,[updateType](int){updateType();});updateType();
if(m_measure)connect(m_measure,&MeasurePanel::measurementCaptureStarted,this,[this]{
m_measureAssets={};m_measureCaptureId=uid();m_measurePairConfirmed->setChecked(false);
auto snapshot=m_measure->lastMeasurementSnapshot();
try{
for(auto pair:{qMakePair(QString("left_camera"),snapshot.leftCal),qMakePair(QString("right_camera"),snapshot.rightCal),qMakePair(QString("right_to_left_pose"),snapshot.stereoPoseFile),qMakePair(QString("hand_eye_archive_only"),m_handeye->text().trimmed())})
if(!pair.second.isEmpty())m_measureAssets[pair.first]=fileInfo(pair.second,true);
m_measureAssets["left_shape_model"]=fileInfo(snapshot.leftModel);m_measureAssets["right_shape_model"]=fileInfo(snapshot.rightModel);
}catch(const std::exception& e){if(m_log)m_log(QString::fromUtf8("[视觉引导采集] 部分文件留档失败:")+sanitizeUserVisibleLog(QString::fromUtf8(e.what())));}
});
layout->addStretch();refreshHistory();
}
void VisionGuidanceCapturePanel::fail(const QString& s) { if(m_log)m_log(QString::fromUtf8("[视觉引导采集] ")+s); QMessageBox::warning(this,QString::fromUtf8("视觉引导采集"),s); }
bool VisionGuidanceCapturePanel::idle() {
if(m_measure && m_measure->isBusy()) { fail(QString::fromUtf8("已有测量任务在执行,请等结束后再采集或估计。")); return false; } return true;
}
QJsonObject VisionGuidanceCapturePanel::readRobot(QLineEdit* const fields[6]) const {
QJsonArray a, texts;
for(int i=0;i<6;++i) { bool ok=false; QString s=fields[i]->text().trimmed(); double v=s.toDouble(&ok);
if(!ok || !std::isfinite(v)) throw std::runtime_error("请完整填写6个有限数值:XYZ单位mm,ABC单位deg");
a.append(v); texts.append(s);
}
return {{"XYZABC_mm_deg",a},{"input_text_XYZABC",texts},{"entry_method","manual"},
{"rotation_convention_resolved",false},{"angle_conversion_applied",false}};
}
QJsonObject VisionGuidanceCapturePanel::context() const {
if(m_tool->text().trimmed().isEmpty() || m_reference->text().trimmed().isEmpty())
throw std::runtime_error("工具号、参考坐标系不能为空");
return {{"dataset_directory",QDir(m_output->text().trimmed()).absolutePath()},{"view_id",m_view->text().trimmed()},
{"object_id",m_object->text().trimmed()},{"tool",m_tool->text().trimmed()},
{"robot_reference_frame",m_reference->text().trimmed()},
{"ABC_definition_note",m_convention->text().trimmed()},
{"model_origin_on_rotation_axis",true},{"visual_RZ_used_for_grasp_heading",false}};
}
void VisionGuidanceCapturePanel::capture(bool imported, bool requireRobot) {
if(!idle())return;
try {
QJsonObject c=context(), r=requireRobot?readRobot(m_photoRobot):QJsonObject();
QJsonObject assets;
// Freeze small calibration files at capture, so later file edits cannot change the record.
for(auto pair:{qMakePair(QString("left_camera"),m_cal),qMakePair(QString("board_description"),m_cpd),qMakePair(QString("hand_eye_archive_only"),m_handeye)})
if(!pair.second->text().trimmed().isEmpty()) assets[pair.first]=fileInfo(pair.second->text().trimmed(),true);
QString source; QImage image; QString begin=now();
if(imported) { source=QFileDialog::getOpenFileName(this,QString::fromUtf8("导入左图"),{},"Images (*.png *.bmp *.jpg *.tif *.tiff)"); if(source.isEmpty())return; image.load(source); }
else { if(!m_camera || !m_camera->isConnected(CameraManager::LEFT))throw std::runtime_error("请先连接左相机"); Cursor cursor; image=m_camera->captureSingleFrame(CameraManager::LEFT); }
if(image.isNull())throw std::runtime_error("图像为空,旧的锁定图像未被替换");
if(requireRobot)c["photo_robot"]=r;
c["capture_id"]=uid(); c["capture_begin_utc"]=begin; c["capture_end_utc"]=now();
c["time_source"]="host_call_interval_not_hardware_exposure_timestamp";
c["image_source"]=imported?"offline_import":"left_camera_fresh_frame"; c["import_path"]=source;
c["camera_coordinate_frame"]="left";
c["robot_pose_exposure_synchronization"]="manual_operator_pairing_not_verified";
c["left_camera_serial"]=m_camera?m_camera->serialNumber(CameraManager::LEFT):QString();
c["image_width"]=image.width(); c["image_height"]=image.height();
m_image=image.copy(); m_capture=c; m_captureAssets=assets;
emit resultImage(QString::fromUtf8("视觉引导 / 标定板原图"),m_image);
m_captureStatus->setText(QString::fromUtf8("图像编号:%1\n拍摄区间:%2 — %3\n请填写这一拍照时刻的示教器XYZABC后同时保存。")
.arg(c["capture_id"].toString(),c["capture_begin_utc"].toString(),c["capture_end_utc"].toString()));
m_poseStatus->setText(QString::fromUtf8("新图像已锁定,尚未估计零件。"));
if(m_log)m_log(QString::fromUtf8("[视觉引导采集] 已锁定左图和手输位姿:")+c["capture_id"].toString());
} catch(const std::exception& e) { fail(sanitizeUserVisibleLog(QString::fromUtf8(e.what()))); }
}
void VisionGuidanceCapturePanel::board(bool save) {
if(!idle())return;
if(m_image.isNull()) { fail(QString::fromUtf8("请先拍摄并锁定位姿。")); return; }
if(!m_boardParallel->isChecked()) { fail(QString::fromUtf8("请核对板面与料盘平行后勾选确认。")); return; }
try {
Cursor cursor;
m_normalStatus->setText(QString::fromUtf8("正在识别HG75标定板…")); m_normalStatus->repaint();
auto cal=m_captureAssets["left_camera"].toObject(), cpd=m_captureAssets["board_description"].toObject();
if(!cal["exists"].toBool() || !cpd["exists"].toBool())throw std::runtime_error("锁定图像时未提供有效内参或CPD,请选择文件后重新拍摄");
// 视觉引导 file APIs require a path: materialize exactly the captured bytes in a temporary directory.
if(!QDir().mkpath(m_output->text().trimmed()))throw std::runtime_error("无法创建数据目录");
QTemporaryDir temp(QDir(m_output->text().trimmed()).absoluteFilePath(".board_XXXXXX"));
if(!temp.isValid())throw std::runtime_error("无法创建板识别临时目录");
auto writeAsset=[&](const QString& name,const QJsonObject& a){QString p=temp.filePath(name); QSaveFile f(p); if(!f.open(QIODevice::WriteOnly) || f.write(QByteArray::fromBase64(a["content_base64"].toString().toLatin1()))<0 || !f.commit())throw std::runtime_error("无法写入临时标定文件"); return p;};
auto calfile=writeAsset("left.cal",cal), cpdfile=writeAsset("board.cpd",cpd);
auto estimate=estimateBoardPose(m_image,calfile,cpdfile);
auto result=estimate.data;
emit resultImage(QString::fromUtf8("视觉引导 / 标定板坐标系(X红 Y绿 Z蓝)"),estimate.axesImage);
result["board_parallel_to_tray_operator_confirmed"]=true; result["tray_height_used"]=false;
QJsonObject record={{"type","board_normal"},{"capture",m_capture},{"board",result}};
if(save){QString id=saveRecord(record,m_image,m_captureAssets);record["record_id"]=id;m_boardRecord=record;}
else {m_pendingRecord=record;m_pendingLeft=m_image;m_pendingRight={};m_pendingAssets=m_captureAssets;showVision(result,false);}
auto n=result["z_axis_camera"].toArray(); m_normalStatus->setText(QString::fromUtf8("板法向(相机系):[%1, %2, %3];标记%4个,重投影RMS %5 px。法向正负采用板+Z,抓取方向需另行确定。")
.arg(n[0].toDouble(),0,'f',6).arg(n[1].toDouble(),0,'f',6).arg(n[2].toDouble(),0,'f',6).arg(result["detected_mark_count"].toInt()).arg(result["reprojection_rms_px"].toDouble(),0,'f',4));
} catch(const HException& e) { fail(QString::fromUtf8("标定板识别失败:")+sanitizeUserVisibleLog(QString::fromLocal8Bit(e.ErrorMessage().TextA()))); }
catch(const std::exception& e) { fail(sanitizeUserVisibleLog(QString::fromUtf8(e.what()))); }
}
void VisionGuidanceCapturePanel::saveTeach() {
try { auto c=context(); if(c["object_id"].toString().isEmpty())throw std::runtime_error("请输入示教对应的零件编号");
auto r=readRobot(m_teachRobot); r["is_ground_truth"]=false; r["purpose"]="desired_grasp_teaching_reference";
QJsonArray references;
for(const auto& record:m_partRecords) {
if(record["capture"].toObject()["dataset_directory"]==c["dataset_directory"] && record["part"].toObject()["object_id"]==c["object_id"])
references.append(record["record_id"]);
}
r["related_part_record_ids"]=references;
saveRecord({{"type","desired_grasp_teach"},{"context",c},{"teach_robot",r}},{});
} catch(const std::exception& e) { fail(sanitizeUserVisibleLog(QString::fromUtf8(e.what()))); }
}
void VisionGuidanceCapturePanel::prepareMeasurement() {
m_pendingRecord={};m_measurePairConfirmed->setChecked(false);
for(auto e:m_visionFields)e->clear();
if(!m_measure){fail(QString::fromUtf8("测量模块不可用。"));return;}
auto snapshot=m_measure->lastMeasurementSnapshot();
if(!snapshot.valid || !snapshot.finalPose.valid){fail(QString::fromUtf8("本次开始测量没有可保存的结果,请重新开始测量。"));return;}
prepareMeasurementSnapshot(snapshot,m_measure->currentGraspOffsetMm());
}
void VisionGuidanceCapturePanel::prepareMeasurementSnapshot(const MeasurementCaptureSnapshot& snapshot,double offset) {
m_pendingRecord={};m_measurePairConfirmed->setChecked(false);
try {
auto c=context(); if(c["object_id"].toString().isEmpty())throw std::runtime_error("当前零件编号不能为空");
c["capture_id"]=m_measureCaptureId;
c["measurement_run_id"]=double(snapshot.runId);
c["capture_begin_utc"]=QDateTime::fromMSecsSinceEpoch(snapshot.captureBeginMs,Qt::UTC).toString(Qt::ISODateWithMs);
c["capture_end_utc"]=QDateTime::fromMSecsSinceEpoch(snapshot.captureEndMs,Qt::UTC).toString(Qt::ISODateWithMs);
c["time_source"]="host_call_interval_not_hardware_exposure_timestamp";
c["image_source"]="start_measurement_raw_frames";
c["robot_pose_exposure_synchronization"]="manual_operator_pairing_not_verified";
c["camera_coordinate_frame"]=snapshot.finalSide==CAM_LEFT?"left":"right";
auto toJson=[](const VisionPoseResult& p){
if(!p.valid)return QJsonObject{{"valid",false}};
HTuple raw; raw[0]=p.tx;raw[1]=p.ty;raw[2]=p.tz;raw[3]=p.rx;raw[4]=p.ry;raw[5]=p.rz;raw[6]=p.poseType;
auto j=poseData(raw); j["valid"]=true;j["score"]=p.score;return j;
};
auto part=toJson(snapshot.finalPose);
auto o=part["origin_camera_mm"].toArray(),a=part["z_axis_camera"].toArray();
part["axial_offset_mm"]=offset;
part["offset_point_camera_mm"]=vec(o[0].toDouble()+offset*a[0].toDouble(),o[1].toDouble()+offset*a[1].toDouble(),o[2].toDouble()+offset*a[2].toDouble());
auto offsetPose=part["pose7_m_deg"].toArray(); auto point=part["offset_point_camera_mm"].toArray();
for(int i=0;i<3;++i)offsetPose[i]=point[i].toDouble()/1000;
part["offset_pose7_m_deg"]=offsetPose;
part["offset_definition"]="p_offset = model_origin + offset_mm * model_positive_z_axis";
part["offset_source"]="existing_pose_tab_grasp_offset_at_save";
part["offset_orientation_unchanged"]=true; part["offset_point_is_gripper_TCP"]=false;
part["object_id"]=c["object_id"];
part["result_source"]="start_measurement_final_output";part["visual_RZ_is_symmetry_gauge"]=true;
part["matcher_parameters"]=matcherParams(snapshot.parameters);
QJsonObject measurement={{"left_raw_pose",toJson(snapshot.leftPose)},{"right_raw_pose",toJson(snapshot.rightPose)},
{"final_origin_pose",toJson(snapshot.finalPose)},{"fusion_consistent",snapshot.fusion.consistent},
{"fusion_position_disparity_mm",snapshot.fusion.positionDisparityMm},
{"fusion_axis_disparity_deg",snapshot.fusion.orientationDisparityDeg},
{"fusion_weight_left",snapshot.fusion.weightLeft},{"fusion_weight_right",snapshot.fusion.weightRight},
{"left_roi_xywh",QJsonArray{snapshot.leftRoi.x(),snapshot.leftRoi.y(),snapshot.leftRoi.width(),snapshot.leftRoi.height()}},
{"right_roi_xywh",QJsonArray{snapshot.rightRoi.x(),snapshot.rightRoi.y(),snapshot.rightRoi.width(),snapshot.rightRoi.height()}}};
m_pendingRecord={{"type","pending_part"},{"capture",c},{"part",part},{"measurement",measurement}};
m_pendingLeft=snapshot.leftImage;m_pendingRight=snapshot.rightImage;m_pendingAssets=m_measureAssets;
const QImage base=snapshot.finalSide==CAM_LEFT?snapshot.leftImage:snapshot.rightImage;
if(m_matcher && !base.isNull()) {
VisionPoseResult drawPose=snapshot.finalPose;
drawPose.tx=point[0].toDouble()/1000;drawPose.ty=point[1].toDouble()/1000;drawPose.tz=point[2].toDouble()/1000;
cv::Mat vis=VisionGuideMatcher::qImageToMat(base);
if(m_matcher->updatePoseProjection(drawPose,snapshot.finalSide,false))m_matcher->drawPoseResult(vis,drawPose);
emit resultImage(QString::fromUtf8("视觉引导 / 零件抓取点"),VisionGuideMatcher::matToQImage(vis));
} else emit resultImage(QString::fromUtf8("视觉引导 / 零件原图"),base);
m_offset->setValue(offset);showVision(part,true);
m_captureStatus->setText(QString::fromUtf8("读取测量%1:%2相机坐标;采图区间 %3 — %4。\n视觉结果已锁定,请填写对应拍照时刻的XYZABC。")
.arg(snapshot.runId).arg(c["camera_coordinate_frame"].toString(),c["capture_begin_utc"].toString(),c["capture_end_utc"].toString()));
} catch(const HException& e){fail(sanitizeUserVisibleLog(QString::fromLocal8Bit(e.ErrorMessage().TextA())));}
catch(const std::exception& e){fail(sanitizeUserVisibleLog(QString::fromUtf8(e.what())));}
}
void VisionGuidanceCapturePanel::showVision(const QJsonObject& pose,bool offset) {
auto raw=pose["pose7_m_deg"].toArray();
auto xyz=pose[offset?"offset_point_camera_mm":"origin_camera_mm"].toArray();
for(int i=0;i<6;++i)m_visionFields[i]->setText(QString::number(i<3?xyz[i].toDouble():raw[i].toDouble(),'f',6));
if(!offset)m_offset->setValue(0);
auto a=pose["z_axis_camera"].toArray();
m_poseStatus->setText(QString::fromUtf8("已得到视觉位姿,尚未保存。poseType=%1;模型 / 板Z轴:[%2, %3, %4]。\n角度按视觉引导原始定义保存,不转换为示教器ABC。")
.arg(pose["pose_type"].toInt()).arg(a[0].toDouble(),0,'f',6).arg(a[1].toDouble(),0,'f',6).arg(a[2].toDouble(),0,'f',6));
}
void VisionGuidanceCapturePanel::saveSelected() {
int type=m_recordType->currentIndex();
if(!m_measurePairConfirmed->isChecked()){fail(QString::fromUtf8("请先核对手输示教器位姿与当前记录的对应关系,并勾选确认。"));return;}
if(type==4){saveTeach();return;}
if(m_pendingRecord.isEmpty()){fail(QString::fromUtf8("请先测量标定板位姿或读取零件位姿。"));return;}
try {
auto c=context(),robot=readRobot(m_photoRobot);robot["entered_utc"]=now();
robot["pairing_method"]="manual_entry_after_visual_result_operator_confirmed";
auto record=m_pendingRecord;auto capture=record["capture"].toObject();record["vision_context_at_measurement"]=capture;
for(auto it=c.begin();it!=c.end();++it)capture[it.key()]=it.value();
capture["photo_robot"]=robot;record["capture"]=capture;
if(type==0){if(!record.contains("board"))throw std::runtime_error("当前结果不是标定板,请重新测量");record["type"]="board_normal";}
else{
if(!record.contains("part"))throw std::runtime_error("当前结果不是零件,请重新读取");
if(c["object_id"].toString().isEmpty())throw std::runtime_error("请填写零件编号");
record["type"]=type==3?"fixed_part_repeat":QString("part_%1").arg(type);
auto part=record["part"].toObject();part["role"]=type;part["object_id"]=c["object_id"];
part["observation_purpose"]=type==3?"fixed_part_multiview_or_repeatability":"two_part_tray_normal";record["part"]=part;
}
QString id=saveRecord(record,m_pendingLeft,m_pendingAssets,m_pendingRight);record["record_id"]=id;
if(type==0)m_boardRecord=record;else if(type<=2)m_partRecords[type-1]=record;
m_poseStatus->setText(QString::fromUtf8("本条视觉位姿与手输XYZABC已同时保存。下次测量请重新获取视觉位姿。"));
m_pendingRecord={};m_measurePairConfirmed->setChecked(false);
}catch(const std::exception& e){fail(sanitizeUserVisibleLog(QString::fromUtf8(e.what())));}
}
void VisionGuidanceCapturePanel::compareNormals() {
try {
if(m_partRecords[0].isEmpty() || m_partRecords[1].isEmpty())throw std::runtime_error("请先保存零件1与零件2");
auto c1=m_partRecords[0]["capture"].toObject(), c2=m_partRecords[1]["capture"].toObject();
if(c1["capture_id"]!=c2["capture_id"])throw std::runtime_error("两个零件需来自同一张锁定左图;跨视角数据需等ABC定义确认后变换");
if(c1["camera_coordinate_frame"]!=c2["camera_coordinate_frame"])throw std::runtime_error("两组结果的相机坐标系不同,需要先变换到统一坐标系");
if(!m_equalHeight->isChecked() || !m_axisParallel->isChecked())throw std::runtime_error("双零件法向需要确认等高与轴线平行于料盘两个条件");
auto p1=m_partRecords[0]["part"].toObject(),p2=m_partRecords[1]["part"].toObject();
if(p1["object_id"]==p2["object_id"])throw std::runtime_error("两个物理零件请使用不同编号");
auto a1=v3(p1["z_axis_camera"].toArray()),a2=v3(p2["z_axis_camera"].toArray());
double axisAngle=std::acos(qBound(0.,std::abs(a1.dot(a2)),1.))*180/CV_PI;
if(axisAngle>5)throw std::runtime_error("两根轴线夹角超过5度;本页简化双零件法仅适用于近似平行的旋转轴");
if(a1.dot(a2)<0)a2=-a2; cv::Vec3d a=a1+a2; a/=cv::norm(a);
auto d=v3(p2["offset_point_camera_mm"].toArray())-v3(p1["offset_point_camera_mm"].toArray());
auto n=a.cross(d); double baseline=cv::norm(n);
if(baseline<1 || baseline<.05*cv::norm(d))throw std::runtime_error("横向基线过小,法向不稳定;请选择相隔更远且非轴向排列的零件");
n/=baseline;
QJsonObject result={{"normal_camera",vec(n[0],n[1],n[2])},{"transverse_baseline_mm",baseline},
{"axis_angle_deg",axisAngle},{"equal_height_operator_confirmed",true},{"axes_parallel_tray_operator_confirmed",true},
{"part1_record_id",m_partRecords[0]["record_id"]},{"part2_record_id",m_partRecords[1]["record_id"]},
{"method","normalize(mean_aligned_axis cross offset_point_baseline)"}};
QString msg=QString::fromUtf8("双零件法向:[%1, %2, %3];横向基线 %4 mm;轴线夹角 %5 deg。")
.arg(n[0],0,'f',6).arg(n[1],0,'f',6).arg(n[2],0,'f',6).arg(baseline,0,'f',3).arg(axisAngle,0,'f',3);
if(!m_boardRecord.isEmpty() && m_boardRecord["capture"].toObject()["capture_id"]==c1["capture_id"]) {
auto bn=v3(m_boardRecord["board"].toObject()["z_axis_camera"].toArray());
double angle=std::acos(qBound(0.,std::abs(n.dot(bn)),1.))*180/CV_PI;
result["board_normal_unsigned_angle_deg"]=angle; result["board_record_id"]=m_boardRecord["record_id"];
msg+=QString::fromUtf8(" 与同图板法向夹角(忽略正负):%1 deg。").arg(angle,0,'f',4);
} else msg+=QString::fromUtf8(" 无同图板法向可对比。");
saveRecord({{"type","two_part_normal"},{"capture",c1},{"normal_analysis",result}},{}); m_normalStatus->setText(msg);
} catch(const std::exception& e) { fail(sanitizeUserVisibleLog(QString::fromUtf8(e.what()))); }
}
QString VisionGuidanceCapturePanel::saveRecord(QJsonObject record,const QImage& image,const QJsonObject& assets,const QImage& rightImage) {
QString root=QDir(m_output->text().trimmed()).absolutePath();
if(m_output->text().trimmed().isEmpty() || !QDir().mkpath(root))throw std::runtime_error("数据目录为空或无法创建");
QString id=QDateTime::currentDateTimeUtc().toString("yyyyMMdd_HHmmss_zzz")+"_"+record["type"].toString()+"_"+uid();
QTemporaryDir pending(QDir(root).filePath(".pending_XXXXXX"));
if(!pending.isValid())throw std::runtime_error("数据目录不可写");
record["schema"]="vision_guidance_capture_v1"; record["record_id"]=id; record["saved_utc"]=now(); record["assets"]=assets;
if(!image.isNull()) {
QString raw=pending.filePath("left_raw.png"); if(!image.save(raw,"PNG"))throw std::runtime_error("原始图像保存失败");
QFile f(raw); if(!f.open(QIODevice::ReadOnly))throw std::runtime_error("无法校验原始图像");
record["image_file"]="left_raw.png"; QCryptographicHash hash(QCryptographicHash::Sha256); hash.addData(&f); record["image_sha256"]=QString::fromLatin1(hash.result().toHex());
}
if(!rightImage.isNull()) {
QString raw=pending.filePath("right_raw.png"); if(!rightImage.save(raw,"PNG"))throw std::runtime_error("右图保存失败");
QFile f(raw);if(!f.open(QIODevice::ReadOnly))throw std::runtime_error("无法校验右图");
QCryptographicHash hash(QCryptographicHash::Sha256);hash.addData(&f);
record["right_image_file"]="right_raw.png";record["right_image_sha256"]=QString::fromLatin1(hash.result().toHex());
}
QSaveFile json(pending.filePath("record.json")); QByteArray bytes=QJsonDocument(sanitizeUserVisibleRecord(record).toObject()).toJson(QJsonDocument::Indented);
if(!json.open(QIODevice::WriteOnly) || json.write(bytes)!=bytes.size() || !json.commit())throw std::runtime_error("记录JSON保存失败");
// Publish image and metadata together; failed writes remain invisible to the history.
if(!QDir(root).rename(QFileInfo(pending.path()).fileName(),id))throw std::runtime_error("发布记录失败,未显示保存成功");
pending.setAutoRemove(false);
auto& s=AppSettings::instance(); s.set("guidance/output",root); s.set("guidance/cpd",m_cpd->text().trimmed());
m_saveStatus->setText(QString::fromUtf8("已保存:")+QDir(root).filePath(id+"/record.json"));
if(m_log)m_log(QString::fromUtf8("[视觉引导采集] 已保存 ")+record["type"].toString()+": "+QDir(root).filePath(id)); refreshHistory(); return id;
}
void VisionGuidanceCapturePanel::refreshHistory() {
m_history->setRowCount(0); QDir root(m_output->text().trimmed());
auto names=root.entryList(QDir::Dirs|QDir::NoDotAndDotDot,QDir::Name|QDir::Reversed);
for(const QString& name:names) {
if(name.startsWith('.'))continue;
QFile f(root.filePath(name+"/record.json")); if(!f.open(QIODevice::ReadOnly))continue;
auto r=QJsonDocument::fromJson(f.readAll()).object(); if(r["schema"]!="vision_guidance_capture_v1")continue;
auto c=r.contains("capture")?r["capture"].toObject():r["context"].toObject();
QString object=r.contains("part")?r["part"].toObject()["object_id"].toString():c["object_id"].toString();
int row=m_history->rowCount(); m_history->insertRow(row);
QString type=r["type"].toString(); if(type=="board_normal")type=QString::fromUtf8("标定板"); else if(type=="photo_observation")type=QString::fromUtf8("原始拍照"); else if(type=="fixed_part_repeat")type=QString::fromUtf8("固定零件重复"); else if(type=="desired_grasp_teach")type=QString::fromUtf8("示教参考"); else if(type=="two_part_normal")type=QString::fromUtf8("双零件法向"); else type.replace("part_",QString::fromUtf8("零件"));
QStringList values={QDateTime::fromString(r["saved_utc"].toString(),Qt::ISODateWithMs).toLocalTime().toString("MM-dd HH:mm:ss"),type,object+" / "+c["view_id"].toString()};
for(int col=0;col<3;++col){auto item=new QTableWidgetItem(values[col]); item->setToolTip(root.filePath(name+"/record.json"));m_history->setItem(row,col,item);}
}
}