Merge pull request #7 from hsp-iit/fix/new-bfl

Update to support latest bayes-filters-lib devel
hsp-iit · Jul 17, 2022 · 070c1d7 · 070c1d7
2 parents de5a9dc + 8c01f80
commit 070c1d7
Show file tree

Hide file tree

Showing 13 changed files with 86 additions and 314 deletions.
diff --git a/CITATION.cff b/CITATION.cff
@@ -20,7 +20,7 @@ authors:
   given-names: "Lorenzo"
   orcid: "https://orcid.org/0000-0002-8777-5233"
 title: "mask-ukf"
-version: v1.1.4
+version: v1.2.0
 doi: 10.5281/zenodo.5448471
-date-released: 2022-01-15
+date-released: 2022-07-17
 url: "https://github.com/hsp-iit/mask-ukf"
diff --git a/dockerfiles/Dockerfile b/dockerfiles/Dockerfile
@@ -7,38 +7,59 @@ LABEL org.opencontainers.image.authors="Nicola A. Piga <nicola.piga@iit.it>"
 # Non-interactive installation mode
 ENV DEBIAN_FRONTEND=noninteractive
 
-# Update apt database
-RUN apt update
-
 # Install essentials
-RUN apt install -y build-essential cmake cmake-curses-gui emacs-nox git htop iputils-ping locales nano python3 python3-pip vim wget
+RUN apt update && \
+    apt install -y build-essential cmake cmake-curses-gui curl emacs-nox git htop iputils-ping locales nano python3 python3-pip sudo vim wget && \
+    rm -rf /var/lib/apt/lists/*
+
+# Install GitHub cli
+RUN curl -fsSL https://cli.github.com/packages/githubcli-archive-keyring.gpg | dd of=/usr/share/keyrings/githubcli-archive-keyring.gpg && \
+    chmod go+r /usr/share/keyrings/githubcli-archive-keyring.gpg && \
+    echo "deb [arch=$(dpkg --print-architecture) signed-by=/usr/share/keyrings/githubcli-archive-keyring.gpg] https://cli.github.com/packages stable main" | tee /etc/apt/sources.list.d/github-cli.list > /dev/null && \
+    apt update && \
+    apt install --no-install-recommends -y -qq gh && \
+    rm -rf /var/lib/apt/lists/*
 
 # Set the locale
 RUN locale-gen en_US.UTF-8
 
 # Install dependencies
-RUN apt install -y libeigen3-dev libmlpack-dev libopencv-dev libpcl-dev libtclap-dev libconfig++-dev
+RUN apt update && \
+    apt install -y libeigen3-dev libmlpack-dev libopencv-dev libpcl-dev libtclap-dev libconfig++-dev
 
 # Install dependencies (from source)
 
-# Install bayes-filters-lib
-RUN git clone https://github.com/robotology/bayes-filters-lib && \
+# Install bayes-filters-lib (devel)
+RUN git clone -b devel https://github.com/robotology/bayes-filters-lib && \
     cd bayes-filters-lib && mkdir build && cd build && \
     cmake .. && \
     make install
 
 # Install nanoflann
 RUN git clone https://github.com/jlblancoc/nanoflann && \
-    cd nanoflann && mkdir build && cd build && \
+    cd nanoflann && git checkout 05d9c35d175fbcedd9f1f854ab49d04ae0bc5dbc && mkdir build && cd build && \
     cmake -DNANOFLANN_BUILD_EXAMPLES:BOOL=OFF -DNANOFLANN_BUILD_TESTS:BOOL=OFF .. && \
     make install
 
-# Create user
-RUN useradd -l -G sudo -md /home/user -s /bin/bash -p user user
+# Create user with passwordless sudo
+RUN useradd -l -G sudo -md /home/user -s /bin/bash -p user user && \
+    sed -i.bkp -e 's/%sudo\s\+ALL=(ALL\(:ALL\)\?)\s\+ALL/%sudo ALL=NOPASSWD:ALL/g' /etc/sudoers
 
 # Switch to user
 USER user
 
+# Configure emacs
+RUN echo "(setq-default indent-tabs-mode nil)" >> /home/user/.emacs.el && \
+    echo "(setq-default tab-width 4)" >> /home/user/.emacs.el && \
+    echo "(setq make-backup-files nil)" >> /home/user/.emacs.el && \
+    echo "(setq auto-save-default nil)" >> /home/user/.emacs.el && \
+    echo "(setq c-default-style \"linux\"" >> /home/user/.emacs.el && \
+    echo "      c-basic-offset 4)" >> /home/user/.emacs.el && \
+    echo "(global-subword-mode 1)" >> /home/user/.emacs.el && \
+    echo "(add-hook 'before-save-hook 'delete-trailing-whitespace)" >> /home/user/.emacs.el && \
+    echo "(custom-set-variables '(custom-enabled-themes '(tango-dark)))" >> /home/user/.emacs.el && \
+    echo "(custom-set-faces)" >> /home/user/.emacs.elx
+
 # Install python dependencies
 RUN pip3 install pyquaternion pyrender opencv-python
 

diff --git a/src/mask-ukf-tracker/CMakeLists.txt b/src/mask-ukf-tracker/CMakeLists.txt
@@ -53,7 +53,6 @@ set(${EXE_TARGET_NAME}_HDR
     include/PointCloudPrediction.h
     include/PointCloudSegmentation.h
     include/StaticPrediction.h
-    include/SUKFCorrection.h
     include/UKFFilter.h
     include/YCBVideoCamera.h
     include/kdTree.h
@@ -72,7 +71,6 @@ set(${EXE_TARGET_NAME}_SRC
     src/PointCloudModel.cpp
     src/PointCloudSegmentation.cpp
     src/StaticPrediction.cpp
-    src/SUKFCorrection.cpp
     src/UKFFilter.cpp
     src/YCBVideoCamera.cpp
     src/main.cpp

diff --git a/src/mask-ukf-tracker/include/Correction.h b/src/mask-ukf-tracker/include/Correction.h
@@ -9,12 +9,12 @@
 #define CORRECTION_H
 
 #include <BayesFilters/AdditiveMeasurementModel.h>
+#include <BayesFilters/SUKFCorrection.h>
 
 #include <ObjectMeasurements.h>
-#include <SUKFCorrection.h>
 
 
-class Correction : public SUKFCorrection
+class Correction : public bfl::SUKFCorrection
 {
 public:
     Correction
@@ -23,7 +23,6 @@ class Correction : public SUKFCorrection
         /**
          * Unscented transform parameters
          */
