mirror of
https://gitlab.freedesktop.org/monado/monado.git
synced 2025-01-04 06:06:17 +00:00
tracking: Add t_fusion for Kalman filter added code.
This commit is contained in:
Ryan Pavlik
Jakob Bornecrantz
parent
c660183848
commit
85b6c7222d
|
|
@ -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()
|
||||
|
|
|
|||
|
|
@ -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,
|
||||
)
|
||||
|
|
|
|||
241
src/xrt/auxiliary/tracking/t_fusion.h
Normal file
241
src/xrt/auxiliary/tracking/t_fusion.h
Normal file
|
|
@ -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 <ryan.pavlik@collabora.com>
|
||||
*
|
||||
* @ingroup aux_tracking
|
||||
*/
|
||||
#pragma once
|
||||
|
||||
|
||||
#ifndef __cplusplus
|
||||
#error "This header is C++-only."
|
||||
#endif
|
||||
|
||||
#include <Eigen/Core>
|
||||
#include <Eigen/Geometry>
|
||||
|
||||
#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 <typename State>
|
||||
class WorldDirectionMeasurement
|
||||
: public flexkalman::MeasurementBase<WorldDirectionMeasurement<State>>
|
||||
{
|
||||
public:
|
||||
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
|
||||
static constexpr size_t Dimension = 3;
|
||||
using MeasurementVector = types::Vector<Dimension>;
|
||||
using MeasurementSquareMatrix = types::SquareMatrix<Dimension>;
|
||||
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<LinAccelWithGravityMeasurement>
|
||||
{
|
||||
public:
|
||||
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
|
||||
static constexpr size_t Dimension = 3;
|
||||
using MeasurementVector = types::Vector<Dimension>;
|
||||
using MeasurementSquareMatrix = types::SquareMatrix<Dimension>;
|
||||
LinAccelWithGravityMeasurement(types::Vector<3> const &direction,
|
||||
types::Vector<3> const &reference,
|
||||
types::Vector<3> const &variance)
|
||||
: direction_(direction), reference_(reference),
|
||||
covariance_(variance.asDiagonal())
|
||||
{}
|
||||
|
||||
// template <typename State>
|
||||
MeasurementSquareMatrix const &
|
||||
getCovariance(State const & /*s*/)
|
||||
{
|
||||
return covariance_;
|
||||
}
|
||||
|
||||
// template <typename State>
|
||||
types::Vector<3>
|
||||
predictMeasurement(State const &s) const
|
||||
{
|
||||
return reference_;
|
||||
}
|
||||
|
||||
// template <typename State>
|
||||
MeasurementVector
|
||||
getResidual(MeasurementVector const &predictedMeasurement,
|
||||
State const &s) const
|
||||
{
|
||||
s.getQuaternion().conjugate() *
|
||||
predictedMeasurement return predictedMeasurement -
|
||||
reference_.normalized();
|
||||
}
|
||||
|
||||
template <typename State>
|
||||
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<BiasedGyroMeasurement>
|
||||
{
|
||||
public:
|
||||
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
|
||||
static constexpr size_t Dimension = 3;
|
||||
using MeasurementVector = types::Vector<Dimension>;
|
||||
using MeasurementSquareMatrix = types::SquareMatrix<Dimension>;
|
||||
BiasedGyroMeasurement(types::Vector<3> const &angVel,
|
||||
types::Vector<3> const &variance)
|
||||
: angVel_(angVel), covariance_(variance.asDiagonal())
|
||||
{}
|
||||
|
||||
template <typename State>
|
||||
MeasurementSquareMatrix const &
|
||||
getCovariance(State const & /*s*/)
|
||||
{
|
||||
return covariance_;
|
||||
}
|
||||
|
||||
template <typename State>
|
||||
types::Vector<3>
|
||||
predictMeasurement(State const &s) const
|
||||
{
|
||||
return s.b().stateVector() + angVel_;
|
||||
}
|
||||
|
||||
template <typename State>
|
||||
MeasurementVector
|
||||
getResidual(MeasurementVector const &predictedMeasurement,
|
||||
State const &s) const
|
||||
{
|
||||
return predictedMeasurement - s.a().angularVelocity();
|
||||
}
|
||||
|
||||
template <typename State>
|
||||
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<AbsolutePositionLeverArmMeasurement>
|
||||
{
|
||||
public:
|
||||
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
|
||||
using State = flexkalman::pose_externalized_rotation::State;
|
||||
static constexpr size_t Dimension = 3;
|
||||
using MeasurementVector = types::Vector<Dimension>;
|
||||
using MeasurementSquareMatrix = types::SquareMatrix<Dimension>;
|
||||
|
||||
/*!
|
||||
* @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
|
||||
Loading…
Reference in a new issue