port mrpt_bayes

Author: jlblancoc

modules/mrpt_bayes/CMakeLists.txt

@@ -1,14 +1,49 @@
-#---------------------------------------------
-# Macro declared in "DeclareMRPTLib.cmake":
-#---------------------------------------------
-define_mrpt_lib(
-	# Lib name
-	bayes
-	# Dependencies
-	mrpt-math
-	mrpt-config
-	)
-
-if(BUILD_mrpt-bayes)
-
-endif()
+# ------------------------------------------------------------------------------
+#        Mobile Robot Programming Toolkit (MRPT)
+#
+# Copyright (c) 2005-2025, Jose Luis Blanco-Claraco, contributors (see Git history)
+# All rights reserved.
+# Released under BSD-3 license. See LICENSE file
+# ------------------------------------------------------------------------------
+
+cmake_minimum_required(VERSION 3.16)
+
+# Tell CMake we'll use C++ for use in its tests/flags
+project(mrpt_bayes LANGUAGES C CXX)
+
+# MRPT CMake scripts: "mrpt_xxx()"
+find_package(mrpt_common REQUIRED)
+find_package(mrpt_config REQUIRED)
+find_package(mrpt_math REQUIRED)
+
+
+# define lib:
+set(LIB_SRCS
+  src/CParticleFilter.cpp
+  src/CParticleFilterCapable.cpp
+)
+
+set(LIB_PUBLIC_HDRS
+  include/mrpt/bayes/CRejectionSamplingCapable.h
+  include/mrpt/bayes/CKalmanFilterCapable.h
+  include/mrpt/bayes/registerAllClasses.h
+  include/mrpt/bayes/CParticleFilterCapable.h
+  include/mrpt/bayes/CProbabilityParticle.h
+  include/mrpt/bayes/CParticleFilterData.h
+  include/mrpt/bayes/CKalmanFilterCapable_impl.h
+  include/mrpt/bayes/CParticleFilter.h
+)
+
+mrpt_add_library(
+  TARGET ${PROJECT_NAME}
+  SOURCES ${LIB_SRCS} ${LIB_PUBLIC_HDRS}
+  PUBLIC_LINK_LIBRARIES
+    mrpt::mrpt_config
+    mrpt::mrpt_math
+#  PRIVATE_LINK_LIBRARIES
+  CMAKE_DEPENDENCIES
+    mrpt_config
+    mrpt_math
+  # Other imported targets:
+)
+

modules/mrpt_bayes/package.xml

@@ -0,0 +1,27 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<!-- This is a ROS package file, intended to allow this library to be built
+     side-by-side to ROS packages in a catkin/ament environment.
+-->
+<package format="3">
+  <name>mrpt_bayes</name>
+  <version>2.20.0</version>
+  <description>The MRPT C++ library mrpt_bayes</description>
+
+  <maintainer email="[email protected]">Jose-Luis Blanco-Claraco</maintainer>
+  <license file="LICENSE">BSD</license>
+
+  <url type="website">https://github.com/MRPT/</url>
+
+  <!-- BUILD TOOLS -->
+  <buildtool_depend>cmake</buildtool_depend>
+
+  <depend>mrpt_common</depend>
+  <depend>mrpt_config</depend>
+  <depend>mrpt_math</depend>
+
+  <export>
+    <build_type>cmake</build_type>
+  </export>
+
+</package>

modules/mrpt_bayes/src/CParticleFilter.cpp

@@ -7,18 +7,15 @@
    | Released under BSD License. See: https://www.mrpt.org/License          |
    +------------------------------------------------------------------------+ */
 
-#include "bayes-precomp.h"  // Precompiled headers
-//
 #include <mrpt/bayes/CParticleFilter.h>         // for CParticleFilter::TPar...
 #include <mrpt/bayes/CParticleFilterCapable.h>  // for CParticleFilterCapable
 #include <mrpt/config/CConfigFileBase.h>        // for CConfigFileBase, MRPT...
 #include <mrpt/core/bits_math.h>                // square()
 #include <mrpt/system/COutputLogger.h>          // for COutputLogger, MRPT_L...
 
