First commit

Author: xkaraman

CMakeLists.txt

@@ -0,0 +1,96 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(amcl_lib_pf)
+
+# Build Particle Filter Library REFERENCED AS PF
+add_subdirectory(include/libPF)
+
+## Compile as C++11, supported in ROS Kinetic and newer
+add_compile_options(-std=c++11)
+
+## Find catkin macros and libraries
+## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
+## is used, also find other catkin packages
+set(PACKAGE_DEPENDENCIES
+  roscpp
+  nav_msgs
+  sensor_msgs
+  std_msgs
+  octomap_ros
+  octomap_msgs
+  pcl_conversions
+  pcl_ros
+  cmake_modules
+  tf2_geometry_msgs
+  tf2
+  tf2_ros
+)
+
+find_package(catkin REQUIRED COMPONENTS ${PACKAGE_DEPENDENCIES} )
+
+find_package(OCTOMAP REQUIRED)
+include_directories(${OCTOMAP_INCLUDE_DIRS})
+
+find_package(Eigen3 REQUIRED)
+
+find_package(PCL 1.7 REQUIRED)
+include_directories(${PCL_INCLUDE_DIRS})
+link_directories(${PCL_LIBRARY_DIRS})
+add_definitions(${PCL_DEFINITIONS})
+
+## System dependencies are found with CMake's conventions
+find_package(Boost REQUIRED COMPONENTS system)
+
+
+catkin_package(
+#  INCLUDE_DIRS include
+#  LIBRARIES amcl_lib_pf
+#  CATKIN_DEPENDS other_catkin_pkg
+#  DEPENDS system_lib
+)
+
+###########
+## Build ##
+###########
+
+## Specify additional locations of header files
+## Your package locations should be listed before other locations
+include_directories(
+ include
+ ${catkin_INCLUDE_DIRS}
+ src include/libPF/include
+)
+
+set(amcl_lib_pf_SOURCES 
+	src/amcl_lib_pf_node.cpp
+	src/RobotState.cpp
+    src/AMCLDepth.cpp
+    src/MapModel.cpp
+    src/RobotMovementModel.cpp
+    src/RobotObservationModel.cpp
+    src/RobotStateDistribution.cpp
+    )
+
+
+## Declare a C++ library
+# add_library(${PROJECT_NAME}
+#   src/${PROJECT_NAME}/amcl_lib_pf.cpp
+# )
+
+## Add cmake target dependencies of the library
+## as an example, code may need to be generated before libraries
+## either from message generation or dynamic reconfigure
+# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
+## Declare a C++ executable
+## With catkin_make all packages are built within a single CMake context
+## The recommended prefix ensures that target names across packages don't collide
+ add_executable(${PROJECT_NAME}_node ${amcl_lib_pf_SOURCES})
+
+## Specify libraries to link a library or executable target against
+target_link_libraries(${PROJECT_NAME}_node  
+   ${catkin_LIBRARIES}
+   ${PCL_LIBRARIES}
+   PF
+ )
+
+

CMakeLists.txt.backup

@@ -0,0 +1,19 @@
+cmake_minimum_required(VERSION 3.5)
+project(libpfsample2)
+
+add_subdirectory(include/libPF)
+
+set(sample2_SOURCES src/main.cpp
+    src/CarMovementModel.cpp
+    src/CarObservationModel.cpp
+    src/CarState.cpp
+    src/CarStateDistribution.cpp
+    )
+
+include_directories(${CMAKE_CURRENT_BINARY_DIR})
+include_directories(src include/libPF/include)
+
+add_executable(sample2 ${sample2_SOURCES} )
+
+target_link_libraries(sample2 PF)
+

include/libPF/CMakeLists.txt

@@ -0,0 +1,7 @@
+project(libPF)
+cmake_minimum_required(VERSION 2.8)
+
+include_directories(include)
+
+add_library(PF 
+src/CRandomNumberGenerator.cpp)

include/libPF/include/libPF/CRandomNumberGenerator.h

@@ -0,0 +1,69 @@
+#ifndef RANDOMNUMBERGENERATOR_H
+#define RANDOMNUMBERGENERATOR_H
+
+#include "libPF/RandomNumberGenerationStrategy.h"
+
+namespace libPF
+{
+
+/**
+ * @class CRandomNumberGenerator
+ *
+ * @brief Class for the generation of random numbers.
+ *
+ * This class can generate randomly generated numbers from uniform and
+ * gaussian distributions.
+ * Note: this is a very simple PRNG, using the C-function rand().
+ *
+ * @author Stephan Wirth
+ */
+class CRandomNumberGenerator : public RandomNumberGenerationStrategy {
+
+  public:
+
+    /**
+     * The constructor calls init().
+     */
+    CRandomNumberGenerator();
+
+    /**
+     * Empty destructor.
+     */
+    ~CRandomNumberGenerator();
+
+    /**
+     * This method creates gaussian distributed random numbers (Box-Müller method).
+     * @param standardDeviation Standard deviation d of the random number to generate.
+     * @return N(0, d*d)-distributed random number
+     */
+    double getGaussian(double standardDeviation) const;
+
+    /**
+     * Generates a uniform distributed random number between min and max.
+     * @param min the minimum value, default is 0.0
+     * @param max the maximum value, default is 1.0
+     * @return random number between min and max, uniform distributed.
+     */
+    double getUniform(double min = 0.0, double max = 1.0) const;
+
+  protected:
+
+    /**
+     * Initializes the seed by calling srand(time(0))
+     */
+    void init();
+
+  private:
+
+    /// stores if there is a buffered gaussian variable or not
+    mutable bool m_GaussianBufferFilled;
+
+    /// buffer for a gaussian distributed variable, the Box-Müller method always
+    /// creates two variables, we return one and store the other here.
+    mutable double m_GaussianBufferVariable;
+
+};
+
+} // end of namespace
+
+#endif // RANDOMNUMBERGENERATOR_H

