t_lowpass_vector.hpp

Go to the documentation of this file.

// Copyright 2019, Collabora, Ltd.
// SPDX-License-Identifier: BSL-1.0
/*!
 * @file
 * @brief  Low-pass IIR filter on vectors
 * @author Ryan Pavlik <ryan.pavlik@collabora.com>
 * @ingroup aux_tracking
 */

#pragma once

#ifndef __cplusplus
#error "This header is C++-only."
#endif

#include "tracking/t_lowpass.hpp"

#include <Eigen/Core>


namespace xrt_fusion {

/*!
 * A very simple low-pass filter, using a "one-pole infinite impulse response"
 * design (one-pole IIR).
 *
 * Configurable in dimension and scalar type.
 */
template <size_t Dim, typename Scalar> class LowPassIIRVectorFilter
{
public:
        EIGEN_MAKE_ALIGNED_OPERATOR_NEW

        using Vector = Eigen::Matrix<Scalar, Dim, 1>;

        /*!
         * Constructor
         *
         * @param cutoff_hz A cutoff frequency in Hertz: signal changes much
         * lower in frequency will be passed through the filter, while signal
         * changes much higher in frequency will be blocked.
         */
        explicit LowPassIIRVectorFilter(Scalar cutoff_hz) noexcept
            : impl_(cutoff_hz, Vector::Zero())
        {}


        /*!
         * Reset the filter to just-created state.
         */
        void
        reset() noexcept
        {
                impl_.reset(Vector::Zero());
        }

        /*!
         * Filter a sample, with an optional weight.
         *
         * @param sample The value to filter
         * @param timestamp_ns The time that this sample was measured.
         * @param weight An optional value between 0 and 1. The smaller this
         * value, the less the current sample influences the filter state. For
         * the first call, this is always assumed to be 1.
         */
        void
        addSample(Vector const &sample,
                  std::uint64_t timestamp_ns,
                  Scalar weight = 1)
        {
                impl_.addSample(sample, timestamp_ns, weight);
        }

        /*!
         * Access the filtered value.
         */
        Vector const &
        getState() const noexcept
        {
                return impl_.state;
        }

        /*!
         * Access the time of last update.
         */
        std::uint64_t
        getTimestampNs() const noexcept
        {
                return impl_.filter_timestamp_ns;
        }

        /*!
         * Access whether we have initialized state.
         */
        bool
        isInitialized() const noexcept
        {
                return impl_.initialized;
        }

private:
        implementation::LowPassIIR<Vector, Scalar> impl_;
};

} // namespace xrt_fusion