-#include <cmath>      // for exp
-#include <cstddef>    // for size_t
-#include <exception>  // for exception
-#include <string>     // for string, allocator
+#include <cmath>    // for exp
+#include <cstddef>  // for size_t
+#include <string>   // for string, allocator
 
 namespace mrpt
 {

modules/mrpt_bayes/src/CParticleFilterCapable.cpp

@@ -7,8 +7,6 @@
    | Released under BSD License. See: https://www.mrpt.org/License          |
    +------------------------------------------------------------------------+ */
 
-#include "bayes-precomp.h"  // Precompiled headers
-//
 #include <mrpt/bayes/CParticleFilterCapable.h>
 #include <mrpt/math/ops_vectors.h>
 #include <mrpt/random.h>
@@ -47,7 +45,10 @@ void CParticleFilterCapable::performResampling(
   performSubstitution(indxs);
 
   // Finally, equal weights:
-  for (size_t i = 0; i < out_particle_count; i++) setW(i, 0 /* Logarithmic weight */);
+  for (size_t i = 0; i < out_particle_count; i++)
+  {
+    setW(i, 0 /* Logarithmic weight */);
+  }
 
   MRPT_END
 }
@@ -69,7 +70,10 @@ void CParticleFilterCapable::computeResampling(
   size_t i, j, M = in_logWeights.size();
   ASSERT_(M > 0);
 
-  if (!out_particle_count) out_particle_count = M;
+  if (!out_particle_count)
+  {
+    out_particle_count = M;
+  }
 
   vector<double> linW(M, 0);
   vector<double>::const_iterator inIt;
@@ -79,7 +83,9 @@ void CParticleFilterCapable::computeResampling(
   // This is to avoid float point range problems:
   double max_log_w = math::maximum(in_logWeights);
   for (i = 0, inIt = in_logWeights.begin(), outIt = linW.begin(); i < M; i++, inIt++, outIt++)
+  {
     linW_SUM += ((*outIt) = exp((*inIt) - max_log_w));
+  }
 
   // Normalize weights:
   ASSERT_(linW_SUM > 0);
@@ -132,7 +138,7 @@ void CParticleFilterCapable::computeResampling(
       size_t R = 0;  // Remainder or residual count
       for (i = 0; i < M; i++)
       {
-        N[i] = int(M * linW[i]);
+        N[i] = static_cast<uint32_t>(M * linW[i]);
         R += N[i];
       }
       size_t N_rnd =
@@ -143,7 +149,12 @@ void CParticleFilterCapable::computeResampling(
       // Fillout the deterministic part of the resampling:
       out_indexes.resize(out_particle_count);
       for (i = 0, j = 0; i < out_particle_count; i++)
-        for (size_t k = 0; k < N[i]; k++) out_indexes[j++] = i;
+      {
+        for (size_t k = 0; k < N[i]; k++)
+        {
+          out_indexes[j++] = i;
+        }
+      }
 
       size_t M_fixed = j;
 
@@ -156,7 +167,10 @@ void CParticleFilterCapable::computeResampling(
         // Compute modified weights:
         vector<double> linW_mod(M);
         const double M_R_1 = 1.0 / N_rnd;
-        for (i = 0; i < M; i++) linW_mod[i] = M_R_1 * (M * linW[i] - N[i]);
+        for (i = 0; i < M; i++)
+        {
+          linW_mod[i] = M_R_1 * (M * linW[i] - N[i]);
+        }
 
         // perform resampling:
         vector<double> Q;
@@ -214,7 +228,9 @@ void CParticleFilterCapable::computeResampling(
       while (i < out_particle_count)
       {
         if (T[i] < Q[j])
+        {
           out_indexes[i++] = (unsigned int)j;
+        }
         else
         {
           j++;
@@ -236,15 +252,20 @@ void CParticleFilterCapable::computeResampling(
       vector<double> T(M + 1);
       double _1_M = 1.0 / M;
       T[0] = getRandomGenerator().drawUniform(0.0, _1_M);
-      for (i = 1; i < M; i++) T[i] = T[i - 1] + _1_M;
+      for (i = 1; i < M; i++)
+      {
+        T[i] = T[i - 1] + _1_M;
+      }
       T[M] = 1;
 
       out_indexes.resize(out_particle_count);
       i = j = 0;
       while (i < out_particle_count)
       {
         if (T[i] < Q[j])
+        {
           out_indexes[i++] = (unsigned int)j;
+        }
         else
         {
           j++;
@@ -355,9 +376,11 @@ void CParticleFilterCapable::prepareFastDrawSample(
     //  -> Use m_fastDrawAuxiliary.CDF, PDF, CDF_indexes
     // --------------------------------------------------------
     if (PF_options.resamplingMethod != CParticleFilter::prMultinomial)
+    {
       THROW_EXCEPTION(
           "resamplingMethod must be 'prMultinomial' for a dynamic number "
           "of particles!");
+    }
 
     size_t i, j = 666666, M = particlesCount();
 
@@ -376,7 +399,9 @@ void CParticleFilterCapable::prepareFastDrawSample(
     double SUM = 0;
     // Save the log likelihoods:
     for (i = 0; i < M; i++)
+    {
       m_fastDrawAuxiliary.PDF[i] = partEvaluator(PF_options, this, i, action, observation);
+    }
     // "Normalize":
     m_fastDrawAuxiliary.PDF += -math::maximum(m_fastDrawAuxiliary.PDF);
     for (i = 0; i < M; i++) SUM += m_fastDrawAuxiliary.PDF[i] = exp(m_fastDrawAuxiliary.PDF[i]);
@@ -386,34 +411,37 @@ void CParticleFilterCapable::prepareFastDrawSample(
 
     // Compute the CDF thresholds:
     for (i = 0; i < PARTICLE_FILTER_CAPABLE_FAST_DRAW_BINS; i++)
+    {
       m_fastDrawAuxiliary.CDF[i] = ((double)i) / ((double)PARTICLE_FILTER_CAPABLE_FAST_DRAW_BINS);
+    }
     m_fastDrawAuxiliary.CDF[PARTICLE_FILTER_CAPABLE_FAST_DRAW_BINS] = 1.0;
 
     // Compute the CDF and save threshold indexes:
     double CDF = 0;  // Cumulative density func.
     for (i = 0, j = 0; i < M && j < PARTICLE_FILTER_CAPABLE_FAST_DRAW_BINS; i++)
     {
       double CDF_next = CDF + m_fastDrawAuxiliary.PDF[i];
-      if (i == (M - 1)) CDF_next = 1.0;  // rounds fix...
-      if (CDF_next > 1.0) CDF_next = 1.0;
+      if (i == (M - 1))
+      {
+        CDF_next = 1.0;  // rounds fix...
+      }
+      if (CDF_next > 1.0)
+      {
+        CDF_next = 1.0;
+      }
 
       while (m_fastDrawAuxiliary.CDF[j] < CDF_next)
+      {
         m_fastDrawAuxiliary.CDF_indexes[j++] = (unsigned int)i;
+      }
 
       CDF = CDF_next;
     }
 
     ASSERT_(j == PARTICLE_FILTER_CAPABLE_FAST_DRAW_BINS);
 
-// Done!
-#if !defined(_MSC_VER) || (_MSC_VER > 1400)  // <=VC2005 doesn't work with this!
-    MRPT_END_WITH_CLEAN_UP(                  /* Debug: */
-                           cout << "j=" << j << "\nm_fastDrawAuxiliary.CDF_indexes:"
-                                << m_fastDrawAuxiliary.CDF_indexes << endl;
-                           cout << "m_fastDrawAuxiliary.CDF:" << m_fastDrawAuxiliary.CDF << endl;);
-#else
+    // Done!
     MRPT_END
-#endif
   }
   else
   {
@@ -426,9 +454,10 @@ void CParticleFilterCapable::prepareFastDrawSample(
     size_t i, M = particlesCount();
     vector<double> PDF(M, 0);
     for (i = 0; i < M; i++)
-      PDF[i] = partEvaluator(
-          PF_options, this, i, action,
-          observation);  // Default evaluator: takes current weight.
+    {
+      // Default evaluator: takes current weight.
+      PDF[i] = partEvaluator(PF_options, this, i, action, observation);
+    }
 
     vector<size_t> idxs;
 
@@ -440,7 +469,9 @@ void CParticleFilterCapable::prepareFastDrawSample(
     m_fastDrawAuxiliary.alreadyDrawnIndexes.resize(idxs.size());
     for (it = idxs.begin(), it2 = m_fastDrawAuxiliary.alreadyDrawnIndexes.begin(); it != idxs.end();
          ++it, ++it2)
+    {
       *it2 = (unsigned int)(*it);
+    }
 
     m_fastDrawAuxiliary.alreadyDrawnNextOne = 0;
   }
@@ -463,9 +494,11 @@ size_t CParticleFilterCapable::fastDrawSample(
     //  -> Use m_fastDrawAuxiliary.CDF, PDF, CDF_indexes
     // --------------------------------------------------------
     if (PF_options.resamplingMethod != CParticleFilter::prMultinomial)
+    {
       THROW_EXCEPTION(
           "resamplingMethod must be 'prMultinomial' for a dynamic number "
           "of particles!");
+    }
 
     double draw = getRandomGenerator().drawUniform(0.0, 0.999999);
     double CDF_next = -1.;
@@ -500,9 +533,11 @@ size_t CParticleFilterCapable::fastDrawSample(
     //  -> Use m_fastDrawAuxiliary.alreadyDrawnIndexes & alreadyDrawnNextOne
     // --------------------------------------------------------
     if (m_fastDrawAuxiliary.alreadyDrawnNextOne >= m_fastDrawAuxiliary.alreadyDrawnIndexes.size())
+    {
       THROW_EXCEPTION(
           "Have you called 'fastDrawSample' more times than the sample "
           "size? Did you forget calling 'prepareFastCall' before?");
+    }
 
     return m_fastDrawAuxiliary.alreadyDrawnIndexes[m_fastDrawAuxiliary.alreadyDrawnNextOne++];
   }
@@ -522,15 +557,24 @@ void CParticleFilterCapable::log2linearWeights(
 
   out_linWeights.resize(N);
 
-  if (!N) return;
+  if (!N)
+  {
+    return;
+  }
 
   double sumW = 0;
   size_t i;
-  for (i = 0; i < N; i++) sumW += out_linWeights[i] = exp(in_logWeights[i]);
+  for (i = 0; i < N; i++)
+  {
+    sumW += out_linWeights[i] = exp(in_logWeights[i]);
+  }
 
   ASSERT_(sumW > 0);
 
-  for (i = 0; i < N; i++) out_linWeights[i] /= sumW;
+  for (i = 0; i < N; i++)
+  {
+    out_linWeights[i] /= sumW;
+  }
 
   MRPT_END
 }