Final clean after new buffer implementation

- Moved InsertEntryAtTimestamp to AddEntrySorted
- Enabeled Chi2 test by default
- Added e2e Chi2 test
- Minor cleanups e.g. removal of unnecessary include

source/mars/include/mars/buffer.h

@@ -202,6 +202,9 @@ class Buffer
   /// \return Index of the added entry. The index is -1 if the entry was removed because the max_buffer_size was
   /// reached.
   ///
+  /// The method finds the closest timestamp based on the shortest 'time' distance between the new and existing buffer
+  /// elements. It then determines if the entry needs to be added before or after the closest entry.
+  ///
   int AddEntrySorted(const BufferEntryType& new_entry, const bool& after = true);
 
   ///
@@ -227,15 +230,6 @@ class Buffer
   ///
   bool IsSorted() const;
 
-  ///
-  /// \brief Inserting new element before the element at the specified position
-  /// \param new_entry ntry that is added to the buffer
-  ///
-  /// The method finds the closest timestamp based on the shortest 'time' distance between the new and existing buffer
-  /// elements. It then determines if the entry needs to be added before or after the closest entry.
-  ///
-  int InsertDataAtTimestamp(const BufferEntryType& new_entry, const bool& after = true);
-
   ///
   /// \brief InsertDataAtIndex Adds 'entry' at buffer position 'index'
   /// \param new_entry Entry buffer entry to be added

source/mars/include/mars/data_utils/write_csv.h

