RoomCalibration.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_RoomCalibration_h_GUID_D53BD552_47D4_4390_C6AC_C6819180D4AF
#define INCLUDED_RoomCalibration_h_GUID_D53BD552_47D4_4390_C6AC_C6819180D4AF

// Internal Includes
#include "BodyIdTypes.h"

// Library/third-party includes
#include <osvr/Util/Angles.h>
#include <osvr/Util/EigenCoreGeometry.h>
#include <osvr/Util/EigenFilters.h>
#include <osvr/Util/TimeValue.h>

#include <boost/optional.hpp>

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

namespace osvr {
namespace vbtracker {
    class TrackingSystem;
    class RoomCalibration {
      public:
        RoomCalibration(Eigen::Vector3d const &camPosition,
                        bool cameraIsForward = true);

        bool wantVideoData(TrackingSystem const &sys,
                           BodyTargetId const &target) const;

        void processVideoData(TrackingSystem const &sys,
                              BodyTargetId const &target,
                              util::time::TimeValue const &timestamp,
                              Eigen::Vector3d const &xlate,
                              Eigen::Quaterniond const &quat);

        void processIMUData(TrackingSystem const &sys, BodyId const &body,
                            util::time::TimeValue const &timestamp,
                            Eigen::Quaterniond const &quat);

        bool postCalibrationUpdate(TrackingSystem &sys);

        bool calibrationComplete() const { return m_calibComplete; }

        boost::optional<util::Angle>
        getCalibrationYaw(BodyId const &body) const;
        Eigen::Isometry3d getCameraPose() const;

      private:
        bool finished() const;

        std::ostream &msgStream() const;
        std::ostream &msg() const;
        std::ostream &instructions() const;
        void endInstructions() const;

        bool haveVideoData() const { return !m_videoTarget.first.empty(); }
        bool haveIMUData() const { return !m_imuBody.empty(); }
        std::size_t m_steadyVideoReports = 0;

        double m_linVel = 0;
        double m_angVel = 0;
        void handleExcessVelocity(double zTranslation);
        enum class InstructionState {
            Uninstructed,
            ToldToMoveCloser,
            ToldDistanceIsGood
        };
        InstructionState m_instructionState = InstructionState::Uninstructed;

        BodyTargetId m_videoTarget;
        util::time::TimeValue m_lastVideoData;

        util::filters::PoseOneEuroFilterd m_poseFilter =
            util::filters::PoseOneEuroFilterd{
                util::filters::one_euro::Params{3, 0.03},
                util::filters::one_euro::Params{1, 0.01}};
        Eigen::Vector3d m_rTc_ln_accum = Eigen::Vector3d::Zero();

        BodyId m_imuBody;
        Eigen::Quaterniond m_imuOrientation = Eigen::Quaterniond::Identity();

        Eigen::Vector3d m_suppliedCamPosition;
        bool m_cameraIsForward;

        bool m_calibComplete = false;
        util::Angle m_imuYaw = 0 * util::radians;
        Eigen::Isometry3d m_cameraPose = Eigen::Isometry3d::Identity();
        Eigen::Isometry3d m_rTi = Eigen::Isometry3d::Identity();
    };

    bool isRoomCalibrationComplete(TrackingSystem const &sys);
} // namespace vbtracker
} // namespace osvr
#endif // INCLUDED_RoomCalibration_h_GUID_D53BD552_47D4_4390_C6AC_C6819180D4AF