include/libPF/include/libPF/CompareParticleWeights.h

@@ -0,0 +1,42 @@
+#ifndef COMPAREPARTICLEWEIGHTS_H
+#define COMPAREPARTICLEWEIGHTS_H  
+
+#include "libPF/Particle.h"
+
+namespace libPF
+{
+
+/** 
+ * @class CompareParticleWeights
+ *
+ * @author Stephan Wirth
+ *
+ * @brief Class with one operator to compare two pointers of particles according
+ *        to the weight of the particles.
+ *
+ * With this class as compare function, std::sort() can be used on arrays of
+ * pointers to Particle. After sorting the array with this function, the particle
+ * with the smallest weight will be at the last position.
+ * 
+ * @see Particle
+ */
+template <class StateType>
+class CompareParticleWeights {
+
+  public:
+
+    /**
+     * @return true if the weight of the particle p1 is higher than the weight of particle p2.
+     */
+    bool operator() (const libPF::Particle<StateType>* p1,
+                     const libPF::Particle<StateType>* p2) const
+    {
+      return p1->getWeight() > p2->getWeight();
+    }
+};
+
+} // end of namespace
+
+#endif
+
+

include/libPF/include/libPF/ImportanceResampling.h

@@ -0,0 +1,108 @@
+#ifndef IMPORTANCERESAMPLING_H
+#define IMPORTANCERESAMPLING_H
+
+#include "libPF/CRandomNumberGenerator.h"
+
+namespace libPF
+{
+
+/** 
+ * @class ImportanceResampling
+ *
+ * @brief A resampling strategy that performs importance resampling
+ *
+ * The resampling strategy defines how the resampling is performed in the resample step
+ * of a particle filter.
+ * 
+ * @author Stephan Wirth
+ *
+ * @see ResamplingStrategy
+ */
+
+template <class StateType>
+class ImportanceResampling : public ResamplingStrategy<StateType>{
+
+    /**
+     * A ParticleList is an array of pointers to Particles.
+     */
+    typedef std::vector< Particle<StateType>* > ParticleList;
+    
+  public:
+    /** 
+     * The constructor of this base class inits some members.
+     */
+    ImportanceResampling<StateType>();
+
+    /**
+     * The destructor is empty.
+     */
+    virtual ~ImportanceResampling();
+
+    /**
+     * This is the main method of ImportanceResampling. It takes two references to
+     * particle lists. The first reference refers to the old particle list, the
+     * second to the new one.
+     * @param source the source list to draw new particles from.
+     * @param destination the destination list where to put the copies.
+     */
+    void resample(const ParticleList& source, const ParticleList& destination) const;
+
+    /**
+     * Sets the Random Number Generator to use in resample() to generate uniformly distributed random numbers.
+     */
+    void setRNG(RandomNumberGenerationStrategy* rng);
+
+  private:
+
+    // Stores a pointer to the random number generator.
+    const RandomNumberGenerationStrategy* m_RNG;
+
+    // The default random number generator
+    CRandomNumberGenerator m_DefaultRNG;
+
+};
+
+
+template <class StateType>
+ImportanceResampling<StateType>::ImportanceResampling() :
+    m_RNG(&m_DefaultRNG) {
+}
+
+template <class StateType>
+ImportanceResampling<StateType>::~ImportanceResampling() {
+}
+
+
+// resampling based on the cumulative distribution function (CDF)
+template <class StateType>
+void ImportanceResampling<StateType>::resample(const ParticleList& sourceList, const ParticleList& destinationList) const {
+
+  double inverseNum = 1.0f / sourceList.size();
+  double start = m_RNG->getUniform() * inverseNum;  // random start in CDF
+  double cumulativeWeight = 0.0f;
+  unsigned int sourceIndex = 0;                     // index to draw from
+  cumulativeWeight += sourceList[sourceIndex]->getWeight();
+  for (unsigned int destIndex = 0; destIndex < destinationList.size(); destIndex++) {
+    double probSum = start + inverseNum * destIndex;     // amount of cumulative weight to reach
+    while (probSum > cumulativeWeight) {                 // sum weights until
+      sourceIndex++;
+      if (sourceIndex >= sourceList.size()) {
+        sourceIndex = sourceList.size() - 1;
+        break;
+      }
+      cumulativeWeight += sourceList[sourceIndex]->getWeight(); // target sum reached
+    }
+    *(destinationList[destIndex]) = *(sourceList[sourceIndex]);  // copy particle (via assignment operator)
+  }
+}
+
+
+template <class StateType>
+void ImportanceResampling<StateType>::setRNG(RandomNumberGenerationStrategy* rng)
+{
+    m_RNG = rng;
+}
+
+} // end of namespace
+#endif // IMPORTANCERESAMPLING_H
+