TrackerThread.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_TrackerThread_h_GUID_6544B03C_4EB4_4B82_77F1_16EF83578C64
#define INCLUDED_TrackerThread_h_GUID_6544B03C_4EB4_4B82_77F1_16EF83578C64

// Internal Includes
#include "CameraParameters.h"
#include "IMUMessage.h"
#include "ThreadsafeBodyReporting.h"
#include "TrackingSystem.h"

#include "ImageSources/ImageSource.h"

// Library/third-party includes
#include <osvr/Util/ClientReportTypesC.h>

#include <opencv2/core/core.hpp> // for basic OpenCV types

#include <boost/noncopyable.hpp>
#include <folly/ProducerConsumerQueue.h>
#include <folly/sorted_vector_types.h>
#include <osvr/TypePack/List.h>

// Standard includes
#include <array>
#include <chrono>
#include <condition_variable>
#include <cstdint>
#include <future>
#include <iosfwd>
#include <mutex>
#include <queue>
#include <thread>

namespace osvr {
namespace vbtracker {
    static const std::chrono::milliseconds IMU_OVERRIDE_SPACING{2};

    static const auto MAX_DEBUG_BEACONS = 34;
    static const auto DEBUG_ANALOGS_PER_BEACON = 6;
    static const auto DEBUG_ANALOGS_REQUIRED =
        MAX_DEBUG_BEACONS * DEBUG_ANALOGS_PER_BEACON;

    using DebugArray = std::array<double, DEBUG_ANALOGS_REQUIRED>;
    struct BodyIdOrdering {
        bool operator()(BodyId const &lhs, BodyId const &rhs) const {
            return lhs.value() < rhs.value();
        }
    };

    using UpdatedBodyIndices = folly::sorted_vector_set<BodyId, BodyIdOrdering>;

    class ImageProcessingThread;

    class TrackerThread : boost::noncopyable {
      public:
        TrackerThread(TrackingSystem &trackingSystem, ImageSource &imageSource,
                      BodyReportingVector &reportingVec,
                      CameraParameters const &camParams,
                      std::int32_t cameraUsecOffset = 0, bool bufferImu = false,
                      bool debugData = false);
        ~TrackerThread();

        void threadAction();

        void permitStart();

        void triggerStop();

        bool submitIMUReport(TrackedBodyIMU &imu,
                             util::time::TimeValue const &tv,
                             OSVR_OrientationReport const &report);
        bool submitIMUReport(TrackedBodyIMU &imu,
                             util::time::TimeValue const &tv,
                             OSVR_AngularVelocityReport const &report);

        bool checkForDebugData(DebugArray &data);

        void signalImageProcessingComplete(ImageOutputDataPtr &&imageData,
                                           cv::Mat const &frame,
                                           cv::Mat const &frameGray);

      private:
        std::ostream &msg() const;
        std::ostream &warn() const;

        void doFrame();

        void setupReportingVectorProcessModels();

        bool setupReportingVectorRoomTransforms();

        void updateReportingVector(UpdatedBodyIndices const &bodyIds);

        void updateReportingVector(BodyId const bodyId);

        void launchTimeConsumingImageStep();

        std::pair<BodyId, ImuMessageCategory>
        processIMUMessage(IMUMessage const &m);

        BodyReporting *getCamPoseReporting() const;
        BodyReporting *getIMUReporting() const;
        BodyReporting *getIMUCamReporting() const;
        BodyReporting *getHMDCamReporting() const;

        void updateExtraCameraReport();
        void updateExtraIMUReports();

        TrackingSystem &m_trackingSystem;
        ImageSource &m_cam;
        BodyReportingVector &m_reportingVec;
        CameraParameters m_camParams;
        std::size_t m_numBodies = 0; //< initialized when loop started.
        const std::int32_t m_cameraUsecOffset = 0;

        const bool m_bufferImu = false;

        const bool m_debugData = false;

        using our_clock = std::chrono::steady_clock;

        bool shouldSendImuReport() {
            auto now = our_clock::now();
            if (now > m_nextImuOverrideReport) {
                m_nextImuOverrideReport = now + IMU_OVERRIDE_SPACING;
                return true;
            }
            return false;
        }

        void setImuOverrideClock() {
            m_nextImuOverrideReport = our_clock::now() + IMU_OVERRIDE_SPACING;
        }

        our_clock::time_point m_nextImuOverrideReport;
        boost::optional<our_clock::time_point> m_nextCameraPoseReport;

        std::promise<void> m_startupSignal;

        bool m_setCameraPose = false;

        cv::Mat m_frame;
        cv::Mat m_frameGray;
        ImageOutputDataPtr m_imageData;

        std::mutex m_runMutex;
        bool m_run = true;

        std::condition_variable m_messageCondVar;
        std::mutex m_messageMutex;
        bool m_timeConsumingImageStepComplete = false;
        folly::ProducerConsumerQueue<IMUMessage> m_imuMessages;

        folly::ProducerConsumerQueue<DebugArray> m_debugDataMessages;

        ImageProcessingThread *imageProcThreadObj_ = nullptr;

        std::thread m_imageThread;
    };
} // namespace vbtracker
} // namespace osvr
#endif // INCLUDED_TrackerThread_h_GUID_6544B03C_4EB4_4B82_77F1_16EF83578C64