Merge branch 'devel'

.gitignore

@@ -54,3 +54,5 @@ Thumbs.db
 dbg*/
 build*/
 xcodeenvvar.txt
+.vs/
+CMakeSettings.json

CMakeLists.txt

@@ -69,7 +69,7 @@ install_basic_package_files(${PROJECT_NAME}
                             NO_SET_AND_CHECK_MACRO
                             VARS_PREFIX ${PROJECT_NAME}
                             NO_CHECK_REQUIRED_COMPONENTS_MACRO
-                            DEPENDENCIES OpenCV)
+                            DEPENDENCIES OpenCV Threads)
 
 # Add the uninstall target
 include(AddUninstallTarget)

src/BayesFilters/CMakeLists.txt

@@ -99,7 +99,7 @@ set_target_properties(${LIBRARY_TARGET_NAME} PROPERTIES VERSION       ${${PROJEC
                                                         PUBLIC_HEADER "${${LIBRARY_TARGET_NAME}_HDR}")
 
 target_include_directories(${LIBRARY_TARGET_NAME} PUBLIC "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
-                                                         "$<INSTALL_INTERFACE:${CMAKE_INSTALL_PREFIX}/${CMAKE_INSTALL_INCLUDEDIR}>")
+                                                         "$<INSTALL_INTERFACE:$<INSTALL_PREFIX>/${CMAKE_INSTALL_INCLUDEDIR}>")
 if(NOT TARGET Eigen3)
     target_include_directories(${LIBRARY_TARGET_NAME} PUBLIC ${EIGEN3_INCLUDE_DIR})
 

src/BayesFilters/include/BayesFilters/EstimatesExtraction.h

@@ -50,18 +50,25 @@ class bfl::EstimatesExtraction
     std::vector<std::string> getInfo() const;
 
 protected:
+    ExtractionMethod extraction_method_ = ExtractionMethod::emode;
+
+    HistoryBuffer hist_buffer_;
+
+    Eigen::VectorXf sm_weights_;
+    Eigen::VectorXf wm_weights_;
+    Eigen::VectorXf em_weights_;
+
+
     enum class Statistics
     {
         mean,
         mode
     };
 
-
     Eigen::VectorXf mean(const Eigen::Ref<const Eigen::MatrixXf>& particles, const Eigen::Ref<const Eigen::VectorXf>& weights) const;
 
     Eigen::VectorXf mode(const Eigen::Ref<const Eigen::MatrixXf>& particles, const Eigen::Ref<const Eigen::VectorXf>& weights) const;
 
-
     Eigen::VectorXf simpleAverage(const Eigen::Ref<const Eigen::MatrixXf>& particles, const Eigen::Ref<const Eigen::VectorXf>& weights,
                                   const Statistics& base_est_ext);
 
@@ -70,16 +77,6 @@ class bfl::EstimatesExtraction
 
     Eigen::VectorXf exponentialAverage(const Eigen::Ref<const Eigen::MatrixXf>& particles, const Eigen::Ref<const Eigen::VectorXf>& weights,
                                        const Statistics& base_est_ext);
-
-
-    ExtractionMethod extraction_method_ = ExtractionMethod::emode;
-
-    HistoryBuffer hist_buffer_;
-
-    Eigen::VectorXf sm_weights_;
-    Eigen::VectorXf wm_weights_;
-    Eigen::VectorXf em_weights_;
-
 };
 
 #endif /* ESTIMATESEXTRACTION_H */

src/BayesFilters/include/BayesFilters/LinearSensor.h

