TrackerThread.cpp

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.

// Internal Includes
#include "TrackerThread.h"
#include "AdditionalReports.h"
#include "ImageProcessingThread.h"
#include "ProcessIMUMessage.h"
#include "SpaceTransformations.h"
#include "TrackedBody.h"
#include "TrackedBodyIMU.h"
#include "TrackedBodyTarget.h"

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

// Standard includes
#include <future>
#include <iostream>
#include <type_traits>

#define OSVR_TRACKER_THREAD_WRAP_WITH_TRY

namespace osvr {
namespace vbtracker {
    // 16 and even 32 was too small - we were dropping messages.
    static const uint32_t IMU_MESSAGE_QUEUE_SIZE = 64 + 1;

    TrackerThread::TrackerThread(TrackingSystem &trackingSystem,
                                 ImageSource &imageSource,
                                 BodyReportingVector &reportingVec,
                                 CameraParameters const &camParams,
                                 std::int32_t cameraUsecOffset, bool bufferImu,
                                 bool debugData)
        : m_trackingSystem(trackingSystem), m_cam(imageSource),
          m_reportingVec(reportingVec), m_camParams(camParams),
          m_cameraUsecOffset(cameraUsecOffset), m_bufferImu(bufferImu),
          m_debugData(debugData), m_imuMessages(IMU_MESSAGE_QUEUE_SIZE),
          m_debugDataMessages(32) {
        msg() << "Tracker thread object created." << std::endl;
    }

    TrackerThread::~TrackerThread() {
        if (m_imageThread.joinable()) {
            m_imageThread.join();
        }
    }

    void TrackerThread::permitStart() { m_startupSignal.set_value(); }

    void TrackerThread::threadAction() {

        msg() << "Tracker thread object invoked, waiting for permitStart()."
              << std::endl;
        m_startupSignal.get_future().wait();
        std::this_thread::sleep_for(std::chrono::milliseconds(500));

        m_numBodies = m_trackingSystem.getNumBodies();
        setupReportingVectorProcessModels();


        ImageProcessingThread imageProcThreadObj{
            m_trackingSystem, m_cam, *this, m_camParams, m_cameraUsecOffset};
        imageProcThreadObj_ = &imageProcThreadObj;
        m_imageThread = std::thread{[&] { imageProcThreadObj.threadAction(); }};

        msg() << "Tracker thread object entering its main execution loop."
              << std::endl;

#ifdef OSVR_TRACKER_THREAD_WRAP_WITH_TRY
        try {
#endif
            bool keepGoing = true;
            while (keepGoing) {
                doFrame();

                {
                    std::lock_guard<std::mutex> lock(m_runMutex);
                    keepGoing = m_run;
                }
                if (!keepGoing) {
                    msg() << "Tracker thread object: Just checked our run flag "
                             "and noticed it turned false..."
                          << std::endl;
                }
            }
#ifdef OSVR_TRACKER_THREAD_WRAP_WITH_TRY
        } catch (std::exception const &e) {
            warn() << "Tracker thread object: exiting because of caught "
                      "exception: "
                   << e.what() << std::endl;
            m_run = false;
        }
#endif
        msg() << "Tracker thread object: functor exiting." << std::endl;

        if (!imageProcThreadObj.exiting()) {
            msg() << "Telling image processing thread to exit." << std::endl;
            imageProcThreadObj.signalExit();
        }
        imageProcThreadObj_ = nullptr;
        if (m_imageThread.joinable()) {
            m_imageThread.join();
        }
    }

    void TrackerThread::triggerStop() {
        msg() << "Tracker thread object: triggerStop() called" << std::endl;
        std::lock_guard<std::mutex> lock(m_runMutex);
        m_run = false;
    }

    bool TrackerThread::submitIMUReport(TrackedBodyIMU &imu,
                                        util::time::TimeValue const &tv,
                                        OSVR_OrientationReport const &report) {
        if (!m_imuMessages.write(makeImuReport(imu, tv, report))) {
            // no room for IMU message!
            // msg() << "Dropped IMU orientation message!\n";
            return false;
        }
        m_messageCondVar.notify_one();
        return true;
    }

