Enable particle initialization using covariances (#259)

* Enable particle initialization using covariances

* Convert standard deviations to variances in tests

include/mcl_3dl/noise_generator_base.h

@@ -0,0 +1,70 @@
+/*
+ * Copyright (c) 2019, the mcl_3dl authors
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ *     * Redistributions of source code must retain the above copyright
+ *       notice, this list of conditions and the following disclaimer.
+ *     * Redistributions in binary form must reproduce the above copyright
+ *       notice, this list of conditions and the following disclaimer in the
+ *       documentation and/or other materials provided with the distribution.
+ *     * Neither the name of the copyright holder nor the names of its
+ *       contributors may be used to endorse or promote products derived from
+ *       this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#ifndef MCL_3DL_NOISE_GENERATOR_BASE_H
+#define MCL_3DL_NOISE_GENERATOR_BASE_H
+
+#include <vector>
+
+namespace mcl_3dl
+{
+template <typename FLT_TYPE>
+class NoiseGeneratorBase
+{
+public:
+  virtual ~NoiseGeneratorBase()
+  {
+  }
+
+  template <typename T>
+  void setMean(const T& mean)
+  {
+    mean_.resize(mean.size());
+    for (size_t i = 0; i < mean.size(); ++i)
+    {
+      mean_[i] = mean[i];
+    }
+  }
+  const std::vector<FLT_TYPE>& getMean() const
+  {
+    return mean_;
+  }
+  size_t getDimension() const
+  {
+    return mean_.size();
+  }
+
+protected:
+  std::vector<FLT_TYPE> mean_;
+};
+
+
+}  // namespace mcl_3dl
+
+#endif  // MCL_3DL_NOISE_GENERATOR_BASE_H

include/mcl_3dl/noise_generators/diagonal_noise_generator.h

@@ -0,0 +1,81 @@
+/*
+ * Copyright (c) 2019, the mcl_3dl authors
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ *     * Redistributions of source code must retain the above copyright
+ *       notice, this list of conditions and the following disclaimer.
+ *     * Redistributions in binary form must reproduce the above copyright
+ *       notice, this list of conditions and the following disclaimer in the
+ *       documentation and/or other materials provided with the distribution.
+ *     * Neither the name of the copyright holder nor the names of its
+ *       contributors may be used to endorse or promote products derived from
+ *       this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#ifndef MCL_3DL_NOISE_GENERATORS_DIAGONAL_NOISE_GENERATOR_H
+#define MCL_3DL_NOISE_GENERATORS_DIAGONAL_NOISE_GENERATOR_H
+
+#include <random>
+#include <vector>
+
+#include <mcl_3dl/noise_generator_base.h>
+
+namespace mcl_3dl
+{
+template <typename FLT_TYPE>
+class DiagonalNoiseGenerator : public NoiseGeneratorBase<FLT_TYPE>
+{
+public:
+  using Parent = NoiseGeneratorBase<FLT_TYPE>;
+
+  template <typename T>
+  DiagonalNoiseGenerator(const T& mean, const T& sigma)
+  {
+    Parent::setMean(mean);
+    setSigma(sigma);
+  }
+
+  template <typename T>
+  void setSigma(const T& sigma)
+  {
+    sigma_.resize(sigma.size());
+    for (size_t i = 0; i < sigma.size(); ++i)
+    {
+      sigma_[i] = sigma[i];
+    }
+  }
+
+  template <typename RANDOM_ENGINE>
+  std::vector<FLT_TYPE> operator()(RANDOM_ENGINE& engine) const
+  {
+    std::vector<FLT_TYPE> noise(sigma_.size());
+    for (size_t i = 0; i < sigma_.size(); i++)
+    {
+      std::normal_distribution<FLT_TYPE> nd(Parent::mean_[i], sigma_[i]);
+      noise[i] = nd(engine);
+    }
+    return noise;
+  }
+
+protected:
+  std::vector<FLT_TYPE> sigma_;
+};
+
+}  // namespace mcl_3dl
+
+#endif  // MCL_3DL_NOISE_GENERATORS_DIAGONAL_NOISE_GENERATOR_H

include/mcl_3dl/noise_generators/multivariate_noise_generator.h

@@ -0,0 +1,100 @@
+/*
+ * Copyright (c) 2019, the mcl_3dl authors
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ *     * Redistributions of source code must retain the above copyright
+ *       notice, this list of conditions and the following disclaimer.
+ *     * Redistributions in binary form must reproduce the above copyright
+ *       notice, this list of conditions and the following disclaimer in the
+ *       documentation and/or other materials provided with the distribution.
+ *     * Neither the name of the copyright holder nor the names of its
+ *       contributors may be used to endorse or promote products derived from
+ *       this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#ifndef MCL_3DL_NOISE_GENERATORS_MULTIVARIATE_NOISE_GENERATOR_H
+#define MCL_3DL_NOISE_GENERATORS_MULTIVARIATE_NOISE_GENERATOR_H
+
+#include <random>
+#include <vector>
+
+#include <Eigen/Core>
+#include <Eigen/Eigenvalues>
+
+#include <mcl_3dl/noise_generator_base.h>
+
+namespace mcl_3dl
+{
+template <typename FLT_TYPE>
+class MultivariateNoiseGenerator : public NoiseGeneratorBase<FLT_TYPE>
+{
+public:
+  using Parent = NoiseGeneratorBase<FLT_TYPE>;
+  using Matrix = Eigen::Matrix<FLT_TYPE, Eigen::Dynamic, Eigen::Dynamic>;
+  using Vector = Eigen::Matrix<FLT_TYPE, Eigen::Dynamic, 1>;
+
+  template <typename MEAN_TYPE, typename COV_TYPE>
+  explicit MultivariateNoiseGenerator(const MEAN_TYPE& mean, const COV_TYPE& covariance)
+  {
+    Parent::setMean(mean);
+    mean_vec_.resize(Parent::mean_.size());
+    for (int i = 0; i < mean_vec_.size(); ++i)
+    {
+      mean_vec_[i] = Parent::mean_[i];
+    }
+    setCovariance(covariance);
+  }
+
+  template <typename COV_TYPE>
+  void setCovariance(const COV_TYPE& covariance)
+  {
+    const size_t dim = Parent::getDimension();
+    Matrix cov_matrix(dim, dim);
+    for (size_t i = 0; i < dim; ++i)
+    {
+      for (size_t j = 0; j < dim; ++j)
+      {
+        cov_matrix(i, j) = covariance[j + i * dim];
+      }
+    }
+    const Eigen::SelfAdjointEigenSolver<Matrix> eigen_solver(cov_matrix);
+    norm_transform_ = eigen_solver.eigenvectors() * eigen_solver.eigenvalues().cwiseSqrt().asDiagonal();
+  }
+
+  template <typename RANDOM_ENGINE>
+  std::vector<FLT_TYPE> operator()(RANDOM_ENGINE& engine) const
+  {
+    const size_t dim = Parent::getDimension();
+    Vector random(dim);
+    std::normal_distribution<FLT_TYPE> nd(0.0, 1.0);
+    for (size_t j = 0; j < dim; ++j)
+    {
+      random[j] = nd(engine);
+    }
+    const Vector sample_noise = mean_vec_ + norm_transform_ * random;
+    return std::vector<FLT_TYPE>(sample_noise.data(), sample_noise.data() + dim);
+  }
+
+protected:
+  Vector mean_vec_;
+  Matrix norm_transform_;
+};
+
+}  // namespace mcl_3dl
+
+#endif  // MCL_3DL_NOISE_GENERATORS_MULTIVARIATE_NOISE_GENERATOR_H

include/mcl_3dl/pf.h

@@ -30,11 +30,13 @@
 #ifndef MCL_3DL_PF_H
 #define MCL_3DL_PF_H
 
-#include <random>
-#include <vector>
 #include <algorithm>
-#include <functional>
 #include <cmath>
+#include <functional>
+#include <random>
+#include <vector>
+
+#include <mcl_3dl/noise_generators/diagonal_noise_generator.h>
 
 namespace mcl_3dl
 {
@@ -65,21 +67,18 @@ class ParticleBase
     T exp = e;
     return ((*this)[k] - exp[k]) * ((*this)[j] - exp[j]);
   }
-  template <typename T>
-  static T generateNoise(
-      std::default_random_engine& engine_,
-      T mean, T sigma)
+
+  template <typename T, typename RANDOM_ENGINE, typename NOISE_GEN>
+  static T generateNoise(RANDOM_ENGINE& engine, const NOISE_GEN& gen)
   {
+    const auto org_noise = gen(engine);
     T noise;
     for (size_t i = 0; i < noise.size(); i++)
     {
-      std::normal_distribution<FLT_TYPE> nd(mean[i], sigma[i]);
-      noise[i] = nd(engine_);
+      noise[i] = org_noise[i];
     }
     return noise;
   }
-
-protected:
 };
 
 template <typename T, typename FLT_TYPE = float>
@@ -151,7 +150,8 @@ class ParticleWeightedMean
   }
 };
 
-template <typename T, typename FLT_TYPE = float, typename MEAN = ParticleWeightedMean<T, FLT_TYPE>>
+template <typename T, typename FLT_TYPE = float, typename MEAN = ParticleWeightedMean<T, FLT_TYPE>,
+          typename RANDOM_ENGINE = std::default_random_engine>
 class ParticleFilter
 {
 public:
@@ -161,14 +161,24 @@ class ParticleFilter
     particles_.resize(num_particles);
   }
   void init(T mean, T sigma)
+  {
+    return initUsingNoiseGenerator(DiagonalNoiseGenerator<FLT_TYPE>(mean, sigma));
+  }
+  template <typename GEN>
+  void initUsingNoiseGenerator(const GEN& generator)
   {
     for (auto& p : particles_)
     {
-      p.state_ = T::generateNoise(engine_, mean, sigma);
+      p.state_ = T::template generateNoise<T>(engine_, generator);
       p.probability_ = 1.0 / particles_.size();
     }
   }
   void resample(T sigma)
+  {
+    resampleUsingNoiseGenerator(DiagonalNoiseGenerator<FLT_TYPE>(T(), sigma));
+  }
+  template <typename GEN>
+  void resampleUsingNoiseGenerator(const GEN& generator)
   {
     FLT_TYPE accum = 0;
     for (auto& p : particles_)
@@ -197,7 +207,7 @@ class ParticleFilter
       }
       else if (it == it_prev)
       {
-        p.state_ = it->state_ + T::generateNoise(engine_, T(), sigma);
+        p.state_ = it->state_ + T::template generateNoise<T>(engine_, generator);
         p.state_.normalize();
       }
       else
@@ -208,10 +218,15 @@ class ParticleFilter
     }
   }
   void noise(T sigma)
+  {
+    addNoiseUsingNoiseGenerator(DiagonalNoiseGenerator<FLT_TYPE>(T(), sigma));
+  }
+  template <typename GEN>
+  void addNoiseUsingNoiseGenerator(const GEN& generator)
   {
     for (auto& p : particles_)
     {
-      p.state_ = p.state_ + T::generateNoise(engine_, T(), sigma);
+      p.state_ = p.state_ + T::template generateNoise<T>(engine_, generator);
     }
   }
   void predict(std::function<void(T&)> model)
@@ -425,7 +440,7 @@ class ParticleFilter
   std::vector<Particle<T, FLT_TYPE>> particles_;
   std::vector<Particle<T, FLT_TYPE>> particles_dup_;
   std::random_device seed_gen_;
-  std::default_random_engine engine_;
+  RANDOM_ENGINE engine_;
   T ie_;
 };