-        const std::size_t state_size,
         const double      alpha,
         const double      beta,
         const double      kappa,

diff --git a/src/mask-ukf-tracker/include/DiscretizedKinematicModel.h b/src/mask-ukf-tracker/include/DiscretizedKinematicModel.h
@@ -9,6 +9,7 @@
 #define DISCRETIZEDKINEMATICMODEL_H
 
 #include <BayesFilters/LinearStateModel.h>
+#include <BayesFilters/VectorDescription.h>
 
 #include <Eigen/Dense>
 
@@ -38,7 +39,9 @@ class DiscretizedKinematicModel : public bfl::LinearStateModel
 
     Eigen::MatrixXd getNoiseCovarianceMatrix();
 
-    std::pair<std::size_t, std::size_t> getOutputSize() const override;
+    bfl::VectorDescription getInputDescription() override;
+
+    bfl::VectorDescription getStateDescription() override;
 
 protected:
     void evaluateStateTransitionMatrix(const double T);

diff --git a/src/mask-ukf-tracker/include/ObjectMeasurements.h b/src/mask-ukf-tracker/include/ObjectMeasurements.h
@@ -10,6 +10,7 @@
 
 #include <BayesFilters/AdditiveMeasurementModel.h>
 #include <BayesFilters/Data.h>
+#include <BayesFilters/VectorDescription.h>
 
 #include <Camera.h>
 #include <PointCloudPrediction.h>
@@ -31,16 +32,16 @@ class ObjectMeasurements : public bfl::AdditiveMeasurementModel
 
     bool freeze(const bfl::Data& data = bfl::Data()) override;
 
-    std::pair<std::size_t, std::size_t> getOutputSize() const;
+    bfl::VectorDescription getInputDescription() const override;
+
+    bfl::VectorDescription getMeasurementDescription() const override;
 
     std::pair<bool, bfl::Data> predictedMeasure(const Eigen::Ref<const Eigen::MatrixXd>& current_states) const override;
 
     std::pair<bool, bfl::Data> innovation(const bfl::Data& predicted_measurements, const bfl::Data& measurements) const override;
 
     std::pair<bool, Eigen::MatrixXd> getNoiseCovarianceMatrix() const override;
 
-    std::pair<bool, Eigen::MatrixXd> getVisualNoiseCovarianceMatrix() const;
-
     bool setProperty(const std::string& property) override;
 
     void enableOutlierRejection(const bool& enable);

diff --git a/src/mask-ukf-tracker/include/SUKFCorrection.h b/src/mask-ukf-tracker/include/SUKFCorrection.h
diff --git a/src/mask-ukf-tracker/include/StaticPrediction.h b/src/mask-ukf-tracker/include/StaticPrediction.h
@@ -11,6 +11,7 @@
 #include <BayesFilters/GaussianMixture.h>
 #include <BayesFilters/GaussianPrediction.h>
 #include <BayesFilters/StateModel.h>
