Files
VisionGuide/source/BoardPoseEstimator.cpp
T

124 lines
7.0 KiB
C++
Raw Normal View History

2026-10-26 10:56:56 +08:00
#ifndef NOMINMAX
#define NOMINMAX
#endif
#include "BoardPoseEstimator.h"
#include <VisionGuideApi.h>
#include <QJsonArray>
#include <QPainter>
#include <QPainterPath>
#include <QPolygonF>
#include <QFont>
#include <cmath>
#include <cstring>
#include <stdexcept>
2026-10-27 22:14:53 +08:00
2026-10-26 10:56:56 +08:00
using namespace VisionGuideApi;
namespace {
struct Handle {
HTuple value;
~Handle(){if(value.Length())try{ClearCalibData(value);}catch(...) {}}
};
QJsonArray xyz(double x,double y,double z){return {x,y,z};}
}
BoardPoseEstimate estimateBoardPose(const QImage& image,const QString& cameraFile,
const QString& descriptionFile,double axisLengthMm,double sigma)
{
if(image.isNull() || axisLengthMm<=0 || !std::isfinite(axisLengthMm))
throw std::runtime_error("图像为空或坐标轴长度无效");
HTuple cam;ReadCamPar(cameraFile.toLocal8Bit().constData(),&cam);
if(cam[cam.Length()-2].I()!=image.width() || cam[cam.Length()-1].I()!=image.height())
throw std::runtime_error("图像分辨率与相机内参不一致,请使用原始图像");
Handle handle;CreateCalibData("calibration_object",1,1,&handle.value);
SetCalibDataCamParam(handle.value,0,HTuple(),cam);
SetCalibDataCalibObject(handle.value,0,descriptionFile.toLocal8Bit().constData());
QImage gray=image.convertToFormat(QImage::Format_Grayscale8);
QByteArray packed(gray.width()*gray.height(),Qt::Uninitialized);
for(int y=0;y<gray.height();++y)memcpy(packed.data()+y*gray.width(),gray.constScanLine(y),gray.width());
HObject hImage;GenImage1(&hImage,"byte",gray.width(),gray.height(),(Hlong)packed.constData());
FindCalibObject(hImage,handle.value,0,0,0,"sigma",sigma);
HTuple pose,rows,cols,idx,rough;GetCalibDataObservPose(handle.value,0,0,0,&pose);
GetCalibDataObservPoints(handle.value,0,0,0,&rows,&cols,&idx,&rough);
if(pose.Length()!=7 || !rows.Length())throw std::runtime_error("未得到有效的标定板位姿与标记点");
HTuple h;PoseToHomMat3d(pose,&h);QJsonArray raw,matrix,marks;
for(int i=0;i<7;++i)raw.append(pose[i].D());
for(int i=0;i<12;++i)matrix.append(h[i].D());
HTuple ox,oy,oz;GetCalibData(handle.value,"calib_obj",0,"x",&ox);
GetCalibData(handle.value,"calib_obj",0,"y",&oy);GetCalibData(handle.value,"calib_obj",0,"z",&oz);
double sum=0,maxErr=0;
for(int i=0;i<idx.Length();++i){
int k=idx[i].I();if(k<0 || k>=ox.Length())throw std::runtime_error("标记点索引越界");
HTuple x,y,z,r,c;AffineTransPoint3d(h,ox[k],oy[k],oz[k],&x,&y,&z);
Project3dPoint(x,y,z,cam,&r,&c);
double d=std::hypot(r.D()-rows[i].D(),c.D()-cols[i].D());sum+=d*d;maxErr=qMax(maxErr,d);
marks.append(QJsonObject{{"row",rows[i].D()},{"column",cols[i].D()},{"index",k}});
}
BoardPoseEstimate result;
result.data={{"pose7_m_deg",raw},{"pose_type",pose[6].I()},
{"T_camera_object_row_major_3x4_m",matrix},
{"origin_camera_mm",xyz(pose[0].D()*1000,pose[1].D()*1000,pose[2].D()*1000)},
{"x_axis_camera",xyz(h[0].D(),h[4].D(),h[8].D())},
{"y_axis_camera",xyz(h[1].D(),h[5].D(),h[9].D())},
{"z_axis_camera",xyz(h[2].D(),h[6].D(),h[10].D())},
{"detected_mark_count",int(rows.Length())},{"detected_marks",marks},
{"reprojection_rms_px",std::sqrt(sum/idx.Length())},{"reprojection_max_px",maxErr},
{"coordinate_frame","board_description_origin_and_axes"},{"axis_length_mm",axisLengthMm},
{"detection_sigma",sigma},
{"axis_colors",QJsonObject{{"X","red"},{"Y","green"},{"Z","blue"}}}};
result.axesImage=drawBoardCoordinateAxes(image,result.data,cameraFile);
return result;
}
QImage drawBoardCoordinateAxes(const QImage& image,QJsonObject& data,const QString& cameraFile) {
HTuple cam,h;ReadCamPar(cameraFile.toLocal8Bit().constData(),&cam);
auto matrix=data["T_camera_object_row_major_3x4_m"].toArray();
if(matrix.size()!=12)throw std::runtime_error("缺少标定板变换矩阵");
for(int i=0;i<12;++i)h[i]=matrix[i].toDouble();
double axisLengthMm=data["axis_length_mm"].toDouble(20.);
auto project=[&](double x,double y,double z){
HTuple cx,cy,cz,row,col;AffineTransPoint3d(h,x,y,z,&cx,&cy,&cz);
if(cz.D()<=0)throw std::runtime_error("坐标轴端点位于相机后方");
Project3dPoint(cx,cy,cz,cam,&row,&col);return QPointF(col.D(),row.D());
};
QPointF origin=project(0,0,0);QJsonArray projected;
data["origin_pixel_xy"]=QJsonArray{origin.x(),origin.y()};
QImage axesImage=image.convertToFormat(QImage::Format_RGB32);
QPainter painter(&axesImage);painter.setRenderHint(QPainter::Antialiasing,true);
double scale=qMax(1.,image.width()/1200.);QFont font; font.setPixelSize(int(17*scale));font.setBold(true);painter.setFont(font);
auto label=[&](QPointF p,const QString& text,const QColor& color){
QPainterPath path;path.addText(p,font,text);
painter.strokePath(path,QPen(Qt::black,2.2*scale,Qt::SolidLine,Qt::RoundCap,Qt::RoundJoin));
painter.fillPath(path,color);
};
const QColor colors[]={QColor(255,65,65),QColor(40,230,95),QColor(65,145,255)};
const char* labels[]={"+X","+Y","+Z"};double length=axisLengthMm/1000;
for(int axis=0;axis<3;++axis){
QPointF end=project(axis==0?length:0,axis==1?length:0,axis==2?length:0);
projected.append(QJsonObject{{"axis",labels[axis]},{"end_pixel_xy",QJsonArray{end.x(),end.y()}}});
// A straight 3D axis becomes a curved image trace under distortion. Sample it.
QPainterPath path;path.moveTo(origin);
for(int k=1;k<=24;++k){double d=length*k/24;path.lineTo(project(axis==0?d:0,axis==1?d:0,axis==2?d:0));}
painter.setPen(QPen(Qt::black,5.4*scale,Qt::SolidLine,Qt::RoundCap));painter.setBrush(Qt::NoBrush);painter.drawPath(path);
painter.setPen(QPen(colors[axis],3.2*scale,Qt::SolidLine,Qt::RoundCap));painter.drawPath(path);
auto tangent=end-project(axis==0?length*.94:0,axis==1?length*.94:0,axis==2?length*.94:0);
double norm=std::hypot(tangent.x(),tangent.y());
if(norm>1){QPointF u=tangent/norm,v(-u.y(),u.x());QPolygonF arrow;
arrow<<end<<(end-u*11*scale+v*5*scale)<<(end-u*11*scale-v*5*scale);
painter.setPen(QPen(Qt::black,1.3*scale));painter.setBrush(colors[axis]);painter.drawPolygon(arrow);}
label(end+QPointF(7*scale,-7*scale),QString::fromLatin1(labels[axis]),colors[axis]);
}
painter.setPen(QPen(Qt::black,1.5*scale));painter.setBrush(Qt::white);painter.drawEllipse(origin,4*scale,4*scale);
label(origin+QPointF(-19*scale,22*scale),"O",Qt::white);
painter.fillRect(QRectF(8*scale,8*scale,625*scale,66*scale),QColor(0,0,0,180));
label(QPointF(17*scale,32*scale),QString("Board frame | +X red +Y green +Z blue | %1 mm axes").arg(axisLengthMm),Qt::white);
label(QPointF(17*scale,61*scale),QString("Marks %1 | reprojection RMS %2 px | poseType %3")
.arg(data["detected_mark_count"].toInt()).arg(data["reprojection_rms_px"].toDouble(),0,'f',4).arg(data["pose_type"].toInt()),Qt::white);
painter.end();data["projected_axes"]=projected;return axesImage;
}