#ifndef NOMINMAX #define NOMINMAX #endif #include "BoardPoseEstimator.h" #include #include #include #include #include #include #include #include #include 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=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<