@@ -14,9 +14,9 @@ namespace bfl {
 class bfl::LinearSensor : public ObservationModel
 {
 public:
-    LinearSensor(float T, float sigma_x, float sigma_y, unsigned int seed) noexcept;
+    LinearSensor(float sigma_x, float sigma_y, unsigned int seed) noexcept;
 
-    LinearSensor(float T, float sigma_x, float sigma_y) noexcept;
+    LinearSensor(float sigma_x, float sigma_y) noexcept;
 
     LinearSensor() noexcept;
 
@@ -41,7 +41,6 @@ class bfl::LinearSensor : public ObservationModel
     bool setProperty(const std::string property) override { return false; };
 
 protected:
-    float                           T_;                /* Sampling interval */
     float                           sigma_x_;          /* x-axis measurement noise std deviation [length] */
     float                           sigma_y_;          /* y-axis measurement noise std deviation [length] */
     Eigen::MatrixXf                 H_;                /* Measurement matrix */

src/BayesFilters/include/BayesFilters/PFVisualCorrection.h

@@ -28,11 +28,6 @@ class bfl::PFVisualCorrection
 
     bool skip(const bool status);
 
-
-    virtual VisualObservationModel& getVisualObservationModel() = 0;
-
-    virtual void setVisualObservationModel(std::unique_ptr<VisualObservationModel> visual_observation_model) = 0;
-
 protected:
     PFVisualCorrection() noexcept;
 

src/BayesFilters/include/BayesFilters/PFVisualCorrectionDecorator.h

@@ -17,10 +17,6 @@ class bfl::PFVisualCorrectionDecorator : public PFVisualCorrection
 
     double likelihood(const Eigen::Ref<const Eigen::MatrixXf>& innovations) override;
 
-    VisualObservationModel& getVisualObservationModel() override;
-
-    void setVisualObservationModel(std::unique_ptr<VisualObservationModel> visual_observation_model) override;
-
 protected:
     PFVisualCorrectionDecorator(std::unique_ptr<PFVisualCorrection> visual_correction) noexcept;
 

src/BayesFilters/src/EstimatesExtraction.cpp

@@ -220,10 +220,9 @@ VectorXf EstimatesExtraction::exponentialAverage(const Ref<const MatrixXf>& part
         em_weights_.resize(history.cols());
         for (unsigned int i = 0; i < history.cols(); ++i)
             em_weights_(i) = std::exp(-(static_cast<double>(i) / history.cols()));
-        
+
         em_weights_ /= em_weights_.sum();
     }
-
-
+
     return mean(history, em_weights_);
 }

src/BayesFilters/src/LinearSensor.cpp

@@ -7,8 +7,7 @@ using namespace bfl;
 using namespace Eigen;
 
 
-LinearSensor::LinearSensor(float T, float sigma_x, float sigma_y, unsigned int seed) noexcept :
-    T_(T),
+LinearSensor::LinearSensor(float sigma_x, float sigma_y, unsigned int seed) noexcept :
     sigma_x_(sigma_x),
     sigma_y_(sigma_y),
     generator_(std::mt19937_64(seed)),
@@ -27,19 +26,18 @@ LinearSensor::LinearSensor(float T, float sigma_x, float sigma_y, unsigned int s
 }
 
 
-LinearSensor::LinearSensor(float T, float sigma_x, float sigma_y) noexcept :
-    LinearSensor(T, sigma_x, sigma_y, 1) { }
+LinearSensor::LinearSensor(float sigma_x, float sigma_y) noexcept :
+    LinearSensor(sigma_x, sigma_y, 1) { }
 
 
 LinearSensor::LinearSensor() noexcept :
-    LinearSensor(1.0, 10.0, 10.0, 1) { }
+    LinearSensor(10.0, 10.0, 1) { }
 
 
 LinearSensor::~LinearSensor() noexcept { }
 
 
 LinearSensor::LinearSensor(const LinearSensor& lin_sense) :
-    T_(lin_sense.T_),
     sigma_x_(lin_sense.sigma_x_),
     sigma_y_(lin_sense.sigma_y_),
     H_(lin_sense.H_),
@@ -51,7 +49,6 @@ LinearSensor::LinearSensor(const LinearSensor& lin_sense) :
 
 
 LinearSensor::LinearSensor(LinearSensor&& lin_sense) noexcept :
-    T_(lin_sense.T_),
     sigma_x_(lin_sense.sigma_x_),
     sigma_y_(lin_sense.sigma_y_),
     H_(std::move(lin_sense.H_)),
@@ -61,7 +58,6 @@ LinearSensor::LinearSensor(LinearSensor&& lin_sense) noexcept :
     distribution_(std::move(lin_sense.distribution_)),
     gauss_rnd_sample_(std::move(lin_sense.gauss_rnd_sample_))
 {
-    lin_sense.T_       = 0.0;
     lin_sense.sigma_x_ = 0.0;
     lin_sense.sigma_y_ = 0.0;
 }
@@ -78,7 +74,6 @@ LinearSensor& LinearSensor::operator=(const LinearSensor& lin_sense) noexcept
 
 LinearSensor& LinearSensor::operator=(LinearSensor&& lin_sense) noexcept
 {
-    T_       = lin_sense.T_;
     sigma_x_ = lin_sense.sigma_x_;
     sigma_y_ = lin_sense.sigma_y_;
     H_       = std::move(lin_sense.H_);
@@ -89,7 +84,6 @@ LinearSensor& LinearSensor::operator=(LinearSensor&& lin_sense) noexcept
     distribution_     = std::move(lin_sense.distribution_);
     gauss_rnd_sample_ = std::move(lin_sense.gauss_rnd_sample_);
 
-    lin_sense.T_       = 0.0;
     lin_sense.sigma_x_ = 0.0;
     lin_sense.sigma_y_ = 0.0;
 

src/BayesFilters/src/PFCorrection.cpp

@@ -7,7 +7,11 @@ using namespace Eigen;
 PFCorrection::PFCorrection() noexcept { };
 
 
-PFCorrection::PFCorrection(PFCorrection&& pf_prediction) noexcept { }
+PFCorrection::PFCorrection(PFCorrection&& pf_prediction) noexcept :
+    skip_(pf_prediction.skip_)
+{
+    pf_prediction.skip_ = false;
+}
 
 
 void PFCorrection::correct(const Ref<const MatrixXf>& pred_states, const Ref<const VectorXf>& pred_weights, const Ref<const MatrixXf>& measurements,

src/BayesFilters/src/PFPrediction.cpp

@@ -10,7 +10,15 @@ using namespace Eigen;
 PFPrediction::PFPrediction() noexcept { };
 
 
-PFPrediction::PFPrediction(PFPrediction&& pf_prediction) noexcept { }
+PFPrediction::PFPrediction(PFPrediction&& pf_prediction) noexcept :
+    skip_prediction_(pf_prediction.skip_prediction_),
+    skip_state_(pf_prediction.skip_state_),
+    skip_exogenous_(pf_prediction.skip_exogenous_)
+{
+    pf_prediction.skip_prediction_ = false;
+    pf_prediction.skip_state_      = false;
+    pf_prediction.skip_exogenous_  = false;
+}
 
 
 void PFPrediction::predict(const Ref<const MatrixXf>& prev_states, const Ref<const VectorXf>& prev_weights,
@@ -62,6 +70,6 @@ ExogenousModel& PFPrediction::getExogenousModel()
 
 void PFPrediction::setExogenousModel(std::unique_ptr<ExogenousModel> exogenous_model)
 {
-    std::cerr << "ERROR::PFPREDICTION::SETEXOGENOUSMODEL" << std::endl;
-    std::cerr << "ERROR:\n\tCall to unimplemented base class method.";
+    std::cerr << "ERROR::PFPREDICTION::SETEXOGENOUSMODEL\n";
+    std::cerr << "ERROR:\n\tCall to unimplemented base class method." << std::endl;
 }

src/BayesFilters/src/PFVisualCorrectionDecorator.cpp

@@ -28,18 +28,6 @@ double PFVisualCorrectionDecorator::likelihood(const Eigen::Ref<const Eigen::Mat
 }
 
 
-VisualObservationModel& PFVisualCorrectionDecorator::getVisualObservationModel()
-{
-    return visual_correction_->getVisualObservationModel();
-}
-
-
-void PFVisualCorrectionDecorator::setVisualObservationModel(std::unique_ptr<VisualObservationModel> visual_observation_model)
-{
-    visual_correction_->setVisualObservationModel(std::move(visual_observation_model));
-}
-
-
 void PFVisualCorrectionDecorator::correctStep(const Eigen::Ref<const Eigen::MatrixXf>& pred_states, const Eigen::Ref<const Eigen::VectorXf>& pred_weights, cv::InputArray measurements,
                                               Eigen::Ref<Eigen::MatrixXf> cor_states, Eigen::Ref<Eigen::VectorXf> cor_weights)
 {

src/BayesFilters/src/SIS.cpp

@@ -9,7 +9,7 @@
 using namespace bfl;
 using namespace Eigen;
 
-    
+
 SIS::SIS() noexcept { }
 
 

src/BayesFilters/src/UpdateParticles.cpp

@@ -9,6 +9,7 @@ using namespace Eigen;
 
 UpdateParticles::UpdateParticles() noexcept { }
 
+
 UpdateParticles::UpdateParticles(UpdateParticles&& pf_correction) noexcept :
     PFCorrection(std::move(pf_correction)) { };
 
@@ -19,13 +20,13 @@ UpdateParticles::~UpdateParticles() noexcept { }
 void UpdateParticles::correctStep(const Ref<const MatrixXf>& pred_states, const Ref<const VectorXf>& pred_weights, const Ref<const MatrixXf>& measurements,
                                   Ref<MatrixXf> cor_states, Ref<VectorXf> cor_weights)
 {
-    cor_states = pred_states;
-
     MatrixXf innovations(measurements.rows(), pred_states.cols());
     innovation(pred_states, measurements, innovations);
 
     for (unsigned int i = 0; i < innovations.cols(); ++i)
         cor_weights(i) = likelihood(innovations.col(i));
+
+    cor_states = pred_states;
 }