Commit 925d1596 authored by Ryan Pavlik's avatar Ryan Pavlik

t/psmv: Switch to using a deque for now until we have a good ring buffer class separately.

parent e48c5331
Pipeline #146895 passed with stages
in 1 minute and 40 seconds
......@@ -27,7 +27,7 @@
#include "flexkalman/PoseSeparatelyDampedConstantVelocity.h"
#include "flexkalman/PoseState.h"
#include <array>
#include <deque>
using State = flexkalman::pose_externalized_rotation::State;
using ProcessModel =
......@@ -47,85 +47,6 @@ namespace {
xrt_quat quat;
xrt_vec3 variance;
};
constexpr size_t RecordBufferLength = 32;
class MeasurementRecordBuffer
{
public:
void
clear() noexcept
{
record_begin = 0;
record_end = 0;
}
size_t
size() const noexcept
{
size_t rend = record_end;
if (rend < record_begin) {
rend += RecordBufferLength;
}
return rend - record_begin;
}
bool
full() const noexcept
{
return size() == RecordBufferLength - 1;
}
bool
empty() const noexcept
{
return record_begin == record_end;
}
void
pop_front()
{
if (!empty()) {
record_begin =
(record_begin + 1) % RecordBufferLength;
}
}
void
push_back(ImuMeasurementRecord const &record)
{
if (full()) {
throw std::runtime_error(
"Ran out of space in IMU record buffer!");
}
records[record_end] = record;
record_end = (record_end + 1) % RecordBufferLength;
}
ImuMeasurementRecord *
begin() noexcept
{
return &(records[record_begin]);
}
ImuMeasurementRecord const *
begin() const noexcept
{
return &(records[record_begin]);
}
ImuMeasurementRecord *
end() noexcept
{
return &(records[record_end]);
}
ImuMeasurementRecord const *
end() const noexcept
{
return &(records[record_end]);
}
private:
size_t record_end = 0;
size_t record_begin = 0;
std::array<ImuMeasurementRecord, RecordBufferLength> records;
};
struct TimestampedState
{
State state;
......@@ -192,9 +113,13 @@ namespace {
TimestampedState finalized_state;
//! These are measurements that have entered fast_state
//! but not finalized_state.
MeasurementRecordBuffer buffer;
/*!
* @brief These are measurements that have entered fast_state
* but not finalized_state.
*
* @todo replace with a ring buffer
*/
std::deque<ImuMeasurementRecord> buffer;
xrt_fusion::SimpleIMUFusion imu;
......@@ -268,11 +193,6 @@ namespace {
const struct xrt_tracking_sample *sample,
const struct xrt_vec3 *orientation_variance_optional)
{
Eigen::Vector3f variance = Eigen::Vector3f::Constant(0.01f);
if (orientation_variance_optional) {
variance = map_vec3(*orientation_variance_optional);
}
imu.handleAccel(map_vec3(sample->accel_m_s2).cast<double>(),
timestamp_ns);
imu.handleGyro(map_vec3(sample->gyro_rad_secs).cast<double>(),
......@@ -282,7 +202,11 @@ namespace {
//! @todo use better measurements instead of the above
//! "simple fusion"
// "push" this record onto the buffer.
// push this record onto the buffer.
Eigen::Vector3f variance = Eigen::Vector3f::Constant(0.01f);
if (orientation_variance_optional) {
variance = map_vec3(*orientation_variance_optional);
}
ImuMeasurementRecord record;
map_quat(record.quat) = imu.getQuat().cast<float>();
record.timestamp_ns = timestamp_ns;
......@@ -291,7 +215,6 @@ namespace {
if (predict_and_incorporate_imu_measurement(fast_state,
record)) {
orientation_state.tracked = true;
orientation_state.valid = true;
} else {
......@@ -313,7 +236,7 @@ namespace {
while (!buffer.empty() &&
buffer.begin()->timestamp_ns <= timestamp_ns) {
if (!predict_and_incorporate_imu_measurement(
finalized_state, *buffer.begin())) {
finalized_state, buffer.front())) {
//! @todo handle more gracefully
reset_filter_and_imu();
return;
......
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment