Merge branch 'development' into main

source/mars/include/mars/buffer.h

@@ -38,7 +38,7 @@ class Buffer
   /// \brief Buffer default constructor
   /// max buffer size is set to 400 by default
   ///
-  Buffer() = default;
+  Buffer();
 
   ///
   /// \brief Buffer constructor
@@ -79,7 +79,10 @@ class Buffer
   /// \brief get_length
   /// \return current number of elements stored in the buffer
   ///
-  int get_length() const;
+  inline int get_length() const
+  {
+    return static_cast<int>(data_.size());
+  }
 
   ///
   /// \brief PrintBufferEntries prints all buffer entries in a formatted way
@@ -199,7 +202,10 @@ class Buffer
   /// \return Index of the added entry. The index is -1 if the entry was removed because the max_buffer_size was
   /// reached.
   ///
-  int AddEntrySorted(const BufferEntryType& new_entry);
+  /// 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);
 
   ///
   /// \brief FindClosestTimestamp Returns the index of the entry which is the closest to the specified 'timestamp' when
@@ -211,44 +217,25 @@ class Buffer
 
   ///
   /// \brief Deletes all states after, and including the given index
+  /// This also deletes all auto generated states
+  ///
   /// \param idx Start index after which all states are deleted
   /// \return true if function was performed correct, false otherwise
   ///
-  bool DeleteStatesStartingAtIdx(const int& idx);
+  bool ClearStatesStartingAtIdx(const int& idx);
 
   ///
   /// \brief Checks if all buffer entrys are correctly sorted by time
   /// \return true if sorted, false otherwise
   ///
   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);
-
   ///
   /// \brief InsertDataAtIndex Adds 'entry' at buffer position 'index'
   /// \param new_entry Entry buffer entry to be added
   /// \param index position at which the entry is added
   ///
-  bool InsertDataAtIndex(const BufferEntryType& new_entry, const int& index);
-
-  ///
-  /// \brief InsertIntermediateData Insert data during the intermediate propagation step
-  ///
-  /// This function is intended for inserting a auto generated measurement and state before a current measurement entry.
-  /// This is required for intermediate propagation information.
-  ///
-  /// \param measurement
-  /// \param state
-  /// \return
-  ///
-  bool InsertIntermediateData(const BufferEntryType& measurement, const BufferEntryType& state);
+  bool OverwriteDataAtIndex(const BufferEntryType& new_entry, const int& index);
 
   ///
   /// \brief get_latest_interm_entrie Get last state pair of imu prop and sensor update
@@ -261,14 +248,15 @@ class Buffer
   ///
   /// \return True if successfull, false of no pair was found
   ///
-  bool get_intermediate_entry_pair(const std::shared_ptr<SensorAbsClass>& sensor_handle, BufferEntryType* imu_state, BufferEntryType* sensor_state) const;
+  bool get_intermediate_entry_pair(const std::shared_ptr<SensorAbsClass>& sensor_handle, BufferEntryType* imu_state,
+                                   BufferEntryType* sensor_state) const;
 
   ///
-  /// \brief CheckForLastHandle Checks if the given sensor handle only exists once in the buffer
-  /// \param sensor_handle
-  /// \return true if current sensor handle is the last in the buffer, false otherwise
+  /// \brief CheckForLastSensorHandleWithState Checks if the given sensor handle only exists once in the buffer and if
+  /// it has a state \param sensor_handle \return true if current sensor handle is the last in the buffer, false
+  /// otherwise
   ///
-  bool CheckForLastSensorHandlePair(const std::shared_ptr<SensorAbsClass>& sensor_handle) const;
+  bool CheckForLastSensorHandleWithState(const std::shared_ptr<SensorAbsClass>& sensor_handle) const;
 
   ///
   /// \brief RemoveOverflowEntrys Removes the oldest entries if max buffer size is reached

source/mars/include/mars/core_logic.h

@@ -30,8 +30,8 @@ class CoreLogic
   EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 
   std::shared_ptr<CoreState> core_states_{ nullptr };  /// Holds a pointer to the core_states
-  Buffer buffer_{ 300 };                               /// Main buffer of the filter
-  Buffer buffer_prior_core_init_{ 100 };               /// Buffer that holds measurements prior initialization
+  Buffer buffer_;                                      /// Main buffer of the filter
+  Buffer buffer_prior_core_init_;                      /// Buffer that holds measurements prior initialization
   SensorManager sensor_manager_;
   bool core_is_initialized_{ false };  /// core_is_initialized_ = true if the core state was initialized, false
                                        /// otherwise
