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;
|
|||
|
|
};
|