    bool
    TrackerThread::submitIMUReport(TrackedBodyIMU &imu,
                                   util::time::TimeValue const &tv,
                                   OSVR_AngularVelocityReport const &report) {
        if (!m_imuMessages.write(makeImuReport(imu, tv, report))) {
            // no room for IMU message!
            return false;
        }
        m_messageCondVar.notify_one();
        return true;
    }

    bool TrackerThread::checkForDebugData(DebugArray &data) {
        return m_debugDataMessages.read(data);
    }

    void
    TrackerThread::signalImageProcessingComplete(ImageOutputDataPtr &&imageData,
                                                 cv::Mat const &frame,
                                                 cv::Mat const &frameGray) {
        m_imageData = std::move(imageData);
        m_frame = frame;
        m_frameGray = frameGray;
        {
            std::lock_guard<std::mutex> lock{m_messageMutex};
            m_timeConsumingImageStepComplete = true;
        }
        m_messageCondVar.notify_one();
    }

    std::ostream &TrackerThread::msg() const {
        return std::cout << "[UnifiedTracker] ";
    }

    std::ostream &TrackerThread::warn() const { return msg() << "Warning: "; }

    void TrackerThread::doFrame() {
        // Check camera status.
        if (!m_cam.ok()) {
            // Hmm, camera seems bad. Might regain it? Skip for now...
            warn() << "Camera is reporting it is not OK." << std::endl;
            return;
        }
        // Trigger a grab.
        if (!m_cam.grab()) {
            // Again failing without quitting, in hopes we get better luck
            // next time...
            warn() << "Camera grab failed." << std::endl;
            return;
        }
        // When we triggered the grab was a good guess of the time
        // for the image before that got moved upstream into the ImageSource
        // library.

        launchTimeConsumingImageStep();
        if (m_bufferImu) {
            setImuOverrideClock();
        }
        UpdatedBodyIndices imuIndices;

        bool finishedImage = false;
        do {

            {
                std::unique_lock<std::mutex> lock(m_messageMutex);
                m_messageCondVar.wait(lock, [&] {
                    return m_timeConsumingImageStepComplete ||
                           !m_imuMessages.isEmpty();
                });
                if (m_timeConsumingImageStepComplete) {
                    finishedImage = true;
                }
                // Otherwise we have some IMU reports to keep us busy in the
                // meantime.
            } // unlock

            if (!finishedImage) {

                IMUMessage message = boost::none;
                if (!m_imuMessages.read(message) || message.empty()) {
                    // couldn't read a message, or read an empty message
                    continue;
                }

                // process it.
                BodyId id;
                ImuMessageCategory cat;
                std::tie(id, cat) = processIMUMessage(message);
                if (id.empty()) {
                    // processed but got an empty body ID
                    continue;
                }

                if (m_bufferImu) {
                    // insert index into the list
                    imuIndices.insert(id);

                    // if it's time, send a report even if we haven't gotten a
                    // video frame with useful things in it yet.
                    if (shouldSendImuReport()) {
                        updateReportingVector(imuIndices);
                        imuIndices.clear();
                    }
                } else {

                    updateReportingVector(id);
                }
            }
        } while (!finishedImage);

        // OK, once we get here, we know the timeConsumingImageStep is complete.
        if (!m_frame.data || !m_frameGray.data) {
            // but it ended early due to error.
            warn() << "Camera retrieve appeared to fail: frames had null "
                      "pointers!"
                   << std::endl;
            return;
        }

        if (!m_imageData) {
            // but it failed to set the pointer? this is very strange...
            warn() << "Initial image processing failed somehow!" << std::endl;
            return;
        }

        // Submit initial image data to the tracking system.
        auto bodyIds =
            m_trackingSystem.updateBodiesFromVideoData(std::move(m_imageData));
        m_imageData.reset();

        // Sort those body IDs so we can merge them with the body IDs from any
        // IMU messages we're about to process.
        UpdatedBodyIndices sortedBodyIds{begin(bodyIds), end(bodyIds)};
        if (m_bufferImu) {
            for (auto &id : imuIndices) {
                sortedBodyIds.insert(id);
            }
            imuIndices.clear();
        }

        // process those IMU messages and add any unique IDs to the vector
        // returned by the image tracker.
        // We only want to process a fixed number of messages so we don't get
        // stuck here in a loop without servicing the camera.
        std::size_t numMessages = m_imuMessages.sizeGuess();
        {
            IMUMessage message = boost::none;
            for (std::size_t i = 0; i < numMessages; ++i) {
                if (!m_imuMessages.read(message)) {
                    // ran out of messages earlier than expected.
                    break;
                }

                auto id = processIMUMessage(message).first;
                if (!id.empty()) {
                    sortedBodyIds.insert(id);
                }
            }
        }

        updateReportingVector(sortedBodyIds);
    }

