//StereoPoseFusion.cpp #include "UserVisibleLog.h" #include "StereoPoseFusion.h" #define NOMINMAX #include #include #include #include using namespace VisionGuideApi; struct StereoPoseFusion::Impl { HTuple poseR2L; // pos_right2left.dat 的内容 (7-tuple) HTuple hmatR2L; // 其 4x4 齐次矩阵 static HTuple poseToHTuple(const VisionPoseResult& p) { HTuple t; t.Append(p.tx); t.Append(p.ty); t.Append(p.tz); t.Append(p.rx); t.Append(p.ry); t.Append(p.rz); t.Append((Hlong)p.poseType); return t; } static void htupleToPose(const HTuple& pt, VisionPoseResult& out) { out.tx = pt[0].D(); out.ty = pt[1].D(); out.tz = pt[2].D(); out.rx = pt[3].D(); out.ry = pt[4].D(); out.rz = pt[5].D(); out.poseType = (pt.Length() > 6) ? pt[6].I() : 0; } }; StereoPoseFusion::StereoPoseFusion(QObject* parent) : QObject(parent), m_d(std::make_unique()) {} StereoPoseFusion::~StereoPoseFusion() = default; bool StereoPoseFusion::loadRightToLeftPose(const QString& file) { try { ReadPose(HTuple(file.toLocal8Bit().constData()), &m_d->poseR2L); PoseToHomMat3d(m_d->poseR2L, &m_d->hmatR2L); m_loaded = true; m_relPoseFile = file; emit logMessage(QString::fromUtf8("[Fusion] 相对位姿加载成功: ") + file); return true; } catch (HException& e) { m_loaded = false; emit logMessage(QString::fromUtf8("[Fusion] 相对位姿加载失败: ") + sanitizeUserVisibleLog(QString::fromLocal8Bit(e.ErrorMessage().TextA()))); return false; } } VisionPoseResult StereoPoseFusion::transformRightToLeft(const VisionPoseResult& poseR) const { VisionPoseResult out = poseR; // 继承 score / std / valid if (!m_loaded) { out.valid = false; out.errorMsg = QString::fromUtf8("stereo 外参未加载"); return out; } if (!poseR.valid) return out; try { HTuple pR = Impl::poseToHTuple(poseR); HTuple hR; PoseToHomMat3d(pR, &hR); HTuple hL; HomMat3dCompose(m_d->hmatR2L, hR, &hL); // T_L = T_L_R * T_R HTuple pL; HomMat3dToPose(hL, &pL); Impl::htupleToPose(pL, out); // Pixel projections in the right image cannot be reused in the left frame. out.projectedContours.clear();out.originRow=out.originCol=0; out.xTipRow=out.xTipCol=out.yTipRow=out.yTipCol=out.zTipRow=out.zTipCol=0; const double vars[3] = {poseR.stdTx*poseR.stdTx,poseR.stdTy*poseR.stdTy,poseR.stdTz*poseR.stdTz}; double sig[3] = {}; for (int row=0;row<3;++row) { double v=0; for (int col=0;col<3;++col) { double a=m_d->hmatR2L[row*4+col].D(); v+=a*a*vars[col]; } sig[row]=std::sqrt(v); } out.stdTx=sig[0]; out.stdTy=sig[1]; out.stdTz=sig[2]; out.valid = true; } catch (HException& e) { out.valid = false; out.errorMsg = sanitizeUserVisibleLog(QString::fromLocal8Bit(e.ErrorMessage().TextA())); } return out; } struct Vec3 { double x = 0.0, y = 0.0, z = 1.0; }; static Vec3 normalizeVec(Vec3 v) { double n = std::sqrt(v.x * v.x + v.y * v.y + v.z * v.z); if (n < 1e-12) return Vec3{ 0.0, 0.0, 1.0 }; return Vec3{ v.x / n, v.y / n, v.z / n }; } static Vec3 symmetryAxisInCamera(const VisionPoseResult& p) { try { HTuple h; HTuple pose; 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((Hlong)p.poseType); PoseToHomMat3d(pose, &h); if (h.Length() >= 11) { // VisionGuide 3D hom-mat is row-major 3x4. The third rotation column // is the model Z axis in camera coordinates. For this rotary part, // model Z is treated as the symmetry axis. return normalizeVec(Vec3{ h[2].D(), h[6].D(), h[10].D() }); } } catch (...) { } return Vec3{ 0.0, 0.0, 1.0 }; } static double symmetryAxisAngleDeg(const VisionPoseResult& a, const VisionPoseResult& b) { Vec3 va = symmetryAxisInCamera(a); Vec3 vb = symmetryAxisInCamera(b); double dot = va.x * vb.x + va.y * vb.y + va.z * vb.z; // A rotary body cannot distinguish axis direction in many views, so +axis // and -axis are treated as equivalent. dot = std::max(-1.0, std::min(1.0, std::abs(dot))); return std::acos(dot) * 180.0 / std::acos(-1.0); } StereoPoseFusion::FusionResult StereoPoseFusion::fuse(const VisionPoseResult& poseL, const VisionPoseResult& poseR_raw, FusionMethod method, double posDispMaxMm, double orientDispMaxDeg,const QString& leftCameraFile,const QString& rightCameraFile) const { FusionResult r; r.leftInLeft = poseL; r.rightInLeft = m_loaded ? transformRightToLeft(poseR_raw) : poseR_raw; if(!m_loaded){r.rightInLeft.valid=false;r.rightInLeft.errorMsg=QString::fromUtf8("双目外参未加载,不能将右路作为左系输出");} const bool L_ok = poseL.valid; const bool R_ok = r.rightInLeft.valid; if (method == FM_LeftOnly || method == FM_RightOnly) { const bool left = method == FM_LeftOnly; r.fused = left ? poseL : r.rightInLeft; r.valid = r.fused.valid; r.weightLeft = left && r.valid ? 1.0 : 0.0; r.weightRight = !left && r.valid ? 1.0 : 0.0; r.message = left ? QStringLiteral("fixed LEFT pose") : QStringLiteral("fixed RIGHT pose in LEFT frame"); return r; } // 退化情况 if (!L_ok && !R_ok) { r.message = QString::fromUtf8("左右位姿均无效"); return r; } if (!L_ok) { r.fused = r.rightInLeft; r.valid = true; r.weightLeft = 0; r.weightRight = 1; r.confidence = 0.40 * std::max(0.0, poseR_raw.score); r.message = QString::fromUtf8("仅右相机有效"); return r; } if (!R_ok || !m_loaded) { r.fused = poseL; r.valid = true; r.weightLeft = 1; r.weightRight = 0; r.confidence = 0.40 * std::max(0.0, poseL.score); r.message = m_loaded ? QString::fromUtf8("仅左相机有效") : QString::fromUtf8("外参未加载,使用左相机"); return r; } // 一致性 const double dx = (poseL.tx - r.rightInLeft.tx) * 1000.0; const double dy = (poseL.ty - r.rightInLeft.ty) * 1000.0; const double dz = (poseL.tz - r.rightInLeft.tz) * 1000.0; r.positionDisparityMm = std::sqrt(dx*dx + dy * dy + dz * dz); r.orientationDisparityDeg = symmetryAxisAngleDeg(poseL, r.rightInLeft); r.consistent = (r.positionDisparityMm < posDispMaxMm) && (r.orientationDisparityDeg <= orientDispMaxDeg); QString reprojectionMessage; if(!leftCameraFile.isEmpty()||!rightCameraFile.isEmpty()){ r.reprojectionChecked=true; try{ HTuple camL,camR,inverse,x,y,z; ReadCamPar(leftCameraFile.toLocal8Bit().constData(),&camL); ReadCamPar(rightCameraFile.toLocal8Bit().constData(),&camR); HomMat3dInvert(m_d->hmatR2L,&inverse); AffineTransPoint3d(inverse,poseL.tx,poseL.ty,poseL.tz,&x,&y,&z); auto distance=[](const HTuple& camera,double ax,double ay,double az,double bx,double by,double bz){ if(az<=0||bz<=0)throw std::runtime_error("Projected point behind camera"); HTuple ar,ac,br,bc;Project3dPoint(ax,ay,az,camera,&ar,&ac);Project3dPoint(bx,by,bz,camera,&br,&bc); return std::hypot(ar[0].D()-br[0].D(),ac[0].D()-bc[0].D()); }; r.leftReprojectionErrorPx=distance(camL,poseL.tx,poseL.ty,poseL.tz,r.rightInLeft.tx,r.rightInLeft.ty,r.rightInLeft.tz); r.rightReprojectionErrorPx=distance(camR,poseR_raw.tx,poseR_raw.ty,poseR_raw.tz,x[0].D(),y[0].D(),z[0].D()); r.reprojectionPassed=std::isfinite(r.leftReprojectionErrorPx)&&std::isfinite(r.rightReprojectionErrorPx)&&r.leftReprojectionErrorPx<=r.reprojectionLimitPx&&r.rightReprojectionErrorPx<=r.reprojectionLimitPx; reprojectionMessage=QStringLiteral(" reprojection L=%1px R=%2px limit=%3px passed=%4").arg(r.leftReprojectionErrorPx,0,'f',2).arg(r.rightReprojectionErrorPx,0,'f',2).arg(r.reprojectionLimitPx).arg(r.reprojectionPassed?"true":"false"); }catch(const HException& e){reprojectionMessage=QStringLiteral(" reprojection failed: ")+sanitizeUserVisibleLog(QString::fromLocal8Bit(e.ErrorMessage().TextA()));} catch(const std::exception& e){reprojectionMessage=QStringLiteral(" reprojection failed: ")+sanitizeUserVisibleLog(QString::fromUtf8(e.what()));} r.consistent=r.consistent&&r.reprojectionPassed; } // 权重 double wL = 0.5, wR = 0.5; switch (method) { case FM_LeftOnly: wL = 1.0; wR = 0.0; break; case FM_RightOnly: wL = 0.0; wR = 1.0; break; case FM_BestScore: wL = (poseL.score >= poseR_raw.score) ? 1.0 : 0.0; wR = 1.0 - wL; break; case FM_WeightedScore: { double sL = std::max(0.0, poseL.score); double sR = std::max(0.0, poseR_raw.score); double s = sL + sR; wL = (s > 1e-6) ? sL / s : 0.5; wR = 1.0 - wL; break; } case FM_InverseVariance: { auto avgSig = [](const VisionPoseResult& p) { return std::sqrt((p.stdTx*p.stdTx+p.stdTy*p.stdTy+p.stdTz*p.stdTz)/3.0); }; const double rawL=avgSig(poseL), rawR=avgSig(r.rightInLeft); if (!std::isfinite(rawL) || !std::isfinite(rawR) || rawL<=0 || rawR<=0) { const double s=std::max(0.0,poseL.score)+std::max(0.0,poseR_raw.score); wL=s>0?std::max(0.0,poseL.score)/s:0.5; wR=1-wL; break; // Missing uncertainty is not perfect precision. } double sigL = std::max(1e-2, avgSig(poseL)); // floor 0.01 mm double sigR = std::max(1e-2, avgSig(r.rightInLeft)); double wLv = 1.0 / (sigL * sigL); double wRv = 1.0 / (sigR * sigR); // 再乘以 score 做软加权 wLv *= std::max(0.0, poseL.score); wRv *= std::max(0.0, poseR_raw.score); double s = wLv + wRv; if (s > 1e-12) { wL = wLv / s; wR = wRv / s; } break; } } // An inconsistent pair always preserves the complete LEFT observation. if (!r.consistent) { wL = 1.0;wR = 0.0; r.message = QString::fromUtf8("两路差异过大 (Δt=%1mm Δr=%2°) → 保留左路") .arg(r.positionDisparityMm, 0, 'f', 2) .arg(r.orientationDisparityDeg, 0, 'f', 2); } else { r.message = QString::fromUtf8("融合 (Δt=%1mm Δr=%2° wL=%3 wR=%4)") .arg(r.positionDisparityMm, 0, 'f', 2) .arg(r.orientationDisparityDeg, 0, 'f', 2) .arg(wL, 0, 'f', 2).arg(wR, 0, 'f', 2); } r.message = r.consistent ? QStringLiteral("rotary fusion OK (dt=%1mm daxis=%2deg wL=%3 wR=%4)") .arg(r.positionDisparityMm, 0, 'f', 2) .arg(r.orientationDisparityDeg, 0, 'f', 2) .arg(wL, 0, 'f', 2).arg(wR, 0, 'f', 2) : QStringLiteral("rotary fusion rejected (dt=%1mm daxis=%2deg), keep LEFT") .arg(r.positionDisparityMm, 0, 'f', 2) .arg(r.orientationDisparityDeg, 0, 'f', 2); r.message+=reprojectionMessage; r.weightLeft = wL; r.weightRight = wR; if(!r.consistent){r.fused=poseL;r.valid=true;r.confidence=.4*std::max(0.0,poseL.score);return r;} // Position is averaged; orientation and its uncertainty are selected from one view. VisionPoseResult& f = r.fused; const auto& R_ = r.rightInLeft; f.tx = wL * poseL.tx + wR * R_.tx; f.ty = wL * poseL.ty + wR * R_.ty; f.tz = wL * poseL.tz + wR * R_.tz; const VisionPoseResult& orientRef = wR<=0 ? poseL : (wL<=0 ? R_ : ((poseR_raw.score > poseL.score) ? R_ : poseL)); f.rx = orientRef.rx; f.ry = orientRef.ry; f.rz = orientRef.rz; f.poseType = orientRef.poseType; f.score = std::max(poseL.score, poseR_raw.score); f.matchTimeMs = poseL.matchTimeMs + poseR_raw.matchTimeMs; // Translation uncertainty uses the actual weights and a rotated diagonal approximation. auto weightedSigma=[&](double l,double r){return std::sqrt(wL*wL*l*l+wR*wR*r*r);}; const bool known=(poseL.stdTx+poseL.stdTy+poseL.stdTz)>0 && (R_.stdTx+R_.stdTy+R_.stdTz)>0; f.stdTx=known?weightedSigma(poseL.stdTx,R_.stdTx):0; f.stdTy=known?weightedSigma(poseL.stdTy,R_.stdTy):0; f.stdTz=known?weightedSigma(poseL.stdTz,R_.stdTz):0; // Orientation is selected from one view, not fused; keep that view's uncertainty. f.stdRx=orientRef.stdRx;f.stdRy=orientRef.stdRy;f.stdRz=orientRef.stdRz; if (wL==0 || wR==0) { f.stdTx=orientRef.stdTx; f.stdTy=orientRef.stdTy; f.stdTz=orientRef.stdTz; f.stdRx=orientRef.stdRx; f.stdRy=orientRef.stdRy; f.stdRz=orientRef.stdRz; } // Averaging changes the pose; callers must project this pose with its camera. f.projectedContours.clear();f.originRow=f.originCol=0; f.xTipRow=f.xTipCol=f.yTipRow=f.yTipCol=f.zTipRow=f.zTipCol=0; f.valid = true; // 置信度:score 均值 × 一致性因子 double avgScore = 0.5 * (poseL.score + poseR_raw.score); double posFactor = std::max(0.0, 1.0 - r.positionDisparityMm / std::max(1e-6, posDispMaxMm)); double axisFactor = std::max(0.0, 1.0 - r.orientationDisparityDeg / std::max(1e-6, orientDispMaxDeg)); double consistFactor = r.consistent ? std::min(posFactor, axisFactor) : 0.3; r.confidence = std::max(0.0, std::min(1.0, avgScore * consistFactor)); r.valid = true; return r; }