diff --git a/.clang-format b/.clang-format index e893617..3573905 100644 --- a/.clang-format +++ b/.clang-format @@ -8,7 +8,6 @@ AllowAllParametersOfDeclarationOnNextLine: false AllowShortIfStatementsOnASingleLine: false AllowShortLoopsOnASingleLine: false AllowShortFunctionsOnASingleLine: None -AllowShortLoopsOnASingleLine: false AlwaysBreakTemplateDeclarations: true AlwaysBreakBeforeMultilineStrings: false BreakBeforeBinaryOperators: false diff --git a/README.md b/README.md index ad9a865..94d5b24 100644 --- a/README.md +++ b/README.md @@ -407,7 +407,7 @@ Leading to the following derivatives ```math \begin{align*} - \frac{\mathrm{d}h(x)}{\mathrm{d}\delta_{rm}} &= -\mathbf{R}_{WI}[\alpha]\left\|\mu\right\|-\mathbf{R}_{WI}\alpha\frac{\mu^\mathsf{T}}{\left\|\mu\right\|}\mathbf{R}_{WI}\left[\left[\omega_i\right]_{\times} \mathbf{P}_{IG}\right]_{\times} \\ + \frac{\mathrm{d}h(x)}{\mathrm{d}\delta_{rm}} &= -\mathbf{R}_{WI}\left[\alpha\right]_{\times} \left\|\mu\right\|-\mathbf{R}_{WI}\alpha\frac{\mu^\mathsf{T}}{\left\|\mu\right\|}\mathbf{R}_{WI}\left[\left[\omega_i\right]_{\times} \mathbf{P}_{IG}\right]_{\times} \\ \frac{\mathrm{d}h(x)}{\mathrm{d}\Delta\mathbf{V}}_{WI} &= \mathbf{R}_{WI}\alpha\frac{\mu^\mathsf{T}}{\left\|\mu\right\|} \\ \frac{\mathrm{d}h(x)}{\mathrm{d}\Delta\mathbf{P}}_{IG} &= \mathbf{R}_{WI}\alpha\frac{\mu^\mathsf{T}}{\left\|\mu\right\|}\mathbf{R}_{WI}\left[\omega_i\right]_{\times} \end{align*} diff --git a/source/mars/CMakeLists.txt b/source/mars/CMakeLists.txt index 3111973..d8b1ba2 100644 --- a/source/mars/CMakeLists.txt +++ b/source/mars/CMakeLists.txt @@ -94,6 +94,9 @@ set(headers ${include_path}/sensors/empty/empty_measurement_type.h ${include_path}/sensors/empty/empty_sensor_class.h ${include_path}/sensors/empty/empty_sensor_state_type.h + ${include_path}/sensors/velocity/velocity_measurement_type.h + ${include_path}/sensors/velocity/velocity_sensor_class.h + ${include_path}/sensors/velocity/velocity_sensor_state_type.h ${include_path}/data_utils/read_csv.h ${include_path}/data_utils/write_csv.h ${include_path}/data_utils/read_sim_data.h @@ -105,6 +108,7 @@ set(headers ${include_path}/data_utils/read_gps_data.h ${include_path}/data_utils/read_mag_data.h ${include_path}/data_utils/read_baro_data.h + ${include_path}/data_utils/read_velocity_data.h ${include_path}/data_utils/filesystem.h ) diff --git a/source/mars/include/mars/buffer.h b/source/mars/include/mars/buffer.h index 9fe0adb..c0fa3a6 100644 --- a/source/mars/include/mars/buffer.h +++ b/source/mars/include/mars/buffer.h @@ -186,6 +186,13 @@ class Buffer /// bool get_entry_at_idx(const int& index, BufferEntryType* entry) const; + /// + /// \brief RemoveSensorFromBuffer Removes all entrys that are associated with the given sensor handle + /// \param sensor_handle Sensor handle to be removed + /// \return true if the operation was performed, false otherwise + /// + bool RemoveSensorFromBuffer(const std::shared_ptr& sensor_handle); + /// /// \brief AddEntrySorted Adds a new entry to the buffer and ensures the buffer is sorted /// \param new_entry new buffer entry to be added diff --git a/source/mars/include/mars/data_utils/read_velocity_data.h b/source/mars/include/mars/data_utils/read_velocity_data.h new file mode 100644 index 0000000..97c218e --- /dev/null +++ b/source/mars/include/mars/data_utils/read_velocity_data.h @@ -0,0 +1,55 @@ +// Copyright (C) 2024 Christian Brommer, Control of Networked Systems, University of Klagenfurt, Austria. +// +// All rights reserved. +// +// This software is licensed under the terms of the BSD-2-Clause-License with +// no commercial use allowed, the full terms of which are made available +// in the LICENSE file. No license in patents is granted. +// +// You can contact the author at + +#ifndef READ_VELOCITY_DATA_H +#define READ_VELOCITY_DATA_H + +#include +#include +#include +#include +#include +#include +#include + +namespace mars +{ +class ReadVelocityData +{ +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + ReadVelocityData(std::vector* data_out, std::shared_ptr sensor, + const std::string& file_path, const double& time_offset = 0) + { + std::vector expect_entry = { "t", "v_x", "v_y", "v_z" }; + + CsvDataType csv_data; + ReadCsv(&csv_data, file_path); + + unsigned long number_of_datapoints = csv_data["t"].size(); + data_out->resize(number_of_datapoints); + + for (size_t k = 0; k < number_of_datapoints; k++) + { + Time time = csv_data["t"][k] + time_offset; + 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(velocity)); + + BufferEntryType current_entry(time, data, sensor, BufferMetadataType::measurement); + data_out->at(k) = current_entry; + } + } +}; +} // namespace mars + +#endif // READ_VELOCITY_DATA_H diff --git a/source/mars/include/mars/sensor_manager.h b/source/mars/include/mars/sensor_manager.h index 063e4f1..b457a3e 100644 --- a/source/mars/include/mars/sensor_manager.h +++ b/source/mars/include/mars/sensor_manager.h @@ -1,4 +1,5 @@ -// Copyright (C) 2022 Christian Brommer, Control of Networked Systems, University of Klagenfurt, Austria. +// Copyright (C) 2024 Christian Brommer and Thomas Jantos, Control of Networked Systems, University of Klagenfurt, +// Austria. // // All rights reserved. // @@ -11,12 +12,133 @@ #ifndef SENSORMANAGER_HPP #define SENSORMANAGER_HPP +#include +#include +#include + namespace mars { class SensorManager { public: + std::vector> sensor_list_; ///< Vector containing all registered sensors SensorManager() = default; + + /// + /// \brief register_sensor Register a sensor with the sensor manager + /// \param sensor Sensor to be registered + /// \return True if the sensor was registered, false if the sensor is already registered + /// + bool register_sensor(std::shared_ptr sensor) + { + // Check if sensor already exists + if (std::find(sensor_list_.begin(), sensor_list_.end(), sensor) != sensor_list_.end()) + { + // Sensor is already registered + return false; + } + + sensor_list_.push_back(sensor); + std::cout << "Registered sensor [" << sensor->name_ << "] with Sensor Manager" << std::endl; + return true; + } + + /// + /// \brief remove_sensor Remove a sensor from the sensor manager + /// \param buffer Buffer to remove the sensor from + /// \param sensor Sensor to be removed + /// \return True if the sensor was removed, false if the sensor is not registered + /// + bool remove_sensor(Buffer& buffer, std::shared_ptr sensor) + { + if (!does_sensor_exist(sensor)) + { + // Sensor is not registered + return false; + } + // Deactive the sensor + this->deactivate_sensor(buffer, sensor); + // Remove the sensor from the list + sensor_list_.erase(std::remove(sensor_list_.begin(), sensor_list_.end(), sensor), sensor_list_.end()); + std::cout << "Removed sensor [" << sensor->name_ << "] from Sensor Manager" << std::endl; + return true; + } + + /// + /// \brief list_sensors Print the information of all registered sensors + /// + void list_sensors() + { + std::cout << "Sensor Manager contains " << sensor_list_.size() << " sensors" << std::endl; + for (auto& sensor : sensor_list_) + { + std::cout << *sensor << std::endl; + } + } + + /// + /// \brief does_sensor_exist Check if a sensor is registered + /// \param sensor Sensor to be checked + /// \return True if the sensor is registered, otherwise false + /// + bool does_sensor_exist(std::shared_ptr sensor) + { + if (std::find(sensor_list_.begin(), sensor_list_.end(), sensor) != sensor_list_.end()) + { + return true; + } + return false; + } + + /// + /// \brief deactivate_sensor Deactivate a sensor + /// \param buffer Buffer to remove the sensor from + /// \param sensor Sensor to be deactivated + /// \return False if the sensor is not registered, otherwise true + /// + bool deactivate_sensor(Buffer& buffer, std::shared_ptr sensor) + { + if (!does_sensor_exist(sensor)) + { + // Sensor is not registered + return false; + } + + // Reset the sensor + sensor->do_update_ = false; + sensor->is_initialized_ = false; + sensor->ref_to_nav_given_ = false; + + // Call buffer to clear all entries of the sensor + if (buffer.RemoveSensorFromBuffer(sensor)) + { + std::cout << "Removed sensor [" << sensor->name_ << "] from buffer" << std::endl; + } + else + { + std::cout << "Could not remove sensor [" << sensor->name_ << "] from buffer as buffer is empty" << std::endl; + return false; + } + return true; + } + + /// + /// \brief activate_sensor Activate a sensor + /// \param sensor Sensor to be activated + /// \return False if the sensor is not registered, otherwise true + /// + bool activate_sensor(std::shared_ptr sensor) + { + if (!does_sensor_exist(sensor)) + { + // Sensor is not registered + return false; + } + + sensor->do_update_ = true; + std::cout << "Activated sensor [" << sensor->name_ << "]" << std::endl; + return true; + } }; } // namespace mars diff --git a/source/mars/include/mars/sensors/sensor_abs_class.h b/source/mars/include/mars/sensors/sensor_abs_class.h index ee1fd5c..e7e5d8f 100644 --- a/source/mars/include/mars/sensors/sensor_abs_class.h +++ b/source/mars/include/mars/sensors/sensor_abs_class.h @@ -22,7 +22,24 @@ class SensorAbsClass : public SensorInterface int id_{ -1 }; std::string name_; ///< Name of the individual sensor instance bool is_initialized_{ false }; ///< True if the sensor has been initialized + bool do_update_{ true }; ///< True if updates should be performed with the sensor int type_{ -1 }; ///< Future feature, holds information such as position or orientation for highlevel decissions + bool const_ref_to_nav_{ true }; ///< True if the reference should not be estimated + bool ref_to_nav_given_{ false }; ///< True if the reference to the navigation frame is given by initial calibration + bool use_dynamic_meas_noise_{ false }; ///< True if dynamic noise values from measurements should be used + + /// + /// \brief operator << Overload the << operator for easy printing of the sensor information + /// + friend std::ostream& operator<<(std::ostream& out, const SensorAbsClass& sensor) + { + out << "\tSensor: " << sensor.name_ << std::endl; + out << "\tInitialized: " << sensor.is_initialized_ << std::endl; + out << "\tDo update: " << sensor.do_update_ << std::endl; + out << "\tReference to nav: " << sensor.const_ref_to_nav_ << std::endl; + out << "\tUse dynamic noise: " << sensor.use_dynamic_meas_noise_ << std::endl; + return out; + } }; } // namespace mars #endif // SENSORABSCLASS_H diff --git a/source/mars/include/mars/sensors/update_sensor_abs_class.h b/source/mars/include/mars/sensors/update_sensor_abs_class.h index faba42e..215bef4 100644 --- a/source/mars/include/mars/sensors/update_sensor_abs_class.h +++ b/source/mars/include/mars/sensors/update_sensor_abs_class.h @@ -36,13 +36,11 @@ class UpdateSensorAbsClass : public SensorAbsClass std::shared_ptr initial_calib_{ nullptr }; bool initial_calib_provided_{ false }; ///< True if an initial calibration was provided - bool const_ref_to_nav_{ true }; ///< True if the reference should not be estimated - bool use_dynamic_meas_noise_{ false }; ///< True if dynamic noise values from measurements should be used Chi2 chi2_; std::shared_ptr core_states_; }; -} // namespace mars +}; // namespace mars #endif // UPDATE_SENSOR_ABS_CLASS_H diff --git a/source/mars/include/mars/sensors/velocity/velocity_measurement_type.h b/source/mars/include/mars/sensors/velocity/velocity_measurement_type.h new file mode 100644 index 0000000..0b2f69a --- /dev/null +++ b/source/mars/include/mars/sensors/velocity/velocity_measurement_type.h @@ -0,0 +1,55 @@ +// Copyright (C) 2024 Christian Brommer, Control of Networked Systems, University of Klagenfurt, +// Austria. +// +// All rights reserved. +// +// This software is licensed under the terms of the BSD-2-Clause-License with +// no commercial use allowed, the full terms of which are made available +// in the LICENSE file. No license in patents is granted. +// +// You can contact the author at + +#ifndef VELOCITYMEASUREMENTTYPE_H +#define VELOCITYMEASUREMENTTYPE_H + +#include +#include +#include + +namespace mars +{ +class VelocityMeasurementType : public BaseMeas +{ +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + Eigen::Vector3d velocity_; ///< Velocity [x y z] + + VelocityMeasurementType() = default; + + VelocityMeasurementType(Eigen::Vector3d velocity) : velocity_(std::move(velocity)) + { + } + + static std::string get_csv_state_header_string() + { + std::stringstream os; + os << "t, "; + os << "v_x, v_y, v_z"; + + return os.str(); + } + + std::string to_csv_string(const double& timestamp) const + { + std::stringstream os; + os.precision(17); + os << timestamp; + + os << ", " << velocity_.x() << ", " << velocity_.y() << ", " << velocity_.z(); + + return os.str(); + } +}; +} // namespace mars +#endif // VELOCITYMEASUREMENTTYPE_H diff --git a/source/mars/include/mars/sensors/velocity/velocity_sensor_class.h b/source/mars/include/mars/sensors/velocity/velocity_sensor_class.h new file mode 100644 index 0000000..21254d0 --- /dev/null +++ b/source/mars/include/mars/sensors/velocity/velocity_sensor_class.h @@ -0,0 +1,252 @@ +// Copyright (C) 2024 Christian Brommer, Control of Networked Systems, University of Klagenfurt, +// Austria. +// +// All rights reserved. +// +// This software is licensed under the terms of the BSD-2-Clause-License with +// no commercial use allowed, the full terms of which are made available +// in the LICENSE file. No license in patents is granted. +// +// You can contact the author at + +#ifndef VELOCITYSENSORCLASS_H +#define VELOCITYSENSORCLASS_H + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace mars +{ +using VelocitySensorData = BindSensorData; + +class VelocitySensorClass : public UpdateSensorAbsClass +{ +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + VelocitySensorClass(const std::string& name, std::shared_ptr core_states) + { + name_ = name; + core_states_ = std::move(core_states); + const_ref_to_nav_ = false; + initial_calib_provided_ = false; + + // chi2 + chi2_.set_dof(3); + + std::cout << "Created: [" << this->name_ << "] Sensor" << std::endl; + } + + virtual ~VelocitySensorClass() = default; + + VelocitySensorStateType get_state(const std::shared_ptr& sensor_data) + { + VelocitySensorData data = *static_cast(sensor_data.get()); + return data.state_; + } + + Eigen::MatrixXd get_covariance(const std::shared_ptr& sensor_data) + { + VelocitySensorData data = *static_cast(sensor_data.get()); + return data.get_full_cov(); + } + + void set_initial_calib(std::shared_ptr calibration) + { + initial_calib_ = calibration; + initial_calib_provided_ = true; + } + + BufferDataType Initialize(const Time& timestamp, std::shared_ptr /*sensor_data*/, + std::shared_ptr latest_core_data) + { + // VelocityMeasurementType measurement = *static_cast(sensor_data.get()); + + VelocitySensorData sensor_state; + std::string calibration_type; + + if (this->initial_calib_provided_) + { + calibration_type = "Given"; + + VelocitySensorData calib = *static_cast(initial_calib_.get()); + + sensor_state.state_ = calib.state_; + sensor_state.sensor_cov_ = calib.sensor_cov_; + } + else + { + calibration_type = "Auto"; + + std::cout << "Velocity calibration AUTO init not implemented yet" << std::endl; + exit(EXIT_FAILURE); + } + + // Bypass core state for the returned object + BufferDataType result(std::make_shared(*latest_core_data.get()), + std::make_shared(sensor_state)); + + is_initialized_ = true; + + std::cout << "Info: Initialized [" << name_ << "] with [" << calibration_type << "] Calibration at t=" << timestamp + << std::endl; + std::cout << "\tPosition[m]: [" << sensor_state.state_.p_iv_.transpose() << " ]" << std::endl; + + if (!initial_calib_provided_) + { + std::cout << "Info: [" << name_ << "] Calibration(rounded):" << std::endl; + std::cout << "\tPosition[m]: [" << sensor_state.state_.p_iv_.transpose() << " ]" << std::endl; + } + + return result; + } + + bool CalcUpdate(const Time& /*timestamp*/, std::shared_ptr measurement, const CoreStateType& prior_core_state, + std::shared_ptr latest_sensor_data, const Eigen::MatrixXd& prior_cov, + BufferDataType* new_state_data) + { + // Cast the sensor measurement and prior state information + VelocityMeasurementType* meas = static_cast(measurement.get()); + VelocitySensorData* prior_sensor_data = static_cast(latest_sensor_data.get()); + + // Decompose sensor measurement + Eigen::Vector3d v_meas = meas->velocity_; + + // Extract sensor state + VelocitySensorStateType prior_sensor_state(prior_sensor_data->state_); + + // Generate measurement noise matrix and check + // if noisevalues from the measurement object should be used + Eigen::MatrixXd R_meas_dyn; + if (meas->has_meas_noise && use_dynamic_meas_noise_) + { + meas->get_meas_noise(&R_meas_dyn); + } + else + { + R_meas_dyn = this->R_.asDiagonal(); + } + + const Eigen::Matrix R_meas = R_meas_dyn; + + const int size_of_core_state = CoreStateType::size_error_; + const int size_of_sensor_state = prior_sensor_state.cov_size_; + const int size_of_full_error_state = size_of_core_state + size_of_sensor_state; + const Eigen::MatrixXd P = prior_cov; + assert(P.size() == size_of_full_error_state * size_of_full_error_state); + + // Calculate the measurement jacobian H + const Eigen::Matrix3d I_3 = Eigen::Matrix3d::Identity(); + const Eigen::Matrix3d O_3 = Eigen::Matrix3d::Zero(); + + const Eigen::Vector3d omega_i = prior_core_state.w_m_; ///< Angular Velocity of the IMU Frame + + // const Eigen::Vector3d P_wi = prior_core_state.p_wi_; + const Eigen::Vector3d V_wi = prior_core_state.v_wi_; + const Eigen::Vector3d b_w = prior_core_state.b_w_; + const Eigen::Matrix3d R_wi = prior_core_state.q_wi_.toRotationMatrix(); + const Eigen::Vector3d P_iv = prior_sensor_state.p_iv_; + + const Eigen::Matrix3d Hv_pwi = O_3; + const Eigen::Matrix3d Hv_vwi = I_3; + const Eigen::Matrix3d Hv_rwi = -R_wi * Utils::Skew(Utils::Skew(omega_i - b_w) * P_iv); + const Eigen::Matrix3d Hv_bw = O_3; + const Eigen::Matrix3d Hv_ba = O_3; + + const Eigen::Matrix3d Hv_piv = R_wi * Utils::Skew(omega_i - b_w); + + // Assemble the jacobian for the velocity (horizontal) + const int num_states = + static_cast(Hv_pwi.cols() + Hv_vwi.cols() + Hv_rwi.cols() + Hv_bw.cols() + Hv_ba.cols() + Hv_piv.cols()); + + // Combine all jacobians (vertical) + Eigen::MatrixXd H(3, num_states); + H << Hv_pwi, Hv_vwi, Hv_rwi, Hv_bw, Hv_ba, Hv_piv; + + Eigen::Vector3d v_est; + v_est = V_wi + R_wi * Utils::Skew(omega_i - b_w) * P_iv; + + // Calculate the residual z = z~ - (estimate) + // Velocity + const Eigen::Vector3d res_v = v_meas - v_est; + + // Combine residuals (vertical) + residual_ = Eigen::MatrixXd(res_v.rows(), 1); + residual_ << res_v; + + // Perform EKF calculations + mars::Ekf ekf(H, R_meas, residual_, P); + const Eigen::MatrixXd correction = ekf.CalculateCorrection(&chi2_); + assert(correction.size() == size_of_full_error_state * 1); + + // Perform Chi2 test + if (!chi2_.passed_ && chi2_.do_test_) + { + chi2_.PrintReport(name_); + return false; + } + + Eigen::MatrixXd P_updated = ekf.CalculateCovUpdate(); + assert(P_updated.size() == size_of_full_error_state * size_of_full_error_state); + P_updated = Utils::EnforceMatrixSymmetry(P_updated); + + // Apply Core Correction + CoreStateVector core_correction = correction.block(0, 0, CoreStateType::size_error_, 1); + CoreStateType corrected_core_state = CoreStateType::ApplyCorrection(prior_core_state, core_correction); + + // Apply Sensor Correction + const Eigen::MatrixXd sensor_correction = correction.block(size_of_core_state, 0, size_of_sensor_state, 1); + const VelocitySensorStateType corrected_sensor_state = ApplyCorrection(prior_sensor_state, sensor_correction); + + // Return Results + // CoreState data + CoreType core_data; + core_data.cov_ = P_updated.block(0, 0, CoreStateType::size_error_, CoreStateType::size_error_); + core_data.state_ = corrected_core_state; + + // SensorState data + std::shared_ptr sensor_data(std::make_shared()); + sensor_data->set_cov(P_updated); + sensor_data->state_ = corrected_sensor_state; + + BufferDataType state_entry(std::make_shared(core_data), sensor_data); + + if (const_ref_to_nav_) + { + // corrected_sensor_data.ref_to_nav = prior_ref_to_nav; + } + else + { + // TODO also estimate ref to nav + } + + *new_state_data = state_entry; + + return true; + } + + VelocitySensorStateType ApplyCorrection(const VelocitySensorStateType& prior_sensor_state, + const Eigen::MatrixXd& correction) + { + // state + error state correction + // with quaternion from small angle approx -> new state + + VelocitySensorStateType corrected_sensor_state; + corrected_sensor_state.p_iv_ = prior_sensor_state.p_iv_ + correction.block(0, 0, 3, 1); + return corrected_sensor_state; + } +}; // namespace mars +} // namespace mars + +#endif // VELOCITYSENSORCLASS_H diff --git a/source/mars/include/mars/sensors/velocity/velocity_sensor_state_type.h b/source/mars/include/mars/sensors/velocity/velocity_sensor_state_type.h new file mode 100644 index 0000000..dc668ff --- /dev/null +++ b/source/mars/include/mars/sensors/velocity/velocity_sensor_state_type.h @@ -0,0 +1,52 @@ +// Copyright (C) 2024 Christian Brommer, Control of Networked Systems, University of Klagenfurt, +// Austria. +// +// All rights reserved. +// +// This software is licensed under the terms of the BSD-2-Clause-License with +// no commercial use allowed, the full terms of which are made available +// in the LICENSE file. No license in patents is granted. +// +// You can contact the author at + +#ifndef VELOCITYSENSORSTATETYPE_H +#define VELOCITYSENSORSTATETYPE_H + +#include +#include + +namespace mars +{ +class VelocitySensorStateType : public BaseStates +{ +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + Eigen::Vector3d p_iv_; + + VelocitySensorStateType() : BaseStates(3) // size of covariance + { + p_iv_.setZero(); + } + + static std::string get_csv_state_header_string() + { + std::stringstream os; + os << "t, "; + os << "p_iv_x, p_iv_y, p_iv_z"; + + return os.str(); + } + + std::string to_csv_string(const double& timestamp) const + { + std::stringstream os; + os.precision(17); + os << timestamp; + + os << ", " << p_iv_(0) << ", " << p_iv_(1) << ", " << p_iv_(2); + return os.str(); + } +}; +} // namespace mars +#endif // VELOCITYSENSORSTATETYPE_H diff --git a/source/mars/include/mars/sensors/vision/vision_sensor_class.h b/source/mars/include/mars/sensors/vision/vision_sensor_class.h index de588ae..4a840bb 100644 --- a/source/mars/include/mars/sensors/vision/vision_sensor_class.h +++ b/source/mars/include/mars/sensors/vision/vision_sensor_class.h @@ -72,10 +72,10 @@ class VisionSensorClass : public UpdateSensorAbsClass initial_calib_provided_ = true; } - BufferDataType Initialize(const Time& timestamp, std::shared_ptr /*sensor_data*/, + BufferDataType Initialize(const Time& timestamp, std::shared_ptr sensor_data, std::shared_ptr latest_core_data) { - // VisionMeasurementType measurement = *static_cast(sensor_data.get()); + VisionMeasurementType measurement = *static_cast(sensor_data.get()); VisionSensorData sensor_state; std::string calibration_type; @@ -88,6 +88,44 @@ class VisionSensorClass : public UpdateSensorAbsClass sensor_state.state_ = calib.state_; sensor_state.sensor_cov_ = calib.sensor_cov_; + + // Overwrite the calibration between the reference world and navigation world in given sensor_state + if (!this->ref_to_nav_given_) + { + // The calibration between reference world and navigation world is not given. + // Calculate it given the current estimate and measurement + + // Orientation Vision World R_vw + + Eigen::Quaterniond q_wi(latest_core_data->state_.q_wi_); + Eigen::Quaterniond q_ic(calib.state_.q_ic_); + Eigen::Quaterniond q_vc(measurement.orientation_); + Eigen::Quaterniond q_vw = (q_wi * q_ic * q_vc.inverse()).inverse(); + + Eigen::Matrix3d R_wi(q_wi.toRotationMatrix()); + Eigen::Matrix3d R_ic(q_ic.toRotationMatrix()); + + Eigen::Matrix3d R_vw(q_vw.toRotationMatrix()); + + Eigen::Vector3d p_wi(latest_core_data->state_.p_wi_); + Eigen::Vector3d p_ic(calib.state_.p_ic_); + Eigen::Vector3d p_vc(measurement.position_); + + Eigen::Vector3d p_vw = -(R_vw * (p_wi + (R_wi * p_ic)) - p_vc); + + sensor_state.state_.q_vw_ = q_vw; + sensor_state.state_.p_vw_ = p_vw; + } + std::cout << "Info: [" << name_ << "] Reference Frame initialized to:" << std::endl; + std::cout << "\tP_vw[m]: [" << sensor_state.state_.p_vw_.transpose() << " ]" << std::endl; + + Eigen::Vector4d q_vw_out(sensor_state.state_.q_vw_.w(), sensor_state.state_.q_vw_.x(), + sensor_state.state_.q_vw_.y(), sensor_state.state_.q_vw_.z()); + + std::cout << "\tq_vw: [" << q_vw_out.transpose() << " ]" << std::endl; + std::cout << "\tR_vw[deg]: [" + << sensor_state.state_.q_vw_.toRotationMatrix().eulerAngles(0, 1, 2).transpose() * (180 / M_PI) << " ]" + << std::endl; } else { @@ -148,6 +186,12 @@ class VisionSensorClass : public UpdateSensorAbsClass std::shared_ptr latest_sensor_data, const Eigen::MatrixXd& prior_cov, BufferDataType* new_state_data) { + // Check if updates should be performed with the sensor + if (!do_update_) + { + return false; + } + // Cast the sensor measurement and prior state information VisionMeasurementType* meas = static_cast(measurement.get()); VisionSensorData* prior_sensor_data = static_cast(latest_sensor_data.get()); diff --git a/source/mars/include/mars/time.h b/source/mars/include/mars/time.h index 0a4e347..1212480 100644 --- a/source/mars/include/mars/time.h +++ b/source/mars/include/mars/time.h @@ -11,6 +11,7 @@ #ifndef TIME_H #define TIME_H +#include #include namespace mars @@ -21,8 +22,9 @@ class Time Time() = default; Time(const double& seconds); - double get_seconds() const; + static Time get_time_now(); + double get_seconds() const; Time abs() const; Time operator+(const Time& rhs) const; diff --git a/source/mars/include/mars/type_definitions/buffer_entry_type.h b/source/mars/include/mars/type_definitions/buffer_entry_type.h index e41fa74..4b64e88 100644 --- a/source/mars/include/mars/type_definitions/buffer_entry_type.h +++ b/source/mars/include/mars/type_definitions/buffer_entry_type.h @@ -27,12 +27,12 @@ enum BufferMetadataType { invalid, core_state, - core_state_auto, ///< auto generated core_state + core_state_auto, ///< auto generated core_state e.g. introduced by interpolation sensor_state, init_state, measurement, ///< regular measurement measurement_ooo, ///< out of order measurement - measurement_auto ///< auto generated measurement + measurement_auto ///< auto generated measurement e.g. introduced by interpolation }; } // namespace BufferMetadataTypes diff --git a/source/mars/source/buffer.cpp b/source/mars/source/buffer.cpp index 6882b6a..eaf9a12 100644 --- a/source/mars/source/buffer.cpp +++ b/source/mars/source/buffer.cpp @@ -299,7 +299,7 @@ bool Buffer::get_closest_state(const Time& timestamp, BufferEntryType* entry, in } else { - continue; + break; } previous_state_index = k; } @@ -339,6 +339,24 @@ bool Buffer::get_entry_at_idx(const int& index, BufferEntryType* entry) const return false; } +bool Buffer::RemoveSensorFromBuffer(const std::shared_ptr& sensor_handle) +{ + if (this->IsEmpty()) + { + return false; + } + + for (int k = 0; k < this->get_length(); k++) + { + if (data_[k].sensor_ == sensor_handle) + { + *data_.erase(data_.begin() + k); + } + } + + return true; +} + int Buffer::AddEntrySorted(const BufferEntryType& new_entry) { int index = InsertDataAtTimestamp(new_entry); @@ -352,7 +370,7 @@ int Buffer::AddEntrySorted(const BufferEntryType& new_entry) } else { - index -= del_idx < index ? 1 : 0; + index -= del_idx < index ? 2 : 0; } } diff --git a/source/mars/source/core_logic.cpp b/source/mars/source/core_logic.cpp index 5cc1145..ba121a4 100644 --- a/source/mars/source/core_logic.cpp +++ b/source/mars/source/core_logic.cpp @@ -382,7 +382,18 @@ bool CoreLogic::ProcessMeasurement(std::shared_ptr sensor, const { if (verbose_) { - std::cout << "[CoreLogic]: Process Measurement (" << sensor->name_ << ")" << std::endl; + std::cout << "[CoreLogic]: Process Measurement (" << sensor->name_ << ")"; + if (!sensor->do_update_) + { + std::cout << ". Sensor is deactivated."; + } + std::cout << std::endl; + } + + if (!sensor->do_update_) + { + // Do not perform update for this sensor + return false; } // Generate buffer entry element for the measurement @@ -475,8 +486,9 @@ bool CoreLogic::ProcessMeasurement(std::shared_ptr sensor, const // Use measurement and perform out of order updates if (verbose_ || verbose_out_of_order_) { - std::cout << "Warning: " << sensor.get()->name_ << " Measurement is out of order. " - << "dt = " << latest_buffer_entry.timestamp_ - timestamp << " " + std::cout << "Warning: " << sensor.get()->name_ << " Measurement is out of order. \n" + << "dt to latest buffer = " << latest_buffer_entry.timestamp_ - timestamp << "\n" + << "dt to time now = " << mars::Time::get_time_now() - timestamp << "\n" << "Latest stamp: " << latest_buffer_entry.timestamp_ << " Measurement stamp: " << timestamp << std::endl; } diff --git a/source/mars/source/time.cpp b/source/mars/source/time.cpp index 342f8a5..2da0adf 100644 --- a/source/mars/source/time.cpp +++ b/source/mars/source/time.cpp @@ -9,8 +9,10 @@ // You can contact the author at #include +#include #include #include +#include namespace mars { @@ -18,6 +20,23 @@ Time::Time(const double& seconds) : seconds_(seconds) { } +//// +/// \brief get_time_now +/// \return seconds since epoch for the current time +/// +Time Time::get_time_now() +{ + using namespace std::chrono; + auto sys_time_now = system_clock::now(); + double sys_sec = double(duration_cast(sys_time_now.time_since_epoch()).count())/1e3; + + // Cast to millisecond precision only + // auto sys_ms = (duration_cast(sys_time_now.time_since_epoch()) - + // duration_cast(sys_time_now.time_since_epoch())); + + return Time(sys_sec); +} + double Time::get_seconds() const { return seconds_; diff --git a/source/tests/mars-test/CMakeLists.txt b/source/tests/mars-test/CMakeLists.txt index 73ea3ae..cecb855 100644 --- a/source/tests/mars-test/CMakeLists.txt +++ b/source/tests/mars-test/CMakeLists.txt @@ -36,6 +36,7 @@ set(sources mars_pose_sensor.cpp mars_vision_sensor.cpp mars_position_sensor.cpp + mars_velocity_sensor.cpp mars_bodyvel_sensor.cpp mars_attitude_sensor.cpp mars_pressure_sensor.cpp diff --git a/source/tests/mars-test/mars_core_state.cpp b/source/tests/mars-test/mars_core_state.cpp index e457400..1fd9938 100644 --- a/source/tests/mars-test/mars_core_state.cpp +++ b/source/tests/mars-test/mars_core_state.cpp @@ -22,7 +22,7 @@ class mars_core_state_test : public testing::Test TEST_F(mars_core_state_test, CTOR_CORE_TYPE) { - mars::CoreType core_type(); + mars::CoreType core_type; } TEST_F(mars_core_state_test, CTOR_CORE_STATE_TYPE) diff --git a/source/tests/mars-test/mars_velocity_sensor.cpp b/source/tests/mars-test/mars_velocity_sensor.cpp new file mode 100644 index 0000000..d249e18 --- /dev/null +++ b/source/tests/mars-test/mars_velocity_sensor.cpp @@ -0,0 +1,79 @@ +// Copyright (C) 2024 Christian Brommer, Control of Networked Systems, University of Klagenfurt, Austria. +// +// All rights reserved. +// +// This software is licensed under the terms of the BSD-2-Clause-License with +// no commercial use allowed, the full terms of which are made available +// in the LICENSE file. No license in patents is granted. +// +// You can contact the author at + +#include +#include +#include +#include +#include +#include +#include + +class mars_velocity_sensor_test : public testing::Test +{ +public: +}; + +TEST_F(mars_velocity_sensor_test, CTOR_VELOCITY_SENSOR) +{ + mars::CoreState core_states; + std::shared_ptr core_states_sptr = std::make_shared(core_states); + mars::VelocitySensorClass velocity_sensor("Velocity", core_states_sptr); +} + +TEST_F(mars_velocity_sensor_test, Velocity_SENSOR_MEASUREMENT) +{ + Eigen::Vector3d velocity; // Velocity [x y z] + + velocity << 1, 2, 3; + mars::VelocityMeasurementType b(velocity); + + Eigen::IOFormat OctaveFmt(Eigen::StreamPrecision, 0, ", ", ";\n", "", "", "[", "]"); + std::cout << b.velocity_.format(OctaveFmt) << std::endl; +} + +TEST_F(mars_velocity_sensor_test, VELOCITY_SENSOR_INIT) +{ + Eigen::Vector3d velocity; // Velocity [x y z] + + velocity << 1, 2, 3; + mars::VelocityMeasurementType measurement(velocity); + + mars::CoreState core_states; + std::shared_ptr core_states_sptr = std::make_shared(core_states); + mars::VelocitySensorClass velocity_sensor("Velocity", core_states_sptr); + + EXPECT_DEATH(velocity_sensor.Initialize(1, std::make_shared(measurement), + std::make_shared()), + ""); +} + +TEST_F(mars_velocity_sensor_test, VELOCITY_UPDATE) +{ + Eigen::Vector3d velocity; // Velocity [x y z] + + velocity << 1, 2, 3; + mars::VelocityMeasurementType measurement(velocity); + + mars::CoreState core_states; + std::shared_ptr core_states_sptr = std::make_shared(core_states); + mars::VelocitySensorClass position_sensor("Velocity", core_states_sptr); + + mars::CoreStateType prior_core_state; + mars::BufferDataType prior_sensor_buffer_data; + Eigen::Matrix prior_cov; + prior_cov.setIdentity(); + + // TODO no update without init + // int timestamp = 1; + // mars::BufferDataType test = + // position_sensor.CalcUpdate(timestamp, std::make_shared(measurement), + // prior_core_state, prior_sensor_buffer_data.sensor_, prior_cov); +}