@@ -23,7 +23,6 @@ class WriteCsv
   inline static std::string vec_to_csv(const Eigen::VectorXd& a)
   {
     std::stringstream os;
-    // TODO(chb) option to pre or post add comma
     for (int k = 0; k < a.size(); k++)
     {
       os << ", " << a(k);

source/mars/include/mars/ekf.h

@@ -80,7 +80,7 @@ class Chi2
   int dof_{ 3 };                   /// Degrees of freedom for the setup
   double chi_value_{ 0.05 };       /// Chi value for the confidence intervall (0.05 represents 95% test)
   double ucv_;                     /// Upper critival value
-  bool do_test_{ false };          /// Determine if the test is performed or not
+  bool do_test_{ true };          /// Determine if the test is performed or not
   bool passed_{ false };           /// Shows if the test passed or not (true=passed)
 
 private:

source/mars/source/buffer.cpp

@@ -1,4 +1,4 @@
-// Copyright (C) 2021 Christian Brommer and Martin Scheiber, Control of Networked Systems, University of Klagenfurt,
+// Copyright (C) 2024 Christian Brommer and Martin Scheiber, Control of Networked Systems, University of Klagenfurt,
 // Austria.
 //
 // All rights reserved.
@@ -365,9 +365,49 @@ bool Buffer::RemoveSensorFromBuffer(const std::shared_ptr<SensorAbsClass>& senso
 
 int Buffer::AddEntrySorted(const BufferEntryType& new_entry, const bool& after)
 {
-  int index = InsertDataAtTimestamp(new_entry, after);
+  if (this->IsEmpty())
+  {
+    data_.push_back(new_entry);
+    // entry is added at idx 0, buffer was empty
+    return 0;
+  }
+
+  BufferEntryType latest_entry;
+  this->get_latest_entry(&latest_entry);
+  if (latest_entry <= new_entry)
+  {
+    data_.push_back(new_entry);
+    // get index based on iterator
+    return static_cast<int>(data_.end() - data_.begin()) - 1;
+  }
+
+  Time previous_time_distance(1e100);
+  const Time timestamp = new_entry.timestamp_;
+
+  // iterate backwards and start with latest entry
+  // find the first entry at which (state_entry_stamp - new_stamp) is >=0
+  // the new entry is entered after this index (idx+1)
+  for (int k = data_.size() - 1; k >= 0; --k)
+  {
+    Time current_time_distance = timestamp - data_[k].timestamp_;
+
+    if (current_time_distance.get_seconds() >= 0)
+    {
+      int insert_idx = k;
+
+      if (after)
+      {
+        insert_idx += 1;
+      }
+
+      data_.insert(data_.begin() + insert_idx, new_entry);
+      return insert_idx;  // return entry index
+    }
+  }
 
-  return index;
+  // If the buffer has only one element and the new entry is older then the existing entry
+  data_.push_front(new_entry);
+  return 0;  // push front adds element at index 0
 }
 
 int Buffer::FindClosestTimestamp(const Time& /*timestamp*/) const
@@ -417,53 +457,6 @@ bool Buffer::IsSorted() const
   return std::is_sorted(data_.begin(), data_.end());
 }
 
-int Buffer::InsertDataAtTimestamp(const BufferEntryType& new_entry, const bool& after)
-{
-  if (this->IsEmpty())
-  {
-    data_.push_back(new_entry);
-    // entry is added at idx 0, buffer was empty
-    return 0;
-  }
-
-  BufferEntryType latest_entry;
-  this->get_latest_entry(&latest_entry);
-  if (latest_entry <= new_entry)
-  {
-    data_.push_back(new_entry);
-    // get index based on iterator
-    return static_cast<int>(data_.end() - data_.begin()) - 1;
-  }
-
-  Time previous_time_distance(1e100);
-  const Time timestamp = new_entry.timestamp_;
-
-  // iterate backwards and start with latest entry
-  // find the first entry at which (state_entry_stamp - new_stamp) is >=0
-  // the new entry is entered after this index (idx+1)
-  for (int k = data_.size() - 1; k >= 0; --k)
-  {
-    Time current_time_distance = timestamp - data_[k].timestamp_;
-
-    if (current_time_distance.get_seconds() >= 0)
-    {
-      int insert_idx = k;
-
-      if (after)
-      {
-        insert_idx += 1;
-      }
-
-      data_.insert(data_.begin() + insert_idx, new_entry);
-      return insert_idx;  // return entry index
-    }
-  }
-
-  // If the buffer has only one element and the new entry is older then the existing entry
-  data_.push_front(new_entry);
-  return 0;  // push front adds element at index 0
-}
-
 bool Buffer::OverwriteDataAtIndex(const BufferEntryType& new_entry, const int& index)
 {
   if (index < (this->get_length()))

source/mars/source/buffer_entry_type.cpp

@@ -1,4 +1,4 @@
-// Copyright (C) 2021 Christian Brommer, Control of Networked Systems, University of Klagenfurt, Austria.
+// Copyright (C) 2024 Christian Brommer, Control of Networked Systems, University of Klagenfurt, Austria.
 //
 // All rights reserved.
 //

source/mars/source/core_logic.cpp

@@ -155,7 +155,7 @@ bool CoreLogic::PerformSensorUpdate(std::shared_ptr<SensorAbsClass> sensor, cons
     BufferDataType init_data =
         sensor->Initialize(timestamp, sensor_data->data_.measurement_, std::make_shared<CoreType>(latest_core_data));
 
-    // TODO the init function should directly receive the state entry and return it
+    // TODO (CHB) the init function should directly receive the state entry and return it
     sensor_data->data_.set_states(init_data.core_state_, init_data.sensor_state_);
     sensor_data->metadata_ = BufferMetadataType::init;
 
@@ -239,12 +239,13 @@ bool CoreLogic::PerformSensorUpdate(std::shared_ptr<SensorAbsClass> sensor, cons
       sensor->CalcUpdate(timestamp, sensor_data->data_.measurement_, prior_core_data.state_,
                          prior_sensor_state_entry.data_.sensor_state_, corrected_cov, &corrected_state_data);
 
-  // TODO: This should also happen inside the update class or a preset object should be given that already has the
+  // TODO(CHB): This should also happen inside the update class or a preset object should be given that already has the
   // measurement
 
   if (successful_update)
   {
     sensor_data->data_.set_states(corrected_state_data.core_state_, corrected_state_data.sensor_state_);
+    sensor_data->metadata_ = mars::BufferMetadataType::invalid;
 
     if (verbose_)
     {
@@ -341,6 +342,7 @@ void CoreLogic::ReworkBufferStartingAtIndex(const int& index)
       //  Processing update sensors
       bool added_interm_state = false;
       PerformSensorUpdate(sensor_handle, timestamp, &current_measurement_buffer_entry, &added_interm_state);
+
       if (added_interm_state)
       {
         current_state_entry_idx++;

source/mars/source/time.cpp

@@ -12,7 +12,6 @@
 #include <chrono>
 #include <cmath>
 #include <iostream>
-#include <iomanip>
 
 namespace mars
 {
@@ -28,7 +27,7 @@ Time Time::get_time_now()
 {
   using namespace std::chrono;
   auto sys_time_now = system_clock::now();
-  double sys_sec = double(duration_cast<milliseconds>(sys_time_now.time_since_epoch()).count())/1e3;
+  double sys_sec = double(duration_cast<milliseconds>(sys_time_now.time_since_epoch()).count()) / 1e3;
 
   // Cast to millisecond precision only
   // auto sys_ms = (duration_cast<milliseconds>(sys_time_now.time_since_epoch()) -

source/tests/mars-e2e-test/CMakeLists.txt

@@ -24,6 +24,7 @@ set(sources
     mars_e2e_imu_prop.cpp
     mars_e2e_imu_pose_update.cpp
     mars_e2e_imu_pose_ooo_rework.cpp
+    mars_e2e_imu_pose_outlier.cpp
     mars_e2e_imu_pose_update_perf.cpp
     mars_e2e_imu_prop_empty_updates.cpp
 )