first Commit
This commit is contained in:
@@ -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;
|
||||
};
|
||||
Reference in New Issue
Block a user