test_pf.cpp
Go to the documentation of this file.
00001 /*
00002  * Copyright (c) 2016-2017, the mcl_3dl authors
00003  * All rights reserved.
00004  *
00005  * Redistribution and use in source and binary forms, with or without
00006  * modification, are permitted provided that the following conditions are met:
00007  *
00008  *     * Redistributions of source code must retain the above copyright
00009  *       notice, this list of conditions and the following disclaimer.
00010  *     * Redistributions in binary form must reproduce the above copyright
00011  *       notice, this list of conditions and the following disclaimer in the
00012  *       documentation and/or other materials provided with the distribution.
00013  *     * Neither the name of the copyright holder nor the names of its 
00014  *       contributors may be used to endorse or promote products derived from 
00015  *       this software without specific prior written permission.
00016  *
00017  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00018  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00019  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00020  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00021  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00022  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00023  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00024  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00025  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00026  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00027  * POSSIBILITY OF SUCH DAMAGE.
00028  */
00029 
00030 #include <cstddef>
00031 #include <vector>
00032 
00033 #include <gtest/gtest.h>
00034 
00035 #include <mcl_3dl/pf.h>
00036 #include <mcl_3dl/nd.h>
00037 
00038 class State : public mcl_3dl::pf::ParticleBase<float>
00039 {
00040 public:
00041   float x;
00042   float& operator[](const size_t i)override
00043   {
00044     switch (i)
00045     {
00046       case 0:
00047         return x;
00048     }
00049     return x;
00050   }
00051   const float& operator[](const size_t i) const
00052   {
00053     switch (i)
00054     {
00055       case 0:
00056         return x;
00057     }
00058     return x;
00059   }
00060   size_t size() const override
00061   {
00062     return 1;
00063   };
00064   explicit State(const float x)
00065   {
00066     this->x = x;
00067   }
00068   State()
00069   {
00070     x = 0;
00071   }
00072   void normalize()
00073   {
00074   }
00075 };
00076 
00077 TEST(Pf, BayesianEstimation)
00078 {
00079   mcl_3dl::pf::ParticleFilter<State, float> pf(1024);
00080   const float center_list[] =
00081       {
00082         10.0, 11.0, 9.5
00083       };
00084 
00085   const float abs_error = 2e-1;
00086   const float sigma = 1.0;
00087   const float sigma2 = 2.0;
00088 
00089   for (auto center : center_list)
00090   {
00091     for (auto center2 : center_list)
00092     {
00093       pf.init(
00094           State(center),
00095           State(sigma));
00096 
00097       ASSERT_NEAR(center, pf.expectation()[0], abs_error);
00098       ASSERT_NEAR(sigma, pf.covariance()[0][0], abs_error);
00099 
00100       auto likelihood = [center2, sigma2](const State& s) -> float
00101       {
00102         return exp(-powf(s[0] - center2, 2.0) / (2.0 * powf(sigma2, 2.0)));
00103       };
00104       pf.measure(likelihood);
00105 
00106       // Numerical representation
00107       const int HISTOGRAM_SIZE = 4000;
00108       const float HISTOGRAM_RESOLUTION = 0.02;
00109 
00110       float dist[HISTOGRAM_SIZE];
00111       mcl_3dl::NormalLikelihood<float> nd1(sigma);
00112       mcl_3dl::NormalLikelihood<float> nd2(sigma2);
00113       double avg = 0;
00114       float total = 0;
00115       for (int i = 0; i < HISTOGRAM_SIZE; i++)
00116       {
00117         const float x = (i - HISTOGRAM_SIZE / 2.0) * HISTOGRAM_RESOLUTION;
00118         dist[i] = nd1(x - center) * nd2(x - center2);
00119 
00120         avg += dist[i] * x;
00121         total += dist[i];
00122       }
00123       avg /= total;
00124       double var = 0;
00125       for (int i = 0; i < HISTOGRAM_SIZE; i++)
00126       {
00127         const float x = (i - HISTOGRAM_SIZE / 2.0) * HISTOGRAM_RESOLUTION - avg;
00128         var += powf(x, 2.0) * dist[i];
00129       }
00130       var /= total;
00131 
00132       // Compare with numerical result
00133       ASSERT_NEAR(avg, pf.expectation()[0], abs_error);
00134       ASSERT_NEAR(var, pf.covariance()[0][0], abs_error);
00135 
00136       // Try resampling
00137       pf.resample(State(0.0));
00138       ASSERT_NEAR(avg, pf.expectation()[0], abs_error);
00139       ASSERT_NEAR(var, pf.covariance()[0][0], abs_error);
00140     }
00141   }
00142 }
00143 
00144 TEST(Pf, VariableParticleSize)
00145 {
00146   const size_t size_num = 3;
00147   const size_t size[size_num] =
00148       {
00149         1024, 2048, 900
00150       };
00151   mcl_3dl::pf::ParticleFilter<State, float> pf(size[0]);
00152 
00153   const float center = 12.3;
00154   const float sigma = 0.45;
00155   pf.init(
00156       State(center),
00157       State(sigma));
00158 
00159   for (size_t i = 0; i < size_num; ++i)
00160   {
00161     ASSERT_EQ(pf.getParticleSize(), size[i]);
00162 
00163     const State e = pf.expectation();
00164     const std::vector<State> v = pf.covariance();
00165     ASSERT_LT(fabs(e[0] - center), 1e-1);
00166     ASSERT_LT(fabs(sqrtf(v[0][0]) - sigma), 1e-1);
00167 
00168     pf.resample(State());
00169 
00170     const State e_r = pf.expectation();
00171     const std::vector<State> v_r = pf.covariance();
00172     ASSERT_LT(fabs(e_r[0] - center), 1e-1);
00173     ASSERT_LT(fabs(sqrtf(v_r[0][0]) - sigma), 1e-1);
00174 
00175     if (i + 1 != size_num)
00176     {
00177       pf.resizeParticle(size[i + 1]);
00178     }
00179   }
00180 }
00181 
00182 TEST(Pf, ResampleFlatLikelihood)
00183 {
00184   mcl_3dl::pf::ParticleFilter<State, float> pf(10);
00185   const float center = 12.3;
00186   const float sigma = 0.45;
00187   pf.init(
00188       State(center),
00189       State(sigma));
00190 
00191   std::vector<float> orig;
00192 
00193   for (size_t i = 0; i < pf.getParticleSize(); ++i)
00194     orig.push_back(pf.getParticle(i)[0]);
00195 
00196   pf.resample(State());
00197 
00198   for (size_t i = 0; i < pf.getParticleSize(); ++i)
00199     ASSERT_EQ(pf.getParticle(i)[0], orig[i]);
00200 }
00201 
00202 TEST(Pf, Iterators)
00203 {
00204   mcl_3dl::pf::ParticleFilter<State, float> pf(10);
00205   const float val0 = 12.3;
00206   const float val1 = 45.6;
00207   pf.init(
00208       State(val0),
00209       State(0.0));
00210 
00211   for (auto it = pf.begin(); it != pf.end(); ++it)
00212   {
00213     ASSERT_EQ(it->state_[0], val0);
00214     it->state_[0] = val1;
00215   }
00216   for (auto it = pf.begin(); it != pf.end(); ++it)
00217     ASSERT_EQ(it->state_[0], val1);
00218 }
00219 
00220 TEST(Pf, AppendParticles)
00221 {
00222   mcl_3dl::pf::ParticleFilter<State, float> pf(10);
00223   const float val0 = 12.3;
00224   const float val1 = 45.6;
00225   pf.init(
00226       State(val0),
00227       State(0.0));
00228   // particles 0-9 has val0
00229 
00230   for (auto it = pf.appendParticle(10); it != pf.end(); ++it)
00231     it->state_[0] = val1;
00232   // appended particles 10-19 has val1
00233 
00234   ASSERT_EQ(pf.getParticleSize(), 20u);
00235   for (size_t i = 0; i < 10; ++i)
00236     ASSERT_EQ(pf.getParticle(i)[0], val0);
00237   for (size_t i = 10; i < 20; ++i)
00238     ASSERT_EQ(pf.getParticle(i)[0], val1);
00239 }
00240 
00241 int main(int argc, char** argv)
00242 {
00243   testing::InitGoogleTest(&argc, argv);
00244 
00245   return RUN_ALL_TESTS();
00246 }


mcl_3dl
Author(s): Atsushi Watanabe
autogenerated on Thu Jun 20 2019 20:04:43