polhemus_coregistration.h

Go to the documentation of this file.

//=============================================================================================================
 
#ifndef UTILS_POLHEMUS_COREGISTRATION_H
#define UTILS_POLHEMUS_COREGISTRATION_H
 
//=============================================================================================================
// INCLUDES
//=============================================================================================================
 
#include "../utils_global.h"
#include "acquired_points.h"
#include "polhemus_connection.h"
 
//=============================================================================================================
// QT INCLUDES
//=============================================================================================================
 
#include <QMatrix4x4>
#include <QObject>
#include <QQuaternion>
#include <QSettings>
#include <QVector3D>
 
#include <vector>
 
//=============================================================================================================
// DEFINE NAMESPACE UTILSLIB
//=============================================================================================================
 
namespace UTILSLIB
{
 
//=============================================================================================================
class UTILSSHARED_EXPORT PolhemusCoregistration : public QObject
{
    Q_OBJECT
 
public:
    explicit PolhemusCoregistration(QObject* parent = nullptr);
    ~PolhemusCoregistration() override = default;
 
    //=========================================================================================================
    // Station assignment
    //=========================================================================================================
 
    void setTrackerStation(int station);
    void setPenStation(int station);
    void setProbeStation(int station);
 
    int trackerStation() const { return m_trackerStation; }
    int penStation()     const { return m_penStation; }
    int probeStation()   const { return m_probeStation; }
 
    //=========================================================================================================
    // Pen tip offset (sensor body frame → tip)
    //=========================================================================================================
 
    void setPenTipOffset(const QVector3D& offset) { m_penTipOffset = offset; }
    QVector3D penTipOffset() const { return m_penTipOffset; }
    void setTipOffsetEnabled(bool on) { m_tipOffsetEnabled = on; }
    bool tipOffsetEnabled() const { return m_tipOffsetEnabled; }
 
    //=========================================================================================================
    // Axis mirroring — compensate for transmitter placement
    //=========================================================================================================

    void setAxisMirror(bool mirrorX, bool mirrorY) { m_mirrorX = mirrorX; m_mirrorY = mirrorY; }
    bool mirrorX() const { return m_mirrorX; }
    bool mirrorY() const { return m_mirrorY; }
 
    //=========================================================================================================
    // Pivot calibration — determine pen tip offset by pivoting pen around its tip
    //=========================================================================================================
 
    enum class PivotState { Idle, WaitingForStart, Collecting, Done };
 
    void startPivotCalibration();
    void cancelPivotCalibration();
    PivotState pivotState() const { return m_pivotState; }
    int pivotSampleCount() const { return static_cast<int>(m_pivotPositions.size()); }
    float pivotResidualMm() const { return m_pivotResidualMm; }
 
    //=========================================================================================================
    // Tracker-to-device calibration offset
    //=========================================================================================================

    void setTrackerToDeviceOffset(const QVector3D& translation,
                                  const QQuaternion& rotation);
 
    QVector3D   trackerToDeviceTranslation() const { return m_offsetTranslation; }
    QQuaternion trackerToDeviceRotation()    const { return m_offsetRotation; }
 
    //=========================================================================================================
    // Optical path calibration — determine the microscope optical axis
    // relative to the tracker sensor.
    //
    // Workflow:
    //   1. Move the OPMI to a position, aim at a point on the head surface,
    //      mark the focus point with the stylus pen → captureOpticalCalibSample().
    //   2. Repeat at 2–3 different positions / focal distances.
    //   3. Call solveOpticalCalibration() to fit a 3D line through the focus
    //      points in the tracker's local frame, yielding the optical axis
    //      direction and the optical center offset from the tracker.
    //=========================================================================================================
 
    struct OpticalCalibSample {
        QVector3D   trackerPos;    
        QQuaternion trackerOri;    
        QVector3D   focusPoint;    
    };

    bool captureOpticalCalibSample();

    int opticalCalibSampleCount() const { return static_cast<int>(m_opticalCalibSamples.size()); }

    void clearOpticalCalibSamples();

    bool captureObjectiveCenter();

    bool hasObjectiveCenter() const { return m_hasObjectiveCenter; }

    void clearObjectiveCenter();

    QVector3D objectiveCenterLocal() const { return m_objectiveCenterLocal; }

