TrackingSystem.h

Go to the documentation of this file.

// Copyright 2016 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_TrackingSystem_h_GUID_B94B2C23_321F_45B4_5167_CB32D2624B50
#define INCLUDED_TrackingSystem_h_GUID_B94B2C23_321F_45B4_5167_CB32D2624B50

// Internal Includes
#include "ConfigParams.h"
#include "ImageProcessing.h"
#include "BodyIdTypes.h"
#include "CameraParameters.h"

// Library/third-party includes
#include <osvr/Util/EigenCoreGeometry.h>
#include <osvr/Util/TimeValue.h>
#include <osvr/Util/TypeSafeIdHash.h>
#include <opencv2/core/core.hpp>

// Standard includes
#include <vector>
#include <memory>
#include <cstddef>
#include <unordered_map>

namespace osvr {
namespace vbtracker {
    class TrackedBody;
    class TrackedBodyTarget;
    using BodyIndices = std::vector<BodyId>;

    using LedUpdateCount = std::unordered_map<BodyTargetId, std::size_t>;

    class TrackingSystem {
      public:
        TrackingSystem(ConfigParams const &params);
        ~TrackingSystem();
        TrackedBody *createTrackedBody();

        ImageOutputDataPtr performInitialImageProcessing(
            util::time::TimeValue const &tv, cv::Mat const &frame,
            cv::Mat const &frameGray, CameraParameters const &camParams);
        LedUpdateCount const &
        updateLedsFromVideoData(ImageOutputDataPtr &&imageData);

        BodyIndices const &
        updateBodiesFromVideoData(ImageOutputDataPtr &&imageData);

        BodyIndices const &processFrame(util::time::TimeValue const &tv,
                                        cv::Mat const &frame,
                                        cv::Mat const &frameGray,
                                        CameraParameters const &camParams) {
            auto imageOutput =
                performInitialImageProcessing(tv, frame, frameGray, camParams);
            return updateBodiesFromVideoData(std::move(imageOutput));
        }

        std::size_t getNumBodies() const { return m_bodies.size(); }
        bool isValidBodyId(BodyId i) const {
            return (!i.empty()) && (i.value() < m_bodies.size());
        }
        TrackedBody &getBody(BodyId i) { return *m_bodies.at(i.value()); }
        TrackedBody const &getBody(BodyId i) const {
            return *m_bodies.at(i.value());
        }
        TrackedBodyTarget *getTarget(BodyTargetId target);
        TrackedBodyTarget const *getTarget(BodyTargetId target) const;

        ConfigParams const &getParams() const { return m_params; }

        void setUseIMU(bool useIMU) { m_params.imu.useOrientation = useIMU; }

        bool haveCameraPose() const;
        void setCameraPose(Eigen::Isometry3d const &camPose);

        Eigen::Isometry3d const &getCameraPose() const;
        Eigen::Isometry3d const &getRoomToCamera() const;

        bool isRoomCalibrationComplete();

        struct Impl;

        void calibrationHandleIMUData(BodyId id,
                                      util::time::TimeValue const &tv,
                                      Eigen::Quaterniond const &quat);

      private:
        void updatePoseEstimates();

        void calibrationVideoPhaseThree();

        using BodyPtr = std::unique_ptr<TrackedBody>;
        ConfigParams m_params;

        BodyIndices m_updated;
        std::vector<BodyPtr> m_bodies;

        std::unique_ptr<Impl> m_impl;

        friend class TrackingDebugDisplay;
    };

} // namespace vbtracker
} // namespace osvr

#endif // INCLUDED_TrackingSystem_h_GUID_B94B2C23_321F_45B4_5167_CB32D2624B50