mirror of
https://gitlab.freedesktop.org/monado/monado.git
synced 2025-01-22 14:41:47 +00:00
tracking: Add t_fusion for Kalman filter added code.
This commit is contained in:
parent
c660183848
commit
85b6c7222d
|
@ -31,6 +31,7 @@ set(TRACKING_SOURCE_FILES
|
||||||
tracking/t_debug_hsv_picker.cpp
|
tracking/t_debug_hsv_picker.cpp
|
||||||
tracking/t_debug_hsv_viewer.cpp
|
tracking/t_debug_hsv_viewer.cpp
|
||||||
tracking/t_file.cpp
|
tracking/t_file.cpp
|
||||||
|
tracking/t_fusion.h
|
||||||
tracking/t_hsv_filter.c
|
tracking/t_hsv_filter.c
|
||||||
tracking/t_tracker_psmv.cpp
|
tracking/t_tracker_psmv.cpp
|
||||||
tracking/t_tracker_psvr.cpp
|
tracking/t_tracker_psvr.cpp
|
||||||
|
@ -104,10 +105,11 @@ if(BUILD_TRACKING)
|
||||||
add_library(aux_tracking OBJECT ${TRACKING_SOURCE_FILES})
|
add_library(aux_tracking OBJECT ${TRACKING_SOURCE_FILES})
|
||||||
set_property(TARGET aux_tracking PROPERTY POSITION_INDEPENDENT_CODE ON)
|
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
|
target_include_directories(aux_tracking SYSTEM
|
||||||
PRIVATE
|
PRIVATE
|
||||||
${EIGEN3_INCLUDE_DIR}
|
${EIGEN3_INCLUDE_DIR}
|
||||||
${OpenCV_INCLUDE_DIRS}
|
${OpenCV_INCLUDE_DIRS}
|
||||||
|
${CMAKE_CURRENT_SOURCE_DIR}/../../external
|
||||||
)
|
)
|
||||||
endif()
|
endif()
|
||||||
|
|
|
@ -100,6 +100,7 @@ tracking_srcs = [
|
||||||
'tracking/t_debug_hsv_picker.cpp',
|
'tracking/t_debug_hsv_picker.cpp',
|
||||||
'tracking/t_debug_hsv_viewer.cpp',
|
'tracking/t_debug_hsv_viewer.cpp',
|
||||||
'tracking/t_file.cpp',
|
'tracking/t_file.cpp',
|
||||||
|
'tracking/t_fusion.h',
|
||||||
'tracking/t_hsv_filter.c',
|
'tracking/t_hsv_filter.c',
|
||||||
'tracking/t_tracker_psmv.cpp',
|
'tracking/t_tracker_psmv.cpp',
|
||||||
'tracking/t_tracker_psvr.cpp',
|
'tracking/t_tracker_psvr.cpp',
|
||||||
|
@ -109,7 +110,7 @@ tracking_srcs = [
|
||||||
lib_aux_tracking = static_library(
|
lib_aux_tracking = static_library(
|
||||||
'aux_tracking',
|
'aux_tracking',
|
||||||
files(tracking_srcs),
|
files(tracking_srcs),
|
||||||
include_directories: xrt_include,
|
include_directories: [xrt_include, external_include],
|
||||||
dependencies: [eigen3, opencv],
|
dependencies: [eigen3, opencv],
|
||||||
build_by_default: build_tracking,
|
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