    bool solveOpticalCalibration();

    void setKnownTrackerToObjectiveDistance(float metres) { m_knownTrackerToObjectiveDist = metres; }
    float knownTrackerToObjectiveDistance() const { return m_knownTrackerToObjectiveDist; }

    bool opticalCalibrationValid() const { return m_opticalCalibValid; }

    QVector3D opticalAxisLocal() const { return m_opticalAxisLocal; }

    QVector3D opticalCenterLocal() const { return m_opticalCenterLocal; }

    float opticalCalibResidualMm() const { return m_opticalCalibResidualMm; }

    float opticalCalibDepthSpreadMm() const { return m_opticalCalibDepthSpreadMm; }

    bool applyOpticalAxisFineAdjust(const QVector3D& targetWorldPos,
                                    float& correctionDeg);

    bool opticalFineAdjustApplied() const { return m_opticalFineAdjustApplied; }

    float opticalFineAdjustDeg() const { return m_opticalFineAdjustDeg; }

    void clearOpticalFineAdjust();

    bool opticalRayInWorld(QVector3D& origin, QVector3D& direction) const;

    bool opticalUpInWorld(QVector3D& up) const;
 
    //=========================================================================================================
    // Digitizer connection
    //=========================================================================================================
 
    void setConnection(PolhemusConnection* conn);
    PolhemusConnection* connection() const { return m_pConn; }
 
    //=========================================================================================================
    // Digitized-point store
    //=========================================================================================================
 
    AcquiredPoints* acquiredPoints() const { return m_pPoints; }
 
    //=========================================================================================================
    // Explicit capture API
    //=========================================================================================================
 
    bool captureCurrentPenPositionAsFiducial(FiducialId id);
    bool captureCurrentPenPositionAsHeadShape();
 
    //=========================================================================================================
    // Model (BEM / FIFF) fiducials — target landmarks for SVD coregistration
    //=========================================================================================================
 
    void setModelFiducial(FiducialId id, const QVector3D& posInModel);
    bool hasModelFiducial(FiducialId id) const;
    bool hasAllModelFiducials() const;
    QVector3D modelFiducial(FiducialId id) const;
 
    bool hasAllPenFiducials() const;
 
    //=========================================================================================================
    // Vertex (CZ / top of head) — 4th reference point for orientation validation
    //=========================================================================================================
 
    void setModelVertex(const QVector3D& pos) { m_modelVertex = pos; m_hasModelVertex = true; }
    bool hasModelVertex() const { return m_hasModelVertex; }
    QVector3D modelVertex() const { return m_modelVertex; }
 
    bool captureCurrentPenPositionAsVertex();
    bool hasPenVertex() const { return m_hasPenVertex; }
    QVector3D penVertex() const { return m_penVertex; }
 
    //=========================================================================================================
    // Registration
    //=========================================================================================================

    bool computeRegistration();

    void resetRegistration();
 
    //=========================================================================================================
    // Session persistence — save/restore registration state across app restarts
    //=========================================================================================================
 
    void saveSessionState(QSettings &settings, const QString &prefix = QStringLiteral("polhemus")) const;
    bool restoreSessionState(QSettings &settings, const QString &prefix = QStringLiteral("polhemus"));
 
    bool registrationValid() const { return m_registrationValid; }
 
    //=========================================================================================================
    // Current state accessors
    //=========================================================================================================
 
    QMatrix4x4  deviceToWorld()    const { return m_deviceToWorld; }
    QMatrix4x4  headToWorld()      const { return m_headToWorld; }
    QMatrix4x4  headToDevice()     const { return m_headToDevice; }
    QMatrix4x4  worldToModel()     const { return m_worldToModel; }
    QVector3D   penPosition()      const { return m_penPosition; }
    QQuaternion penOrientation()   const { return m_penOrientation; }
    bool        haveLivePenPosition() const { return m_havePenPos; }
 