+#include <BayesFilters/VectorDescription.h>
 
 #include <memory>
 
@@ -35,12 +36,24 @@ class StaticModel : public bfl::StateModel
         return false;
     };
 
-    std::pair<std::size_t, std::size_t> getOutputSize() const override
+    bfl::VectorDescription getInputDescription() override
     {
-        return std::make_pair(9, 3);
+        // 9 linear components (x, y, z, x_dot, y_dot, z_dot, yaw_dot, pitch_dot, roll_dot)
+        // 3 angular components (yaw, pitch, roll)
+        // 12 noise components
+        return bfl::VectorDescription(9, 3, 12, bfl::VectorDescription::CircularType::Euler);
+    };
+
+    bfl::VectorDescription getStateDescription() override
+    {
+        // 9 linear components (x, y, z, x_dot, y_dot, z_dot, yaw_dot, pitch_dot, roll_dot)
+        // 3 angular components (yaw, pitch, roll)
+        // 0 noise components
+        return bfl::VectorDescription(9, 3, 0, bfl::VectorDescription::CircularType::Euler);
     };
 };
 
+
 class StaticPrediction : public bfl::GaussianPrediction
 {
 public:

diff --git a/src/mask-ukf-tracker/src/Correction.cpp b/src/mask-ukf-tracker/src/Correction.cpp
@@ -19,7 +19,6 @@ Correction::Correction
     /**
      * Unscented transform parameters
      */
-    const std::size_t state_size,
     const double      alpha,
     const double      beta,
     const double      kappa,
@@ -29,7 +28,7 @@ Correction::Correction
      */
     const std::size_t meas_sub_size
 ) noexcept :
-    SUKFCorrection(std::move(meas_model), state_size, alpha, beta, kappa, meas_sub_size, true)
+    SUKFCorrection(std::move(meas_model), alpha, beta, kappa, meas_sub_size, true)
 { }
 
 

diff --git a/src/mask-ukf-tracker/src/DiscretizedKinematicModel.cpp b/src/mask-ukf-tracker/src/DiscretizedKinematicModel.cpp
@@ -10,6 +10,7 @@
 #include <Eigen/Dense>
 
 using namespace Eigen;
+using namespace bfl;
 
 
 DiscretizedKinematicModel::DiscretizedKinematicModel
@@ -76,11 +77,22 @@ void DiscretizedKinematicModel::evaluateNoiseCovarianceMatrix(const double T)
     Q_.block<6, 6>(6, 6) = Q_ang;
 }
 
-std::pair<std::size_t, std::size_t> DiscretizedKinematicModel::getOutputSize() const
+
+VectorDescription DiscretizedKinematicModel::getInputDescription()
+{
+    // 9 linear components (x, y, z, x_dot, y_dot, z_dot, yaw_dot, pitch_dot, roll_dot)
+    // 3 angular components (yaw, pitch, roll)
+    // 12 noise components
+    return VectorDescription(9, 3, 12, VectorDescription::CircularType::Euler);
+}
+
+
+VectorDescription DiscretizedKinematicModel::getStateDescription()
 {
     // 9 linear components (x, y, z, x_dot, y_dot, z_dot, yaw_dot, pitch_dot, roll_dot)
     // 3 angular components (yaw, pitch, roll)
-    return std::make_pair(9, 3);
+    // 0 noise components
+    return VectorDescription(9, 3, 0, VectorDescription::CircularType::Euler);
 }
 
 

diff --git a/src/mask-ukf-tracker/src/ObjectMeasurements.cpp b/src/mask-ukf-tracker/src/ObjectMeasurements.cpp
@@ -213,9 +213,19 @@ bool ObjectMeasurements::freeze(const Data& data)
 }
 
 
-std::pair<std::size_t, std::size_t> ObjectMeasurements::getOutputSize() const
+VectorDescription ObjectMeasurements::getInputDescription() const
 {
-    return std::make_pair(measurement_.size(), 0);
+    // 9 linear components (x, y, z, x_dot, y_dot, z_dot, yaw_dot, pitch_dot, roll_dot)
+    // 3 angular components (yaw, pitch, roll)
+    // 12 noise components
+    return VectorDescription(9, 3, 12, VectorDescription::CircularType::Euler);
+}
+
+
+VectorDescription ObjectMeasurements::getMeasurementDescription() const
+{
+    // measurement_.size() linear components
+    return VectorDescription(measurement_.size());
 }