118 lines
7.0 KiB
C++
118 lines
7.0 KiB
C++
#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>
|
|
|
|
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;
|
|
}
|