315 lines
12 KiB
C++
315 lines
12 KiB
C++
//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;
|
||
|
|
}
|