first Commit

This commit is contained in:
lidg
2026-10-26 10:56:56 +08:00
commit c8cd533d9b
81 changed files with 128540 additions and 0 deletions
+66
View File
@@ -0,0 +1,66 @@
//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;
};