//StereoPoseFusion.h #pragma once #include #include #include #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 m_d; bool m_loaded = false; QString m_relPoseFile; };