@@ -72,8 +72,10 @@ class CoreLogic
   ///
   /// \brief PerformSensorUpdate Returns new state with corrected state and updated covariance
   ///
-  bool PerformSensorUpdate(BufferEntryType* state_buffer_entry_return, std::shared_ptr<SensorAbsClass> sensor,
-                           const Time& timestamp, std::shared_ptr<BufferDataType> data);
+  bool PerformSensorUpdate(std::shared_ptr<SensorAbsClass> sensor, const Time& timestamp, BufferEntryType* sensor_data);
+  bool PerformSensorUpdate(std::shared_ptr<SensorAbsClass> sensor, const Time& timestamp, BufferEntryType* sensor_data,
+                           bool* added_interm_state);
+
   ///
   /// \brief PerformCoreStatePropagation Propagates the core state and returns the new state entry
   ///
@@ -84,9 +86,9 @@ class CoreLogic
   /// The core_state propagation function needs to be able to
   /// handle the data structure of the propagation sensor.
   ///
-  BufferEntryType PerformCoreStatePropagation(std::shared_ptr<SensorAbsClass> sensor, const Time& timestamp,
-                                              const std::shared_ptr<BufferDataType>& data_measurement,
-                                              const std::shared_ptr<BufferEntryType>& prior_state_entry);
+  void PerformCoreStatePropagation(std::shared_ptr<SensorAbsClass> sensor, const Time& timestamp,
+                                   const BufferEntryType& prior_state_entry, BufferEntryType* sensor_entry);
+
   ///
   /// \brief ReworkBufferStartingAtIndex Reprocesses the buffer after an out of order update,
   /// starting at given 'idx'

source/mars/include/mars/data_utils/read_baro_data.h

@@ -45,9 +45,9 @@ class ReadBarometerData
       double pressure(csv_data["p"][k]);
 
       BufferDataType data;
-      data.set_sensor_data(std::make_shared<PressureMeasurementType>(pressure, temperature));
+      data.set_measurement(std::make_shared<PressureMeasurementType>(pressure, temperature));
 
-      BufferEntryType current_entry(time, data, sensor, BufferMetadataType::measurement);
+      BufferEntryType current_entry(time, data, sensor);
       data_out->at(k) = current_entry;
     }
   }

source/mars/include/mars/data_utils/read_csv.h

