Files
VisionGuide/source/StereoPoseFusion.cpp
T

315 lines
12 KiB
C++
Raw Normal View History

2026-10-26 10:56:56 +08:00
//StereoPoseFusion.cpp
#include "UserVisibleLog.h"
#include "StereoPoseFusion.h"
#define NOMINMAX
#include <VisionGuideApi.h>
#include <cmath>
#include <algorithm>
#include <stdexcept>
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<Impl>()) {}
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;
}