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