TrackedBody.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_TrackedBody_h_GUID_1FC60169_196A_4F89_551C_E1B41531BBC8
#define INCLUDED_TrackedBody_h_GUID_1FC60169_196A_4F89_551C_E1B41531BBC8

// Internal Includes
#include "ConfigParams.h"
#include "BodyIdTypes.h"
#include "ModelTypes.h"
#include "CannedIMUMeasurement.h"

// Library/third-party includes
#include <osvr/Util/EigenCoreGeometry.h>
#include <osvr/Util/TimeValue.h>
#include <boost/assert.hpp>

// Standard includes
#include <memory>

namespace osvr {
namespace vbtracker {
    class TrackingSystem;
    class TrackedBodyIMU;
    class TrackedBodyTarget;
    struct TargetSetupData;

    class TrackedBody {
      public:
        TrackedBody(TrackingSystem &system, BodyId id);
        ~TrackedBody();
        TrackedBody(TrackedBody const &) = delete;
        TrackedBody &operator=(TrackedBody const &) = delete;
        EIGEN_MAKE_ALIGNED_OPERATOR_NEW

        TrackedBodyIMU *
        createIntegratedIMU(double orientationVariance,
                            double angularVelocityVariance = 1.0);

        TrackedBodyTarget *createTarget(Eigen::Vector3d const &targetToBody,
                                        TargetSetupData const &setupData);

        BodyId getId() const;

        ConfigParams const &getParams() const;

        BodyState const &getState() const { return m_state; }

        bool hasIMU() const { return static_cast<bool>(m_imu); }

        TrackedBodyIMU &getIMU() {
            BOOST_ASSERT_MSG(m_imu, "getIMU() called when hasIMU() is false!");
            return *m_imu;
        }

        TrackedBodyIMU const &getIMU() const {
            BOOST_ASSERT_MSG(m_imu, "getIMU() called when hasIMU() is false!");
            return *m_imu;
        }

        std::size_t getNumTargets() const { return m_target ? 1 : 0; }

        TrackedBodyTarget *getTarget(TargetId id) {
            if (TargetId(0) == id) {
                return m_target.get();
            }
            return nullptr;
        }

        TrackedBodyTarget const *getTarget(TargetId id) const {
            if (TargetId(0) == id) {
                return m_target.get();
            }
            return nullptr;
        }

        template <typename F> void forEachTarget(F &&f) {
            if (m_target) {
                std::forward<F>(f)(*m_target);
            }
        }

        template <typename F> void forEachTarget(F &&f) const {
            if (m_target) {
                std::forward<F>(f)(*m_target);
            }
        }

        bool hasPoseEstimate() const;

        bool hasEverHadPoseEstimate() const;

        bool getStateAtOrBefore(osvr::util::time::TimeValue const &desiredTime,
                                osvr::util::time::TimeValue &outTime,
                                BodyState &outState);

        void replaceStateSnapshot(osvr::util::time::TimeValue const &origTime,
                                  osvr::util::time::TimeValue const &newTime,
                                  BodyState const &newState);

        void pruneHistory(OSVR_TimeValue const &videoTime);

        osvr::util::time::TimeValue getStateTime() const;

        BodyProcessModel &getProcessModel() { return m_processModel; }

        BodyState &getState() { return m_state; }

        void incorporateNewMeasurementFromIMU(util::time::TimeValue const &tv,
                                              CannedIMUMeasurement const &meas);

        TrackingSystem &getSystem() { return m_system; }
        TrackingSystem const &getSystem() const { return m_system; }

      private:
        void applyIMUMeasurement(util::time::TimeValue const &tv,
                                 CannedIMUMeasurement const &meas);
        void pushState();
        TrackingSystem &m_system;
        const BodyId m_id;

        util::time::TimeValue m_stateTime;
        BodyState m_state;
        BodyProcessModel m_processModel;
        struct Impl;
        std::unique_ptr<Impl> m_impl;
        std::unique_ptr<TrackedBodyIMU> m_imu;
        std::unique_ptr<TrackedBodyTarget> m_target;
    };
} // namespace vbtracker
} // namespace osvr

#endif // INCLUDED_TrackedBody_h_GUID_1FC60169_196A_4F89_551C_E1B41531BBC8