OptimizationBase.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_OptimizationBase_h_GUID_C3489E14_63D4_4290_45C0_225AC84A8BF5
#define INCLUDED_OptimizationBase_h_GUID_C3489E14_63D4_4290_45C0_225AC84A8BF5

// Internal Includes
#include "UtilityFunctions.h"

#include <ConfigParams.h>
#include <MakeHDKTrackingSystem.h>
#include <TrackedBodyTarget.h>

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

// Standard includes
#include <memory>
#include <utility>

namespace osvr {
namespace vbtracker {

    struct OptimCommonData {
        CameraParameters const &camParams;
        ConfigParams const &initialParams;
    };
    class OptimData {
      public:
        OptimData(OptimData const &) = delete;
        OptimData(OptimData &&) = default;
        OptimData &operator=(OptimData const &) = delete;

        static OptimData make(ConfigParams const &params,
                              OptimCommonData const &commonData) {

            auto system = makeHDKTrackingSystem(params);
            auto &body = system->getBody(BodyId(0));
            auto &target = *(body.getTarget(TargetId(0)));
            return OptimData(std::move(system), body, target,
                             commonData.camParams);
        }
        static OptimData make(OptimCommonData const &commonData) {
            return OptimData::make(commonData.initialParams, commonData);
        }

        TrackingSystem &getSystem() { return *system_; }
        TrackedBody &getBody() { return *body_; }
        TrackedBodyTarget &getTarget() { return *target_; }
        TrackedBodyTarget const &getTarget() const { return *target_; }
        CameraParameters const &getCamParams() { return camParams_; }

      private:
        OptimData(std::unique_ptr<TrackingSystem> &&system, TrackedBody &body,
                  TrackedBodyTarget &target, CameraParameters const &camParams)
            : camParams_(camParams), system_(std::move(system)), body_(&body),
              target_(&target) {}

        CameraParameters const &camParams_;
        std::unique_ptr<TrackingSystem> system_;
        TrackedBody *body_ = nullptr;
        TrackedBodyTarget *target_ = nullptr;
    };

    class FeedDataWithoutProcessing {
      public:
        void operator()(OptimData &optim, TimestampedMeasurements const &row) {
            auto inputData =
                makeImageOutputDataFromRow(row, optim.getCamParams());
            optim.getSystem().updateLedsFromVideoData(std::move(inputData));
        }
    };

    class MainAlgoUnderStudy {
      public:
        void operator()(OptimData &optim, TimestampedMeasurements const &row) {
            auto inputData =
                makeImageOutputDataFromRow(row, optim.getCamParams());
            auto indices = optim.getSystem().updateBodiesFromVideoData(
                std::move(inputData));
            gotPose = optim.getBody().hasPoseEstimate();
            if (gotPose) {
                pose = optim.getBody().getState().getIsometry();
            }
        }
        bool havePose() const { return gotPose; }
        Eigen::Isometry3d const &getPose() const { return pose; }
        std::size_t getNumResets(OptimData const &optim) {
            return optim.getTarget().numTrackingResets();
        }

      private:
        bool gotPose = false;
        Eigen::Isometry3d pose;
    };

    class RansacOneEuro {
      public:
        void operator()(OptimData &optim, TimestampedMeasurements const &row) {
            gotPose = false;
            flippedPose_ = false;
            Eigen::Vector3d pos;
            Eigen::Quaterniond quat;
            auto gotRansac =
                optim.getTarget().uncalibratedRANSACPoseEstimateFromLeds(
                    optim.getCamParams(), pos, quat);
            if (gotRansac) {
                double yAxisYComponent = (quat * Eigen::Vector3d::UnitY()).y();
                if (yAxisYComponent < 0) {
                    // std::cout << "RANSAC picked upside-down!" << std::endl;
                    flippedPose_ = true;
                    numFlips_++;
                    return;
                }
                double dt = 1;
                if (isFirst) {
                    isFirst = false;
                } else {
                    dt = osvrTimeValueDurationSeconds(&row.tv, &last);
                }
                ransacPoseFilter.filter(dt, pos, quat);
                last = row.tv;
                gotPose = true;
                pose = ransacPoseFilter.getIsometry();
            }
        }
        bool flippedPose() const { return flippedPose_; }
        std::size_t getNumFlips() const { return numFlips_; }
        bool havePose() const { return gotPose; }
        Eigen::Isometry3d const &getPose() const { return pose; }

      private:
        util::filters::PoseOneEuroFilterd ransacPoseFilter;
        TimeValue last;
        bool isFirst = true;
        bool gotPose = false;
        bool flippedPose_ = false;
        std::size_t numFlips_ = 0;
        Eigen::Isometry3d pose;
    };

    class ReferenceTracker {
      public:
        ReferenceTracker()
            : base_(reftracker::getBaseTransform(
                  reftracker::getRefTrackerTransformParams())),
              inner_(reftracker::getInnerTransform(
                  reftracker::getRefTrackerTransformParams())) {}

        void operator()(OptimData &optim, TimestampedMeasurements const &row) {
            // Update our pose by the reference pose transformed.
            pose_ = base_ * makeIsometry(row.xlate, row.rot) * inner_;
        }
        bool havePose() const { return true; }
        Eigen::Isometry3d const &getPose() const { return pose_; }

      private:
        Eigen::Isometry3d base_;
        Eigen::Isometry3d inner_;
        Eigen::Isometry3d pose_;
    };
} // namespace vbtracker
} // namespace osvr
#endif // INCLUDED_OptimizationBase_h_GUID_C3489E14_63D4_4290_45C0_225AC84A8BF5