    QVector3D   probePosition()    const { return m_probePosition; }
    QQuaternion probeOrientation() const { return m_probeOrientation; }
    bool        haveLiveProbePosition() const { return m_haveProbePos; }
 
signals:
    void devicePoseChanged(const QMatrix4x4& deviceToWorld);
    void penPoseChanged(const QVector3D& position, const QQuaternion& orientation);
    void probePoseChanged(const QVector3D& position, const QQuaternion& orientation);
    void penButtonPressed(const QVector3D& position, const QQuaternion& orientation);
    void registrationChanged();
    void pivotStateChanged(PolhemusCoregistration::PivotState state);
    void pivotCalibrationDone(const QVector3D& offset, float residualMm);
    void pivotSampleCollected(int sampleCount, float angularSpanDeg);
    void opticalCalibrationChanged();
 
private slots:
    void onPointReceived(int station, const QVector3D& position, const QQuaternion& orientation);
    void onPenButtonPressedFromConn(int station, const QVector3D& position, const QQuaternion& orientation);
 
private:
    QMatrix4x4 buildDevicePose(const QVector3D& trackerPos,
                               const QQuaternion& trackerOri) const;
    QMatrix4x4 buildHeadFrame() const;
 
    int m_trackerStation = 2;
    int m_penStation     = 1;
    int m_probeStation   = 3;
 
    QVector3D   m_offsetTranslation;
    QQuaternion m_offsetRotation;
    QVector3D   m_penTipOffset;        // tip offset in sensor body frame (metres)
    bool        m_tipOffsetEnabled = false;
    bool        m_mirrorX = false;     // negate X of incoming Polhemus positions
    bool        m_mirrorY = false;     // negate Y of incoming Polhemus positions
 
    PolhemusConnection* m_pConn   = nullptr;
    AcquiredPoints*     m_pPoints = nullptr;
 
    QVector3D   m_penPosition;
    QQuaternion m_penOrientation;
    bool        m_havePenPos = false;
 
    QVector3D   m_probePosition;
    QQuaternion m_probeOrientation;
    bool        m_haveProbePos = false;
 
    QMatrix4x4  m_deviceToWorld;
    QMatrix4x4  m_headToWorld;
    QMatrix4x4  m_headToDevice;
    QMatrix4x4  m_worldToModel;
    bool        m_registrationValid = false;
 
    // Pen fiducials (captured from stylus in Polhemus world frame)
    QVector3D   m_penFid[4];          // indexed by FiducialId (1..3)
    bool        m_hasPenFid[4] = {};
 
    // Model fiducials (from FIFF / BEM, in MRI/surface-RAS frame)
    QVector3D   m_modelFid[4];        // indexed by FiducialId (1..3)
    bool        m_hasModelFid[4] = {};
 
    // Vertex / CZ (top of head) for orientation validation
    QVector3D   m_modelVertex;
    bool        m_hasModelVertex = false;
    QVector3D   m_penVertex;
    bool        m_hasPenVertex = false;
 
    // Pivot calibration state
    PivotState  m_pivotState = PivotState::Idle;
    std::vector<QVector3D>   m_pivotPositions;
    std::vector<QQuaternion> m_pivotOrientations;
    float m_pivotResidualMm = 0.0f;
 
    // Optical path calibration state
    std::vector<OpticalCalibSample> m_opticalCalibSamples;
    QVector3D m_opticalAxisLocal;           // optical axis direction in tracker body frame
    QVector3D m_opticalCenterLocal;         // optical center position in tracker body frame
    float     m_opticalCalibResidualMm = 0.0f;
    float     m_opticalCalibDepthSpreadMm = 0.0f;
    bool      m_opticalCalibValid = false;
 
    // Directly captured objective center (in tracker-local frame, metres).
    // When available, the solver uses this instead of the distance constraint.
    QVector3D m_objectiveCenterLocal;       // objective center in tracker body frame
    bool      m_hasObjectiveCenter = false;
 
    // Fine adjustment — small correction rotation applied on top of the
    // solved optical axis to compensate for residual misalignment between
    // the tracked probe position and its appearance in the video.
    QVector3D m_opticalAxisPreFineAdjust;   // axis before fine-adjust (for undo)
    float     m_opticalFineAdjustDeg = 0.0f;
    bool      m_opticalFineAdjustApplied = false;
 
    // Known tracker-to-objective distance (metres) — fallback constraint when
    // no direct objective center capture is available.
    float     m_knownTrackerToObjectiveDist = 0.200f;  // metres
 
    bool solvePivotCalibration();
};
 
} // namespace UTILSLIB
 
#endif // UTILS_POLHEMUS_COREGISTRATION_H