@@ -30,8 +30,9 @@ class ReadCsv
 {
   char delim{ ',' };
   HeaderMapType header_map;
+
 public:
-  ReadCsv(CsvDataType* csv_data, const std::string& file_path, char delim_=',') : delim(delim_)
+  ReadCsv(CsvDataType* csv_data, const std::string& file_path, char delim_ = ',') : delim(delim_)
   {
     if (!mars::filesystem::IsFile(file_path))
     {
@@ -62,42 +63,45 @@ class ReadCsv
     }
 
     CsvDataType csv_data_int;
-    for(auto it=header_map.begin(); it != header_map.end(); it++) {
-      csv_data_int[it->second].resize(rows-1, 0.0);
+    for (auto it = header_map.begin(); it != header_map.end(); it++)
+    {
+      csv_data_int[it->second].resize(rows - 1, 0.0);
     }
 
     // Read columns associated to header tokens
     std::string line;
     int line_counter = 0;
-    int parsed_row_counter = first_value_row; // header already parsed.
+    int parsed_row_counter = first_value_row;  // header already parsed.
 
     while (std::getline(file_, line))
     {
-
       std::stringstream row_stream(line);
       std::string token;
       int column_counter = 0;
 
       double item;
       while (std::getline(row_stream, token, delim))
       {
-        if(column_counter >= (int)header_map.size()) {
+        if (column_counter >= (int)header_map.size())
+        {
           std::cout << "ReadCsv(): Warning: too many entries in row!" << std::endl;
-          ++column_counter; // to indicate a corrupted row
+          ++column_counter;  // to indicate a corrupted row
           break;
         }
 
         std::istringstream is(token);
         is >> item;
-        csv_data_int[header_map[column_counter]][line_counter]=(item);
+        csv_data_int[header_map[column_counter]][line_counter] = (item);
         ++column_counter;
       }
 
       // check if row was corrupted, if so, overwrite current line with the next one
-      if(column_counter != (int)header_map.size()) {
+      if (column_counter != (int)header_map.size())
+      {
         std::cout << "ReadCsv(): Warning: corrupted row=" << parsed_row_counter << " will be skipped!" << std::endl;
       }
-      else {
+      else
+      {
         line_counter++;
       }
 
@@ -106,7 +110,8 @@ class ReadCsv
     }
 
     // shrink to the actual size
-    for(auto it=header_map.begin(); it != header_map.end(); it++) {
+    for (auto it = header_map.begin(); it != header_map.end(); it++)
+    {
       csv_data_int[it->second].resize(line_counter);
     }
 

source/mars/include/mars/data_utils/read_gps_data.h

@@ -42,10 +42,10 @@ class ReadGpsData
       Time time = csv_data["t"][k] + time_offset;
 
       BufferDataType data;
-      data.set_sensor_data(
+      data.set_measurement(
           std::make_shared<GpsMeasurementType>(csv_data["lat"][k], csv_data["long"][k], csv_data["alt"][k]));
 
-      BufferEntryType current_entry(time, data, sensor, BufferMetadataType::measurement);
+      BufferEntryType current_entry(time, data, sensor);
       data_out->at(k) = current_entry;
     }
   }

source/mars/include/mars/data_utils/read_gps_w_vel_data.h

@@ -42,11 +42,11 @@ class ReadGpsWithVelData
       Time time = csv_data["t"][k] + time_offset;
 
       BufferDataType data;
-      data.set_sensor_data(std::make_shared<GpsVelMeasurementType>(csv_data["lat"][k], csv_data["long"][k],
+      data.set_measurement(std::make_shared<GpsVelMeasurementType>(csv_data["lat"][k], csv_data["long"][k],
                                                                    csv_data["alt"][k], csv_data["v_x"][k],
                                                                    csv_data["v_y"][k], csv_data["v_z"][k]));
 
-      BufferEntryType current_entry(time, data, sensor, BufferMetadataType::measurement);
+      BufferEntryType current_entry(time, data, sensor);
       data_out->at(k) = current_entry;
     }
   }

source/mars/include/mars/data_utils/read_imu_data.h

@@ -45,9 +45,9 @@ class ReadImuData
       Eigen::Vector3d angular_velocity(csv_data["w_x"][k], csv_data["w_y"][k], csv_data["w_z"][k]);
 
       BufferDataType data;
-      data.set_sensor_data(std::make_shared<IMUMeasurementType>(linear_acceleration, angular_velocity));
+      data.set_measurement(std::make_shared<IMUMeasurementType>(linear_acceleration, angular_velocity));
 
-      BufferEntryType current_entry(time, data, sensor, BufferMetadataType::measurement);
+      BufferEntryType current_entry(time, data, sensor);
       data_out->at(k) = current_entry;
     }
   }

source/mars/include/mars/data_utils/read_mag_data.h

@@ -45,9 +45,9 @@ class ReadMagData
       Eigen::Vector3d mag_vec(csv_data["cart_x"][k], csv_data["cart_y"][k], csv_data["cart_z"][k]);
 
       BufferDataType data;
-      data.set_sensor_data(std::make_shared<MagMeasurementType>(mag_vec));
+      data.set_measurement(std::make_shared<MagMeasurementType>(mag_vec));
 
-      BufferEntryType current_entry(time, data, sensor, BufferMetadataType::measurement);
+      BufferEntryType current_entry(time, data, sensor);
       data_out->at(k) = current_entry;
     }
   }

source/mars/include/mars/data_utils/read_pose_data.h

@@ -47,9 +47,9 @@ class ReadPoseData
       orientation = Utils::NormalizeQuaternion(orientation, "pose csv reader");
 
       BufferDataType data;
-      data.set_sensor_data(std::make_shared<PoseMeasurementType>(position, orientation));
+      data.set_measurement(std::make_shared<PoseMeasurementType>(position, orientation));
 
-      BufferEntryType current_entry(time, data, sensor, BufferMetadataType::measurement);
+      BufferEntryType current_entry(time, data, sensor);
       data_out->at(k) = current_entry;
     }
   }

source/mars/include/mars/data_utils/read_position_data.h

