diff --git a/src/xrt/auxiliary/CMakeLists.txt b/src/xrt/auxiliary/CMakeLists.txt index 9168599f2..9407a122b 100644 --- a/src/xrt/auxiliary/CMakeLists.txt +++ b/src/xrt/auxiliary/CMakeLists.txt @@ -31,6 +31,7 @@ set(TRACKING_SOURCE_FILES tracking/t_debug_hsv_picker.cpp tracking/t_debug_hsv_viewer.cpp tracking/t_file.cpp + tracking/t_fusion.h tracking/t_hsv_filter.c tracking/t_tracker_psmv.cpp tracking/t_tracker_psvr.cpp @@ -104,10 +105,11 @@ if(BUILD_TRACKING) add_library(aux_tracking OBJECT ${TRACKING_SOURCE_FILES}) set_property(TARGET aux_tracking PROPERTY POSITION_INDEPENDENT_CODE ON) - # Math files has extra include(s). + # Tracking files have extra includes. target_include_directories(aux_tracking SYSTEM PRIVATE ${EIGEN3_INCLUDE_DIR} ${OpenCV_INCLUDE_DIRS} + ${CMAKE_CURRENT_SOURCE_DIR}/../../external ) endif() diff --git a/src/xrt/auxiliary/meson.build b/src/xrt/auxiliary/meson.build index fadffc0b3..4fad34f9d 100644 --- a/src/xrt/auxiliary/meson.build +++ b/src/xrt/auxiliary/meson.build @@ -100,6 +100,7 @@ tracking_srcs = [ 'tracking/t_debug_hsv_picker.cpp', 'tracking/t_debug_hsv_viewer.cpp', 'tracking/t_file.cpp', + 'tracking/t_fusion.h', 'tracking/t_hsv_filter.c', 'tracking/t_tracker_psmv.cpp', 'tracking/t_tracker_psvr.cpp', @@ -109,7 +110,7 @@ tracking_srcs = [ lib_aux_tracking = static_library( 'aux_tracking', files(tracking_srcs), - include_directories: xrt_include, + include_directories: [xrt_include, external_include], dependencies: [eigen3, opencv], build_by_default: build_tracking, ) diff --git a/src/xrt/auxiliary/tracking/t_fusion.h b/src/xrt/auxiliary/tracking/t_fusion.h new file mode 100644 index 000000000..a59ef5f42 --- /dev/null +++ b/src/xrt/auxiliary/tracking/t_fusion.h @@ -0,0 +1,241 @@ +// Copyright 2019, Collabora, Ltd. +// SPDX-License-Identifier: BSL-1.0 +/*! + * @file + * @brief C++ sensor fusion/filtering code that uses flexkalman + * @author Ryan Pavlik + * + * @ingroup aux_tracking + */ +#pragma once + + +#ifndef __cplusplus +#error "This header is C++-only." +#endif + +#include +#include + +#include "flexkalman/AugmentedProcessModel.h" +#include "flexkalman/AugmentedState.h" +#include "flexkalman/BaseTypes.h" +#include "flexkalman/PoseState.h" + +namespace xrt_fusion { +namespace types = flexkalman::types; +using flexkalman::types::Vector; + +//! For things like accelerometers, which on some level measure the local vector +//! of a world direction. +template +class WorldDirectionMeasurement + : public flexkalman::MeasurementBase> +{ +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + static constexpr size_t Dimension = 3; + using MeasurementVector = types::Vector; + using MeasurementSquareMatrix = types::SquareMatrix; + WorldDirectionMeasurement(types::Vector<3> const &direction, + types::Vector<3> const &reference, + types::Vector<3> const &variance) + : direction_(direction.normalized()), + reference_(reference.normalized()), + covariance_(variance.asDiagonal()) + {} + + MeasurementSquareMatrix const & + getCovariance(State const & /*s*/) + { + return covariance_; + } + + types::Vector<3> + predictMeasurement(State const &s) const + { + return s.getCombinedQuaternion() * reference_; + } + + MeasurementVector + getResidual(MeasurementVector const &predictedMeasurement, + State const &s) const + { + return predictedMeasurement - reference_; + } + + MeasurementVector + getResidual(State const &s) const + { + return getResidual(predictMeasurement(s), s); + } + +private: + types::Vector<3> direction_; + types::Vector<3> reference_; + MeasurementSquareMatrix covariance_; +}; +#if 0 +//! For things like accelerometers, which on some level measure the local vector +//! of a world direction. +class LinAccelWithGravityMeasurement + : public flexkalman::MeasurementBase +{ +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + static constexpr size_t Dimension = 3; + using MeasurementVector = types::Vector; + using MeasurementSquareMatrix = types::SquareMatrix; + LinAccelWithGravityMeasurement(types::Vector<3> const &direction, + types::Vector<3> const &reference, + types::Vector<3> const &variance) + : direction_(direction), reference_(reference), + covariance_(variance.asDiagonal()) + {} + + // template + MeasurementSquareMatrix const & + getCovariance(State const & /*s*/) + { + return covariance_; + } + + // template + types::Vector<3> + predictMeasurement(State const &s) const + { + return reference_; + } + + // template + MeasurementVector + getResidual(MeasurementVector const &predictedMeasurement, + State const &s) const + { + s.getQuaternion().conjugate() * + predictedMeasurement return predictedMeasurement - + reference_.normalized(); + } + + template + MeasurementVector + getResidual(State const &s) const + { + MeasurementVector residual = + direction_ - reference_ * s.getQuaternion(); + return getResidual(predictMeasurement(s), s); + } + +private: + types::Vector<3> direction_; + types::Vector<3> reference_; + MeasurementSquareMatrix covariance_; +}; +#endif + +class BiasedGyroMeasurement + : public flexkalman::MeasurementBase +{ +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + static constexpr size_t Dimension = 3; + using MeasurementVector = types::Vector; + using MeasurementSquareMatrix = types::SquareMatrix; + BiasedGyroMeasurement(types::Vector<3> const &angVel, + types::Vector<3> const &variance) + : angVel_(angVel), covariance_(variance.asDiagonal()) + {} + + template + MeasurementSquareMatrix const & + getCovariance(State const & /*s*/) + { + return covariance_; + } + + template + types::Vector<3> + predictMeasurement(State const &s) const + { + return s.b().stateVector() + angVel_; + } + + template + MeasurementVector + getResidual(MeasurementVector const &predictedMeasurement, + State const &s) const + { + return predictedMeasurement - s.a().angularVelocity(); + } + + template + MeasurementVector + getResidual(State const &s) const + { + return getResidual(predictMeasurement(s), s); + } + +private: + types::Vector<3> angVel_; + MeasurementSquareMatrix covariance_; +}; +/*! + * For PS Move-like things, where there's a directly-computed absolute position + * that is not at the tracked body's origin. + */ +class AbsolutePositionLeverArmMeasurement + : public flexkalman::MeasurementBase +{ +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + using State = flexkalman::pose_externalized_rotation::State; + static constexpr size_t Dimension = 3; + using MeasurementVector = types::Vector; + using MeasurementSquareMatrix = types::SquareMatrix; + + /*! + * @todo the point we get from the camera isn't the center of the ball, + * but the center of the visible surface of the ball - a closer + * approximation would be translation along the vector to the center of + * projection.... + */ + AbsolutePositionLeverArmMeasurement( + MeasurementVector const &measurement, + MeasurementVector const &knownLocationInBodySpace, + MeasurementVector const &variance) + : measurement_(measurement), + knownLocationInBodySpace_(knownLocationInBodySpace), + covariance_(variance.asDiagonal()) + {} + + MeasurementSquareMatrix const & + getCovariance(State const & /*s*/) + { + return covariance_; + } + + types::Vector<3> + predictMeasurement(State const &s) const + { + return s.getIsometry() * knownLocationInBodySpace_; + } + + MeasurementVector + getResidual(MeasurementVector const &predictedMeasurement, + State const & /*s*/) const + { + return measurement_ - predictedMeasurement; + } + + MeasurementVector + getResidual(State const &s) const + { + return getResidual(predictMeasurement(s), s); + } + +private: + MeasurementVector measurement_; + MeasurementVector knownLocationInBodySpace_; + MeasurementSquareMatrix covariance_; +}; +} // namespace xrt_fusion