From 3fed113159f0d8154ca2bcb25c35728b04d6261c Mon Sep 17 00:00:00 2001 From: Jose Luis Blanco-Claraco Date: Thu, 19 Oct 2023 11:05:38 +0200 Subject: [PATCH 1/5] version bump --- appveyor.yml | 2 +- doc/source/doxygen-docs/changelog.md | 3 +++ package.xml | 2 +- version_prefix.txt | 2 +- 4 files changed, 6 insertions(+), 3 deletions(-) diff --git a/appveyor.yml b/appveyor.yml index 63ec9259ee..cf906242a5 100644 --- a/appveyor.yml +++ b/appveyor.yml @@ -1,5 +1,5 @@ # version format -version: 2.11.0-{branch}-build{build} +version: 2.11.1-{branch}-build{build} os: Visual Studio 2019 diff --git a/doc/source/doxygen-docs/changelog.md b/doc/source/doxygen-docs/changelog.md index 5b31a310ae..8baa0fac35 100644 --- a/doc/source/doxygen-docs/changelog.md +++ b/doc/source/doxygen-docs/changelog.md @@ -1,5 +1,8 @@ \page changelog Change Log +# Version 2.11.1: UNRELEASED +(None yet) + # Version 2.11.0: Released Oct 19th, 2023 - Changes in libraries: - \ref mrpt_maps_grp diff --git a/package.xml b/package.xml index 9d9aa422bf..beea563a7d 100644 --- a/package.xml +++ b/package.xml @@ -7,7 +7,7 @@ mrpt2 - 2.11.0 + 2.11.1 Mobile Robot Programming Toolkit (MRPT) version 2.x Jose-Luis Blanco-Claraco diff --git a/version_prefix.txt b/version_prefix.txt index 4f01d59c29..84519a1082 100644 --- a/version_prefix.txt +++ b/version_prefix.txt @@ -1,4 +1,4 @@ -2.11.0 +2.11.1 # IMPORTANT: This file is parsed by CMake, don't add any comment to # the first line. # This file is used in both Windows and Linux scripts to automatically From 25448db6deab8be20ac70dd242264787ad700f6b Mon Sep 17 00:00:00 2001 From: Jose Luis Blanco-Claraco Date: Thu, 19 Oct 2023 23:42:18 +0200 Subject: [PATCH 2/5] Fix gcc warnings --- doc/source/doxygen-docs/changelog.md | 4 +++- libs/math/src/MatrixBase_impl.h | 4 ++-- 2 files changed, 5 insertions(+), 3 deletions(-) diff --git a/doc/source/doxygen-docs/changelog.md b/doc/source/doxygen-docs/changelog.md index 8baa0fac35..37c6addabf 100644 --- a/doc/source/doxygen-docs/changelog.md +++ b/doc/source/doxygen-docs/changelog.md @@ -1,7 +1,9 @@ \page changelog Change Log # Version 2.11.1: UNRELEASED -(None yet) +- Changes in libraries: + - \ref mrpt_math_grp + - Fix several build warnings introduced in the last version. # Version 2.11.0: Released Oct 19th, 2023 - Changes in libraries: diff --git a/libs/math/src/MatrixBase_impl.h b/libs/math/src/MatrixBase_impl.h index 54254e6669..e51df74576 100644 --- a/libs/math/src/MatrixBase_impl.h +++ b/libs/math/src/MatrixBase_impl.h @@ -45,7 +45,7 @@ void MatrixBase::removeColumns( idxs.resize(itEnd - idxs.begin()); for (const auto idx : idxs) { - ASSERT_LT_(idx, mbDerived().cols()); + ASSERT_LT_(idx, static_cast(mbDerived().cols())); } unsafeRemoveColumns(idxs); } @@ -76,7 +76,7 @@ void MatrixBase::removeRows( idxs.resize(itEnd - idxs.begin()); for (const auto idx : idxs) { - ASSERT_LT_(idx, mbDerived().rows()); + ASSERT_LT_(idx, static_cast(mbDerived().rows())); } unsafeRemoveRows(idxs); } From a280d55f6d2405732b991eb1644ac3ba0e9f04bc Mon Sep 17 00:00:00 2001 From: Jose Luis Blanco-Claraco Date: Fri, 20 Oct 2023 00:38:53 +0200 Subject: [PATCH 3/5] Add EKF tutorial --- .../example-bayes_tracking_example.md | 9 +- doc/source/tutorial-ekf.rst | 238 ++++++++++++++++++ doc/source/tutorials.rst | 1 + samples/bayes_tracking_example/README.md | 9 +- samples/bayes_tracking_example/test.cpp | 3 +- 5 files changed, 254 insertions(+), 6 deletions(-) create mode 100644 doc/source/tutorial-ekf.rst diff --git a/doc/source/doxygen-docs/example-bayes_tracking_example.md b/doc/source/doxygen-docs/example-bayes_tracking_example.md index ff61a39a13..3a97392927 100644 --- a/doc/source/doxygen-docs/example-bayes_tracking_example.md +++ b/doc/source/doxygen-docs/example-bayes_tracking_example.md @@ -2,8 +2,13 @@ This example illustrates how to implement an Extended Kalman Filter (EKF) and a particle filter (PF) using mrpt::bayes classes, for the problem of -tracking a 2D mobile target with state space being its location and its -velocity vector. +tracking a 2D mobile target with **state space** being its **location** and its +**velocity vector**. + +Demo video: [https://www.youtube.com/watch?v=0_gGXYzjcGE](https://www.youtube.com/watch?v=0_gGXYzjcGE) + +The equations of this example and the theory behind them are explained in [this tutorial](tutorial-ekf.html). + ![bayes_tracking_example screenshot](doc/source/images/bayes_tracking_example_screenshot.png) diff --git a/doc/source/tutorial-ekf.rst b/doc/source/tutorial-ekf.rst new file mode 100644 index 0000000000..81aeffcdcd --- /dev/null +++ b/doc/source/tutorial-ekf.rst @@ -0,0 +1,238 @@ +.. _tutorial-ekf: + +=================================================================== +Writing an Extended Kalman Filter (EKF) with mrpt::bayes +=================================================================== + +.. contents:: :local: + +.. toctree:: + :maxdepth: 2 + +In MRPT, the family of `Kalman Filter algorithms `_ +such as the Extended KF (EKF) or the Iterative EKF (IEKF) are centralized in +one single virtual class, `mrpt::bayes::CKalmanFilterCapable `_. + +This C++ class keeps the **system state vector** and the system **covariance matrix**, as well as a +generic method to execute one complete iteration of the selected algorithm. + +In practice, solving a specific problem requires **deriving a new class** from this virtual class +and implementing a few methods such as transforming the state vector through the transition model, +or computing the Jacobian of the observation model linearized at some given value of the state space. + +This page will teach you the implementation details of the 2D target tracking example +shown in this video (`full source code `_): + +.. raw:: html + +
+ +
+ + + +1. Kalman Filters in the MRPT +-------------------------------- + +A set of parameters that are problem-independent can be changed in the member `KF_options `_ +of this class, where the most important parameter is the **selection of the KF algorithm**. +Currently it can be chosen from the following ones: + +- Naive EKF: The basic EKF algorithm. +- Iterative Kalman Filter (IKF): This method re-linearizes the Jacobians around increasingly more + accurate values of the state vector. + +.. note:: + + An alternative implementation of Bayesian filtering in MRPT are Particle Filters. + + +2. Writing a KF class for a specific problem +---------------------------------------------- + +2.1. Deriving a new class +~~~~~~~~~~~~~~~~~~~~~~~~~~ + +First a new class must be derived from ``mrpt::bayes::CKalmanFilterCapable``. +A public method must be declared as an entry point for the user, which takes the domain-specific input data +(range observations, sonar measurements, temperatures, etc.) and calls the protected method ``runOneKalmanIteration()`` +of the parent class. +There are **two fundamental types of systems** the user can build by deriving a new class: + +- **A regular tracking problem**: The size of the state vector remains unchanged over time. This size must be returned by the virtual method `get_vehicle_size()`. +- **A SLAM-like problem**: The size of the state vector grows as new map elements are incorporated over time. + In this case the first ``get_vehicle_size()`` scalar elements of the state vector correspond to the state of + the vehicle/robot/camera/... and the rest of the state vector is a whole number of ``get_feature_size()`` sub-vectors, + each describing one map element (landmark, feature,...). + + +2.2. The internal flow of the algorithm +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ + +The method ``mrpt::bayes::CKalmanFilterCapable::runOneKalmanIteration()`` will sequentially call each of the virtual methods, +according to the following order: + +- **During the KF prediction stage**: + + - ``OnGetAction()`` + - ``OnTransitionModel()`` + - ``OnTransitionJacobian()`` + - ``OnTransitionNoise()`` + - ``OnNormalizeStateVector()``: This can be optionally implemented if required for the concrete problem. + - ``OnGetObservations()``: This is the ideal place for generating observations in those applications where it + requires an estimate of the current state (e.g. in visual SLAM, to predict where each landmark will be found in the image). + At this point the internal state vector and covariance contain the "prior distribution", i.e. updated through the transition model. + This is also the place where data association must be solved (in mapping problems). + +- **During the KF update stage**: + + - ``OnObservationModelAndJacobians()`` + - ``OnNormalizeStateVector()`` + - If the system implements a mapping problem, and the data association returned by ``OnGetObservations()`` indicates the + existence of unmapped observations, then the next method will be invoked for each of these new features: ``OnInverseObservationModel()``. + +- ``OnPostIteration()``: A placeholder for any code the user wants to execute after each iteration (e.g. logging, visualization,...). + + +3. An example +------------------- + +An example of a KF implementation can be found under `samples/bayesianTracking `_ +for the problem of **tracking a vehicle** from noisy **range and bearing** data. + +Here we will derive the required equations to be implemented, as well as how they are actually implemented in C++. +Note that this problem is also implemented as a Particle Filter in the same example in order to visualize side-to-side +the performance of both approaches. + +3.1. Problem Statement +~~~~~~~~~~~~~~~~~~~~~~~~~~~ + +The problem of **range-bearing tracking** is that of estimating the vehicle state: + +.. math:: + + \mathbf{x}=(x ~ y ~ v_x ~ v_y) + +where x and y are the Cartesian coordinates of the vehicle, and vx and vy are the linear velocities. +Thus, we will use a simple constant velocity model, where the transition function is: + +.. math:: + + \mathbf{x_k} = f(\mathbf{x_{k-1}}, \Delta t) = \left\{ \begin{array}{l} x_{k-1} + v_{x_k} \Delta t \\ y_{k-1} + v_{y_k} \Delta t \end{array} \right. + +We will consider the time interval between steps :math:`Delta t` as the action $u$ of the system. +The observation vector :math:`z=(z_b ~ z_r)` consists of the bearing and range of the vehicle from some point (arbitrarily set to the origin): + +.. math:: + + z_b = atan2(y,x) + +.. math:: + + z_r = \sqrt{ x^2 + y^2 } + +Then, it is straightforward to obtain the required Jacobian of the transition function: + +.. math:: + + \frac{\partial f}{\partial x} = \left( \begin{array}{cccc} 1 ~ 0 ~ \Delta t ~ 0 \\ 0 ~ 1 ~ 0 ~ \Delta t \\ 0 ~ 0 ~ 1 ~ 0 \\ 0 ~ 0 ~ 0 ~ 1 \end{array} \right) + +and the Jacobian of the observation model: + +.. math:: + + \frac{\partial h}{\partial x} = \left( \begin{array}{cccc} \frac{-y}{x^2+y^2} ~ \frac{1}{x\left(1+\left( \frac{y}{x} \right)^2\right)} ~ 0 ~ 0 \\ \frac{x}{\sqrt{x^2+y^2}} ~ \frac{y}{\sqrt{x^2+y^2}} ~ 0 ~ 0 \end{array} \right) + + +3.2 Implementation +~~~~~~~~~~~~~~~~~~~~ + +The most important implemented methods are detailed below. For further details refer to the complete sources of the example. + +3.2.1 The transition model +^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +The constant-velocity model is implemented simply as: + + +.. code-block:: cpp + + /** Implements the transition model $latex \hat{x}_{k|k-1} = f( \hat{x}_{k-1|k-1}, u_k ) $ + * \param in_u The vector returned by OnGetAction. + * \param inout_x At input has $latex \hat{x}_{k-1|k-1} $, at output must have $latex \hat{x}_{k|k-1} $. + * \param out_skip Set this to true if for some reason you want to skip the prediction step (to do not modify either the vector or the covariance). Default:false + */ + void CRangeBearing::OnTransitionModel( + const KFArray_ACT &in_u, + KFArray_VEH &inout_x, + bool &out_skipPrediction + ) const + { + // in_u[0] : Delta time + // in_out_x: [0]:x [1]:y [2]:vx [3]: vy + inout_x[0] += in_u[0] * inout_x[2]; + inout_x[1] += in_u[0] * inout_x[3]; + } + +3.2.2 The transition model Jacobian +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +This is just the Jacobian of the state propagation equation above: + +.. code-block:: cpp + + /** Implements the transition Jacobian $latex \frac{\partial f}{\partial x} $ + * \param out_F Must return the Jacobian. + * The returned matrix must be $N \times N$latex with N being either the size of the whole state vector or get_vehicle_size(). + */ + void CRangeBearing::OnTransitionJacobian(KFMatrix_VxV &F) const + { + F.unit(); + + F(0,2) = m_deltaTime; + F(1,3) = m_deltaTime; + } + +3.2.3 The observations and the observation model +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +.. code-block:: cpp + + void CRangeBearing::OnGetObservationsAndDataAssociation( + std::vector &out_z, + vector_int &out_data_association, + const vector &in_all_predictions, + const KFMatrix &in_S, + const vector_size_t &in_lm_indices_in_S, + const KFMatrix_OxO &in_R + ) + { + out_z.resize(1); + out_z[0][0] = m_obsBearing; + out_z[0][1] = m_obsRange; + + out_data_association.clear(); // Not used + } + + /** Implements the observation prediction $latex h_i(x) $. + * \param idx_landmark_to_predict The indices of the landmarks in the map whose predictions are expected as output. For non SLAM-like problems, this input value is undefined and the application should just generate one observation for the given problem. + * \param out_predictions The predicted observations. + */ + void CRangeBearing::OnObservationModel( + const vector_size_t &idx_landmarks_to_predict, + std::vector &out_predictions + ) const + { + // predicted bearing: + kftype x = m_xkk[0]; + kftype y = m_xkk[1]; + + kftype h_bear = atan2(y,x); + kftype h_range = sqrt(square(x)+square(y)); + + // idx_landmarks_to_predict is ignored in NON-SLAM problems + out_predictions.resize(1); + out_predictions[0][0] = h_bear; + out_predictions[0][1] = h_range; + } + diff --git a/doc/source/tutorials.rst b/doc/source/tutorials.rst index b7f9a2bbe9..d1d6ad765a 100644 --- a/doc/source/tutorials.rst +++ b/doc/source/tutorials.rst @@ -47,6 +47,7 @@ Note: This page is in the process of being imported from https://www.mrpt.org/tu :maxdepth: 2 :caption: Maths, Geometry + tutorial-ekf page_tutorial_math_levenberg_marquardt tutorial-ransac tutorial-pdf-over-poses diff --git a/samples/bayes_tracking_example/README.md b/samples/bayes_tracking_example/README.md index 6911cf160a..1fbacc547d 100644 --- a/samples/bayes_tracking_example/README.md +++ b/samples/bayes_tracking_example/README.md @@ -1,4 +1,9 @@ This example illustrates how to implement an Extended Kalman Filter (EKF) and a particle filter (PF) using mrpt::bayes classes, for the problem of -tracking a 2D mobile target with state space being its location and its -velocity vector. +tracking a 2D mobile target with **state space** being its **location** and its +**velocity vector**. + +Demo video: [https://www.youtube.com/watch?v=0_gGXYzjcGE](https://www.youtube.com/watch?v=0_gGXYzjcGE) + +The equations of this example and the theory behind them are explained in [this tutorial](tutorial-ekf.html). + diff --git a/samples/bayes_tracking_example/test.cpp b/samples/bayes_tracking_example/test.cpp index b47df71079..fcf928aa02 100644 --- a/samples/bayes_tracking_example/test.cpp +++ b/samples/bayes_tracking_example/test.cpp @@ -329,7 +329,7 @@ void TestBayesianTracking() printf( "Real: x:%.03f y=%.03f heading=%.03f v=%.03f w=%.03f\n", x, y, phi, v, w); - cout << "EKF: " << EKF_xkk << endl; + cout << "EKF: " << EKF_xkk.transpose() << endl; // Show PF state: cout << "Particle filter ESS: " << particles.ESS() << endl; @@ -377,7 +377,6 @@ void TestBayesianTracking() particles.getMean(avrg_x, avrg_y, avrg_vx, avrg_vy); - vector vx(2), vy(2); vx[0] = avrg_x; vx[1] = vx[0] + avrg_vx * 1; vy[0] = avrg_y; From 88de8034a403b3f3681a6f017bf533f2ae2c9bf2 Mon Sep 17 00:00:00 2001 From: Jose Luis Blanco-Claraco Date: Sun, 22 Oct 2023 22:32:16 +0200 Subject: [PATCH 4/5] Flexible voxel occupancy threshold --- doc/source/doxygen-docs/changelog.md | 2 ++ libs/maps/include/mrpt/maps/CVoxelMapOccupancyBase.h | 9 +++++++-- 2 files changed, 9 insertions(+), 2 deletions(-) diff --git a/doc/source/doxygen-docs/changelog.md b/doc/source/doxygen-docs/changelog.md index 37c6addabf..4618e970e6 100644 --- a/doc/source/doxygen-docs/changelog.md +++ b/doc/source/doxygen-docs/changelog.md @@ -4,6 +4,8 @@ - Changes in libraries: - \ref mrpt_math_grp - Fix several build warnings introduced in the last version. + - \ref mrpt_maps_grp + - Voxel maps: A voxel is considered occupied if its occupancy is larger than `likelihoodOptions.occupiedThreshold` instead of a fixed `0.5`. # Version 2.11.0: Released Oct 19th, 2023 - Changes in libraries: diff --git a/libs/maps/include/mrpt/maps/CVoxelMapOccupancyBase.h b/libs/maps/include/mrpt/maps/CVoxelMapOccupancyBase.h index f8f05c5536..be31c0b747 100644 --- a/libs/maps/include/mrpt/maps/CVoxelMapOccupancyBase.h +++ b/libs/maps/include/mrpt/maps/CVoxelMapOccupancyBase.h @@ -163,6 +163,9 @@ class CVoxelMapOccupancyBase : public CVoxelMapBase, * while also updating the voxel map in another thread. * * The point cloud is cached, and invalidated upon map updates. + * + * A voxel is considered occupied if its occupancy is larger than + * `likelihoodOptions.occupiedThreshold` (Range: [0,1], default: 0.6) */ mrpt::maps::CSimplePointsMap::Ptr getOccupiedVoxels() const; @@ -623,8 +626,10 @@ void CVoxelMapOccupancyBase::updateCachedProperties() auto& grid = const_cast&>(base_t::m_impl->grid); + const double freenessThreshold = 1.0 - likelihoodOptions.occupiedThreshold; + // Go thru all voxels: - auto lmbdPerVoxel = [this, &grid]( + auto lmbdPerVoxel = [this, freenessThreshold, &grid]( voxel_node_t& data, const Bonxai::CoordT& coord) { using mrpt::img::TColor; @@ -634,7 +639,7 @@ void CVoxelMapOccupancyBase::updateCachedProperties() m_bbox.updateWithPoint({pt.x, pt.y, pt.z}); - if (occFreeness < 0.5) + if (occFreeness < freenessThreshold) { m_cachedOccupied->insertPointFast(pt.x, pt.y, pt.z); } From 0660153659d76cfaf289c48e1fcb12d2c046edd4 Mon Sep 17 00:00:00 2001 From: Jose Luis Blanco-Claraco Date: Mon, 23 Oct 2023 16:07:43 +0200 Subject: [PATCH 5/5] release date --- doc/source/doxygen-docs/changelog.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/doc/source/doxygen-docs/changelog.md b/doc/source/doxygen-docs/changelog.md index 4618e970e6..f920aeeb52 100644 --- a/doc/source/doxygen-docs/changelog.md +++ b/doc/source/doxygen-docs/changelog.md @@ -1,6 +1,6 @@ \page changelog Change Log -# Version 2.11.1: UNRELEASED +# Version 2.11.1: Released Oct 23rd, 2023 - Changes in libraries: - \ref mrpt_math_grp - Fix several build warnings introduced in the last version.