    std::pair<BodyId, ImuMessageCategory>
    TrackerThread::processIMUMessage(IMUMessage const &m) {
        return osvr::vbtracker::processImuMessage(m);
    }

    BodyReporting *TrackerThread::getCamPoseReporting() const {
#ifdef OSVR_OUTPUT_CAMERA_POSE
        return m_reportingVec[m_numBodies + extra_outputs::outputCamIndex]
            .get();
#else
        return nullptr;
#endif
    }

    BodyReporting *TrackerThread::getIMUReporting() const {
#ifdef OSVR_OUTPUT_IMU
        return m_reportingVec[m_numBodies + extra_outputs::outputImuIndex]
            .get();
#else
        return nullptr;
#endif
    }

    BodyReporting *TrackerThread::getIMUCamReporting() const {
#ifdef OSVR_OUTPUT_IMU_CAM
        return m_reportingVec[m_numBodies + extra_outputs::outputImuCamIndex]
            .get();
#else
        return nullptr;
#endif
    }

    BodyReporting *TrackerThread::getHMDCamReporting() const {

#ifdef OSVR_OUTPUT_HMD_CAM
        return m_reportingVec[m_numBodies + extra_outputs::outputHMDCamIndex]
            .get();
#else
        return nullptr;
#endif
    }

    void TrackerThread::updateExtraCameraReport() {
#ifndef OSVR_OUTPUT_CAMERA_POSE
        return;
#endif

        using namespace Eigen;
        using namespace std::chrono;
        auto rightNow = our_clock::now();
        if (!m_nextCameraPoseReport || rightNow > *m_nextCameraPoseReport) {
            m_nextCameraPoseReport = rightNow + seconds(1);
            BodyState state;
            state.position() = m_trackingSystem.getCameraPose().translation();
            state.setQuaternion(
                Quaterniond(m_trackingSystem.getCameraPose().rotation()));
            getCamPoseReporting()->updateState(util::time::getNow(), state);
        }
    }

    void TrackerThread::updateExtraIMUReports() {

#if !defined(OSVR_OUTPUT_IMU) && !defined(OSVR_OUTPUT_IMU_CAM)
        // Not reporting these special reports, do nothing.
        return;
#endif

        if (!m_trackingSystem.getBody(BodyId(0)).hasIMU()) {
            // no IMU, nothing to report.
            return;
        }
        auto &imu = m_trackingSystem.getBody(BodyId(0)).getIMU();
        if (!imu.hasPoseEstimate()) {
            return;
        }
        Eigen::Quaterniond imuQuat = imu.getPoseEstimate();

#ifdef OSVR_OUTPUT_IMU
        {
            BodyState state;
            state.setQuaternion(imuQuat);
            getIMUReporting()->updateState(imu.getLastUpdate(), state);
        }
#endif

#ifdef OSVR_OUTPUT_IMU_CAM

        {
            BodyState state;
            state.setQuaternion(getCameraRotation(m_trackingSystem) * imuQuat);
            // Put this one up in the air a little so we can tell the
            // difference.
            state.position() = Eigen::Vector3d(0, 0.5, 0);
            getIMUCamReporting()->updateState(imu.getLastUpdate(), state);
        }
#endif
    }

