BeaconBasedPoseEstimator.h

Go to the documentation of this file.

// Copyright 2015 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_BeaconBasedPoseEstimator_h_GUID_7B983CED_F2C5_4B86_109A_948863B665B1
#define INCLUDED_BeaconBasedPoseEstimator_h_GUID_7B983CED_F2C5_4B86_109A_948863B665B1

// Internal Includes
#include "Types.h"
#include "LED.h"
#include "CameraParameters.h"

// Library/third-party includes
#include <osvr/Util/TimeValue.h>
#include <osvr/Util/ClientReportTypesC.h>
#include <osvr/Kalman/PureVectorState.h>
#include <osvr/Kalman/PoseState.h>
#include <osvr/Kalman/PoseSeparatelyDampedConstantVelocity.h>

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

namespace osvr {
namespace vbtracker {

    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 BeaconBasedPoseEstimator {
      public:
        static BeaconIDPredicate getDefaultBeaconFixedPredicate() {
            return [](int id) { return id <= 4; };
        }

        BeaconBasedPoseEstimator(CameraParameters const &camParams,
                                 size_t requiredInliers = 4,
                                 size_t permittedOutliers = 2,
                                 ConfigParams const &params = ConfigParams{});
        EIGEN_MAKE_ALIGNED_OPERATOR_NEW
        bool EstimatePoseFromLeds(LedGroup &leds, OSVR_TimeValue const &tv,
                                  OSVR_PoseState &out);

        std::size_t getNumBeacons() const { return m_beacons.size(); }

        bool ProjectBeaconsToImage(std::vector<cv::Point2f> &outPose);

        void permitKalmanMode(bool permitKalman);

        OSVR_PoseState GetState() const;
        OSVR_PoseState GetPredictedState(double dt) const;
        Eigen::Vector3d GetLinearVelocity() const;
        Eigen::Vector3d GetAngularVelocity() const;

        bool SetBeacons(const Point3Vector &beacons,
                        Vec3Vector const &emissionDirection,
                        std::vector<double> const &variance,
                        BeaconIDPredicate const &autocalibrationFixedPredicate,
                        double beaconAutocalibErrorScale = 1);
        bool SetCameraParameters(CameraParameters const &camParams);

        void dumpBeaconLocationsToStream(std::ostream &os) const;

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

        Eigen::Vector3d getBeaconAutocalibPosition(std::size_t i) const;

        Eigen::Vector3d getBeaconAutocalibVariance(std::size_t i) const;

      private:
        void m_updateBeaconCentroid(const Point3Vector &beacons);
        void m_updateBeaconDebugInfoArray();
        Eigen::Vector3d m_convertInternalPositionRepToExternal(
            Eigen::Vector3d const &pos) const;

        bool m_estimatePoseFromLeds(LedGroup &leds, OSVR_TimeValue const &tv,
                                    OSVR_PoseState &out);

        bool m_pnpransacEstimator(LedGroup &leds);

        bool m_kalmanAutocalibEstimator(LedGroup &leds, double dt);

        bool m_forceRansacIfKalmanNeedsReset(LedGroup const &leds);

        void m_resetState(Eigen::Vector3d const &xlate,
                          Eigen::Quaterniond const &quat);
        using BeaconState = kalman::PureVectorState<3>;
        using BeaconStateVec = std::vector<std::unique_ptr<BeaconState>>;
        BeaconStateVec m_beacons;
        std::vector<double> m_beaconMeasurementVariance;
        std::vector<bool> m_beaconFixed;
        Vec3Vector m_beaconEmissionDirection;

        std::vector<BeaconData> m_beaconDebugData;

        CameraParameters m_camParams;
        size_t m_requiredInliers;   //< How many inliers do we require?
        size_t m_permittedOutliers; //< How many outliers do we allow?

        ConfigParams const m_params;

        Eigen::Vector3d m_centroid;

        OSVR_TimeValue m_prev;
        bool m_gotPrev = false;

        bool m_permitKalman = true;

        bool m_gotPose = false;
        bool m_gotMeasurement = false;
        cv::Mat m_rvec;
        cv::Mat m_tvec;
        using State = kalman::pose_externalized_rotation::State;
        State m_state;
        using ProcessModel =
            osvr::kalman::PoseSeparatelyDampedConstantVelocityProcessModel;
        ProcessModel m_model;

        std::size_t m_framesInProbation = 0;
        std::size_t m_framesWithoutUtilizedMeasurements = 0;

        std::size_t m_framesWithoutIdentifiedBlobs = 0;
    };

} // namespace vbtracker
} // namespace osvr

#endif // INCLUDED_BeaconBasedPoseEstimator_h_GUID_7B983CED_F2C5_4B86_109A_948863B665B1