Files
VisionGuide/source/StereoPoseFusion.h
T
2026-10-26 10:56:56 +08:00

67 lines
2.1 KiB
C++
Raw Blame History

This file contains ambiguous Unicode characters
This file contains Unicode characters that might be confused with other characters. If you think that this is intentional, you can safely ignore this warning. Use the Escape button to reveal them.
//StereoPoseFusion.h
#pragma once
#include <QObject>
#include <QString>
#include <memory>
#include "VisionGuideMatcher.h"
/**
* 将左右相机各自估计出的位姿融合为一个在"左相机系"下的最终位姿。
*
* 输出始终在左相机系;世界/机器人坐标仍需另外应用手眼变换。
*/
class StereoPoseFusion : public QObject
{
Q_OBJECT
public:
enum FusionMethod {
FM_BestScore = 0, // 选 score 高的那一路
FM_WeightedScore = 1, // 按 score 线性加权
FM_InverseVariance = 2, // 按 VisionGuide covPose 逆方差加权(推荐)
FM_LeftOnly = 3,
FM_RightOnly = 4,
};
struct FusionResult {
bool valid = false;
VisionPoseResult fused; // 融合位姿(在 LEFT 相机系)
VisionPoseResult leftInLeft; // 原始左位姿
VisionPoseResult rightInLeft; // 右位姿被外参搬到 LEFT 系
double positionDisparityMm = 0; // ‖t_L − t_R_inL‖
double orientationDisparityDeg = 0; // max(|Δrx|, |Δry|, |Δrz|)
double confidence = 0; // 0..1
bool consistent = false; // 是否通过一致性阈值
double weightLeft = 0, weightRight = 0;
bool reprojectionChecked = false, reprojectionPassed = false;
double leftReprojectionErrorPx = -1, rightReprojectionErrorPx = -1;
double reprojectionLimitPx = 3.0;
QString message;
};
explicit StereoPoseFusion(QObject* parent = nullptr);
~StereoPoseFusion();
bool loadRightToLeftPose(const QString& relPoseFile);
bool isReady() const { return m_loaded; }
QString relPoseFile() const { return m_relPoseFile; }
VisionPoseResult transformRightToLeft(const VisionPoseResult& poseInRight) const;
FusionResult fuse(const VisionPoseResult& leftPose,
const VisionPoseResult& rightPose,
FusionMethod method = FM_InverseVariance,
double posDispMaxMm = 1.0,
double orientDispMaxDeg = 10.0,
const QString& leftCameraFile = QString(),
const QString& rightCameraFile = QString()) const;
signals:
void logMessage(const QString& msg);
private:
struct Impl;
std::unique_ptr<Impl> m_d;
bool m_loaded = false;
QString m_relPoseFile;
};