    void TrackerThread::setupReportingVectorProcessModels() {
        for (BodyId::wrapped_type i = 0; i < m_numBodies; ++i) {
            auto bodyId = BodyId{i};
            auto &body = m_trackingSystem.getBody(bodyId);
            m_reportingVec[i]->initProcessModel(body.getProcessModel());
#ifdef OSVR_OUTPUT_HMD_CAM
            if (bodyId == BodyId(0)) {
                getHMDCamReporting()->initProcessModel(body.getProcessModel());
            }
#endif
        }
    }

    bool TrackerThread::setupReportingVectorRoomTransforms() {
        if (!m_trackingSystem.haveCameraPose()) {
            return false;
        }
        if (m_setCameraPose) {
            return true;
        }

        m_setCameraPose = true;

        auto cameraPoseReporting = getCamPoseReporting();
        auto imuAlignedReporting = getIMUReporting();
        auto imuCameraSpaceReporting = getIMUCamReporting();
        auto hmdCameraSpaceReporting = getHMDCamReporting();
        Eigen::Isometry3d trackerToRoomXform = m_trackingSystem.getCameraPose();
        for (auto &reporting : m_reportingVec) {

            auto reportingRaw = reporting.get();
            if (reportingRaw == cameraPoseReporting ||
                reportingRaw == imuAlignedReporting ||
                reportingRaw == imuCameraSpaceReporting ||
                reportingRaw == hmdCameraSpaceReporting) {
                continue;
            }

            reporting->setTrackerToRoomTransform(trackerToRoomXform);
        }
        return true;
    }

    void
    TrackerThread::updateReportingVector(UpdatedBodyIndices const &bodyIds) {
        if (!setupReportingVectorRoomTransforms()) {
            // false return means that we don't have calibration data yet, so no
            // sense in reporting the other things.
            return;
        }
        for (auto const &bodyId : bodyIds) {
            updateReportingVector(bodyId);
#ifdef OSVR_OUTPUT_HMD_CAM
            if (bodyId == BodyId(0)) {
                auto &body = m_trackingSystem.getBody(bodyId);
                getHMDCamReporting()->updateState(body.getStateTime(),
                                                  body.getState());
            }
#endif
        }

        updateExtraCameraReport();
        updateExtraIMUReports();
    }

    void TrackerThread::updateReportingVector(BodyId const bodyId) {
        auto &body = m_trackingSystem.getBody(bodyId);
        m_reportingVec[bodyId.value()]->updateState(body.getStateTime(),
                                                    body.getState());
        if (m_debugData && bodyId == BodyId(0)) {
            DebugArray newDebugArray = {};
            auto &target = *body.getTarget(TargetId(0));
            auto &debugData = target.getBeaconDebugData();
            for (int i = 0; i < MAX_DEBUG_BEACONS; ++i) {
                auto baseIdx = i * DEBUG_ANALOGS_PER_BEACON;
                auto &thisBeacon = debugData[i];
                newDebugArray[baseIdx] = thisBeacon.variance;
                newDebugArray[baseIdx + 1] = thisBeacon.measurement.x;
                newDebugArray[baseIdx + 2] = thisBeacon.measurement.y;
                newDebugArray[baseIdx + 3] = thisBeacon.residual.x;
                newDebugArray[baseIdx + 4] = thisBeacon.residual.y;
            }
            m_debugDataMessages.write(newDebugArray);
        }
    }

    void TrackerThread::launchTimeConsumingImageStep() {
        m_timeConsumingImageStepComplete = false;

        imageProcThreadObj_->signalDoFrame();
    }
} // namespace vbtracker
} // namespace osvr