VideoIMUFusion.h

Go to the documentation of this file.

// Copyright 2015 Sensics, Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//        http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

#ifndef INCLUDED_VideoIMUFusion_h_GUID_85338EA5_58E6_4787_16D2_EC53201EFE9F
#define INCLUDED_VideoIMUFusion_h_GUID_85338EA5_58E6_4787_16D2_EC53201EFE9F

// Internal Includes
#include "FusionParams.h"
#include <osvr/Util/ClientReportTypesC.h>
#include <osvr/Util/EigenCoreGeometry.h>
#include <osvr/Util/TimeValue.h>

// Library/third-party includes
// - none

// Standard includes
#include <memory>
#include <cassert>

class VideoIMUFusion {
  public:
    VideoIMUFusion(VideoIMUFusionParams const &params = VideoIMUFusionParams());
    ~VideoIMUFusion();

    EIGEN_MAKE_ALIGNED_OPERATOR_NEW

    void handleIMUData(const OSVR_TimeValue &timestamp,
                       const OSVR_OrientationReport &report);

    void handleIMUVelocity(const OSVR_TimeValue &timestamp,
                           const Eigen::Vector3d &angVel);
    void handleVideoTrackerDataWhileRunning(const OSVR_TimeValue &timestamp,
                                            const OSVR_PoseReport &report);
    void handleVideoTrackerDataDuringStartup(
        const OSVR_TimeValue &timestamp, const OSVR_PoseReport &report,
        const OSVR_OrientationState &orientation);

    bool running() const { return m_state == State::Running; }

    OSVR_PoseState const &getLatestPose() const { return m_lastPose; }

    osvr::util::time::TimeValue const &getLatestTime() const {
        return m_lastTime;
    }

    OSVR_VelocityState const &getLatestVelocity() const {
        return m_lastVelocity;
    }
    osvr::util::time::TimeValue const &getLatestVelocityTime() const {
        return m_lastVelTime;
    }

    OSVR_PoseState const &getLatestReorientedVideoPose() const {
        assert(running());
        return m_reorientedVideo;
    }

    OSVR_PoseState const &getLatestCameraPose() const {
        assert(running());
        return m_camera;
    }

    Eigen::Matrix<double, 12, 12> const &getErrorCovariance() const;

  private:
    void enterCameraPoseAcquisitionState();
    void enterRunningState(Eigen::Isometry3d const &rTc,
                           const OSVR_TimeValue &timestamp,
                           const OSVR_PoseReport &report,
                           const OSVR_OrientationState &orientation);
    void updateFusedOutput(const OSVR_TimeValue &timestamp);
    enum class State {
        AcquiringCameraPose,
        Running
    };

    State m_state = State::AcquiringCameraPose;

    class StartupData;
    std::unique_ptr<StartupData> m_startupData;
    class RunningData;
    std::unique_ptr<RunningData> m_runningData;
    Eigen::Isometry3d m_rTc;

    OSVR_PoseState m_camera;
    OSVR_PoseState m_reorientedVideo;
    OSVR_PoseState m_lastPose;
    osvr::util::time::TimeValue m_lastTime;
    OSVR_VelocityState m_lastVelocity;
    osvr::util::time::TimeValue m_lastVelTime;
    VideoIMUFusionParams m_params;
    Eigen::Isometry3d m_roomCalib;
};

#endif // INCLUDED_VideoIMUFusion_h_GUID_85338EA5_58E6_4787_16D2_EC53201EFE9F