TrackedBodyTarget.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_TrackedBodyTarget_h_GUID_E4315530_5C4F_4DB4_2497_11686F0F6E0E
#define INCLUDED_TrackedBodyTarget_h_GUID_E4315530_5C4F_4DB4_2497_11686F0F6E0E

// Internal Includes
#include "BeaconIdTypes.h"
#include "BeaconSetupData.h"
#include "BodyIdTypes.h"
#include "LedMeasurement.h"
#include "ModelTypes.h"
#include "Types.h"

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

// Standard includes
#include <iosfwd>
#include <vector>

namespace osvr {
namespace vbtracker {
    struct CameraParameters;

    struct BeaconData {
        bool seen = false;
        double size = 0;
        cv::Point2d measurement = {0, 0};
        cv::Point2d residual = {0, 0};
        double variance = 0;
        void reset() { *this = BeaconData{}; }
    };

    class TrackedBody;
    struct BodyTargetInterface;

    enum class TargetStatusMeasurement {
        MaxPosErrorVariance,
        PosErrorVarianceLimit,
        NumUsableLeds,
        NumUsedLeds
    };

    class TrackedBodyTarget {
      public:
        TrackedBodyTarget(TrackedBody &body,
                          BodyTargetInterface const &bodyIface,
                          Eigen::Vector3d const &targetToBody,
                          TargetSetupData const &setupData, TargetId id);
        ~TrackedBodyTarget();
        EIGEN_MAKE_ALIGNED_OPERATOR_NEW

        std::vector<BeaconData> const &getBeaconDebugData() const {
            return m_beaconDebugData;
        }

        UnderlyingBeaconIdType getNumBeacons() const {
            return static_cast<UnderlyingBeaconIdType>(m_numBeacons);
        }

        TrackedBody &getBody() { return m_body; }
        TrackedBody const &getBody() const { return m_body; }

        TargetId getId() const { return m_id; }

        BodyTargetId getQualifiedId() const;

        Eigen::Vector3d getBeaconAutocalibPosition(ZeroBasedBeaconId i) const;
        Eigen::Vector3d getBeaconAutocalibVariance(ZeroBasedBeaconId i) const;

        void resetBeaconAutocalib();

        std::size_t
        processLedMeasurements(LedMeasurementVec const &undistortedLeds);

        void disableKalman();

        void permitKalman();

        bool updatePoseEstimateFromLeds(
            CameraParameters const &camParams,
            osvr::util::time::TimeValue const &tv, BodyState &bodyState,
            osvr::util::time::TimeValue const &startingTime,
            bool validStateAndTime);

        bool uncalibratedRANSACPoseEstimateFromLeds(
            CameraParameters const &camParams, Eigen::Vector3d &xlate,
            Eigen::Quaterniond &quat, int skipBrightsCutoff = -1,
            std::size_t iterations = 5);

        bool hasPoseEstimate() const { return m_hasPoseEstimate; }

        osvr::util::time::TimeValue const &getLastUpdate() const;

        Eigen::Vector3d const &getBeaconOffset() const {
            return m_beaconOffset;
        }

        LedGroup const &leds() const;

        LedPtrList const &usableLeds() const;

        std::size_t numTrackingResets() const;

        double
        getInternalStatusMeasurement(TargetStatusMeasurement measurement) const;

        Eigen::Vector3d const &getTargetToBody() const {
            return m_targetToBody;
        }

        Eigen::Vector3d getStateCorrection() const;

        static Eigen::Vector3d
        computeTranslationCorrection(Eigen::Vector3d const &bodyFrameOffset,
                                     Eigen::Quaterniond const &orientation);

        Eigen::Vector3d computeTranslationCorrectionToBody(
            Eigen::Quaterniond const &orientation) const;

      private:
        std::ostream &msg() const;
        void enterKalmanMode();
        void enterRANSACMode();
        void enterRANSACKalmanMode();

        void dumpBeaconsToConsole() const;

        LedGroup &leds();

        void updateUsableLeds();

        LedPtrList &usableLeds();

        ConfigParams const &getParams() const;
        void m_verifyInvariants() const {
            BOOST_ASSERT_MSG(m_beacons.size() ==
                                 m_beaconMeasurementVariance.size(),
                             "Must have the same number of beacons as default "
                             "measurement variances.");
            BOOST_ASSERT_MSG(
                m_beacons.size() == m_beaconFixed.size(),
                "Must have the same number of beacons as beacon fixed flags.");
            BOOST_ASSERT_MSG(m_beacons.size() ==
                                 m_beaconEmissionDirection.size(),
                             "Must have the same number of beacons as beacon "
                             "emission directions.");
        }
        using BeaconState = kalman::PureVectorState<3>;
        using BeaconStateVec = std::vector<std::unique_ptr<BeaconState>>;

        TrackedBody &m_body;

        const TargetId m_id;

        Eigen::Vector3d m_targetToBody;

        const std::size_t m_numBeacons;
        BeaconStateVec m_beacons;
        std::vector<double> m_beaconMeasurementVariance;
        std::vector<bool> m_beaconFixed;
        Vec3Vector m_beaconEmissionDirection;
        std::vector<BeaconData> m_beaconDebugData;

        BeaconStateVec m_origBeacons;

        Eigen::Vector3d m_beaconOffset;

        bool m_hasPoseEstimate = false;

        struct Impl;
        std::unique_ptr<Impl> m_impl;
    };
} // namespace vbtracker
} // namespace osvr

#endif // INCLUDED_TrackedBodyTarget_h_GUID_E4315530_5C4F_4DB4_2497_11686F0F6E0E