67 lines
2.1 KiB
C++
67 lines
2.1 KiB
C++
//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;
|
||
};
|