@@ -43,9 +43,9 @@ class ReadPositionData
       Eigen::Vector3d position(csv_data["p_x"][k], csv_data["p_y"][k], csv_data["p_z"][k]);
 
       BufferDataType data;
-      data.set_sensor_data(std::make_shared<PositionMeasurementType>(position));
+      data.set_measurement(std::make_shared<PositionMeasurementType>(position));
 
-      BufferEntryType current_entry(time, data, sensor, BufferMetadataType::measurement);
+      BufferEntryType current_entry(time, data, sensor);
       data_out->at(k) = current_entry;
     }
   }

source/mars/include/mars/data_utils/read_sim_data.h

@@ -65,10 +65,11 @@ class ReadSimData
       core_ground_truth.b_a_ = bAcc;
 
       BufferDataType data;
-      data.set_core_data(std::make_shared<CoreStateType>(core_ground_truth));
-      data.set_sensor_data(std::make_shared<IMUMeasurementType>(a_imu, w_imu));
+      // TODO
+      // data.set_core_state(std::make_shared<CoreStateType>(core_ground_truth));
+      data.set_measurement(std::make_shared<IMUMeasurementType>(a_imu, w_imu));
 
-      BufferEntryType current_entry(time, data, sensor, BufferMetadataType::measurement);
+      BufferEntryType current_entry(time, data, sensor);
       data_out->at(k) = current_entry;
     }
   }

source/mars/include/mars/data_utils/read_velocity_data.h

@@ -43,9 +43,9 @@ class ReadVelocityData
       Eigen::Vector3d velocity(csv_data["v_x"][k], csv_data["v_y"][k], csv_data["v_z"][k]);
 
       BufferDataType data;
-      data.set_sensor_data(std::make_shared<VelocityMeasurementType>(velocity));
+      data.set_measurement(std::make_shared<VelocityMeasurementType>(velocity));
 
-      BufferEntryType current_entry(time, data, sensor, BufferMetadataType::measurement);
+      BufferEntryType current_entry(time, data, sensor);
       data_out->at(k) = current_entry;
     }
   }

source/mars/include/mars/data_utils/read_vision_data.h

@@ -46,9 +46,9 @@ class ReadVisionData
       orientation = Utils::NormalizeQuaternion(orientation, "vision csv reader");
 
       BufferDataType data;
-      data.set_sensor_data(std::make_shared<VisionMeasurementType>(position, orientation));
+      data.set_measurement(std::make_shared<VisionMeasurementType>(position, orientation));
 
-      BufferEntryType current_entry(time, data, sensor, BufferMetadataType::measurement);
+      BufferEntryType current_entry(time, data, sensor);
       data_out->at(k) = current_entry;
     }
   }

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/include/mars/sensors/gps/gps_utils.cpp

@@ -69,7 +69,7 @@ mars::GpsCoordinates mars::GPSInit::get_gps_mean(const std::shared_ptr<mars::Sen
   {
     if ((cur_time - (*it)->timestamp_).get_seconds() <= init_duration_)
     {
-      const GpsCoordinates meas = (*static_cast<GpsMeasurementType*>((*it)->data_.sensor_.get())).coordinates_;
+      const GpsCoordinates meas = (*static_cast<GpsMeasurementType*>((*it)->data_.measurement_.get())).coordinates_;
 
       avg_ref += meas;
       cnt_meas++;

source/mars/include/mars/sensors/pressure/pressure_utils.cpp

@@ -60,7 +60,7 @@ mars::Pressure mars::PressureInit::get_press_mean(const std::shared_ptr<mars::Se
   {
     if ((cur_time - (*it)->timestamp_).get_seconds() <= init_duration_)
     {
-      const PressureMeasurementType meas = *static_cast<PressureMeasurementType*>((*it)->data_.sensor_.get());
+      const PressureMeasurementType meas = *static_cast<PressureMeasurementType*>((*it)->data_.sensor_state_.get());
 
       avg_pressure += meas.pressure_;
       cnt_meas++;

source/mars/include/mars/sensors/update_sensor_abs_class.h

@@ -41,6 +41,6 @@ class UpdateSensorAbsClass : public SensorAbsClass
 
   std::shared_ptr<CoreState> core_states_;
 };
-};  // namespace mars
+}  // namespace mars
 
 #endif  // UPDATE_SENSOR_ABS_CLASS_H