PoseEstimator_RANSACKalman.cpp

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.

// Internal Includes
#include "PoseEstimator_RANSACKalman.h"

// Library/third-party includes
#include <osvr/Kalman/AbsoluteOrientationMeasurement.h>
#include <osvr/Kalman/AbsolutePositionMeasurement.h>
#include <osvr/Kalman/FlexibleKalmanFilter.h>

// Standard includes
// - none

namespace osvr {
namespace vbtracker {
    RANSACKalmanPoseEstimator::RANSACKalmanPoseEstimator(
        double positionVarianceScale, double orientationVariance)
        : m_positionVarianceScale(positionVarianceScale),
          m_orientationVariance(orientationVariance) {}

    bool RANSACKalmanPoseEstimator::
    operator()(EstimatorInOutParams const &p, LedPtrList const &leds,
               osvr::util::time::TimeValue const &frameTime) {

        Eigen::Vector3d xlate;
        Eigen::Quaterniond quat;
        {
            auto ret = m_ransac(p.camParams, leds, p.beacons, p.beaconDebug,
                                xlate, quat);
            if (!ret) {
                return false;
            }
        }


        if (p.startingTime != frameTime) {
            auto dt = util::time::duration(frameTime, p.startingTime);
            kalman::predict(p.state, p.processModel, dt);
        }

        {
            kalman::AbsoluteOrientationMeasurement<BodyState> meas(
                quat, Eigen::Vector3d::Constant(m_orientationVariance));
            kalman::correct(p.state, p.processModel, meas);
        }
        {
            kalman::AbsolutePositionMeasurement<BodyState> meas(
                xlate, Eigen::Vector3d::Constant(m_positionVarianceScale *
                                                 xlate.z() * xlate.z()));
            kalman::correct(p.state, p.processModel, meas);
        }
        return true;
    }
} // namespace vbtracker
} // namespace osvr