test_nd.cpp
Go to the documentation of this file.
1 /*
2  * Copyright (c) 2016-2017, the mcl_3dl authors
3  * All rights reserved.
4  *
5  * Redistribution and use in source and binary forms, with or without
6  * modification, are permitted provided that the following conditions are met:
7  *
8  * * Redistributions of source code must retain the above copyright
9  * notice, this list of conditions and the following disclaimer.
10  * * Redistributions in binary form must reproduce the above copyright
11  * notice, this list of conditions and the following disclaimer in the
12  * documentation and/or other materials provided with the distribution.
13  * * Neither the name of the copyright holder nor the names of its
14  * contributors may be used to endorse or promote products derived from
15  * this software without specific prior written permission.
16  *
17  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
18  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
19  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
20  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
21  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
22  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
23  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
24  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
25  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
26  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
27  * POSSIBILITY OF SUCH DAMAGE.
28  */
29 
30 #include <cmath>
31 #include <cstddef>
32 
33 #include <Eigen/Geometry>
34 
35 #include <gtest/gtest.h>
36 
37 #include <mcl_3dl/nd.h>
38 
39 TEST(NormalLiklihood, Normality)
40 {
41  for (double sigma = 1.0; sigma <= 3.0; sigma += 1.0)
42  {
43  // Check distribution
45  const double likelihood0 = 1.0 / std::sqrt(M_PI * 2.0 * sigma * sigma);
46  ASSERT_NEAR(nl(0.0), likelihood0, 1e-6);
47  ASSERT_NEAR(nl(sigma), likelihood0 * 0.60653066, 1e-6);
48  ASSERT_NEAR(nl(-sigma), likelihood0 * 0.60653066, 1e-6);
49  ASSERT_NEAR(nl(3.0 * sigma), likelihood0 * 0.011108997, 1e-6);
50  ASSERT_NEAR(nl(-3.0 * sigma), likelihood0 * 0.011108997, 1e-6);
51 
52  // Check integrated value
53  double sum(0.0);
54  const double step = 0.1;
55  for (double i = -100.0; i < 100.0; i += step)
56  {
57  sum += nl(i) * step;
58  }
59  ASSERT_NEAR(sum, 1.0, 1e-6);
60  }
61 }
62 
63 TEST(NormalLiklihood, NormalityNd)
64 {
65  for (double sigma = 1.0; sigma <= 3.0; sigma += 1.0)
66  {
67  // Check distribution
68  using NormalLikelihood2d = mcl_3dl::NormalLikelihoodNd<double, 2>;
69  NormalLikelihood2d::Matrix cov;
70  cov << sigma, 0,
71  0, sigma * 2;
72  NormalLikelihood2d nl(cov);
73 
74  const double likelihood0 = 1.0 / (M_PI * 2.0 * sqrt(cov.determinant()));
75  ASSERT_NEAR(
76  nl(NormalLikelihood2d::Vector(0.0, 0.0)), likelihood0, 1e-6);
77 
78  const double e1a =
79  exp(-0.5 *
80  NormalLikelihood2d::Vector(sigma, 0.0).transpose() *
81  cov.inverse() *
82  NormalLikelihood2d::Vector(sigma, 0.0));
83  const double e1b =
84  exp(-0.5 *
85  NormalLikelihood2d::Vector(0.0, sigma).transpose() *
86  cov.inverse() *
87  NormalLikelihood2d::Vector(0.0, sigma));
88  ASSERT_NEAR(
89  nl(NormalLikelihood2d::Vector(sigma, 0.0)), likelihood0 * e1a, 1e-6);
90  ASSERT_NEAR(
91  nl(NormalLikelihood2d::Vector(-sigma, 0.0)), likelihood0 * e1a, 1e-6);
92  ASSERT_NEAR(
93  nl(NormalLikelihood2d::Vector(0.0, sigma)), likelihood0 * e1b, 1e-6);
94  ASSERT_NEAR(
95  nl(NormalLikelihood2d::Vector(0.0, -sigma)), likelihood0 * e1b, 1e-6);
96 
97  const double e2a =
98  exp(-0.5 *
99  NormalLikelihood2d::Vector(3.0 * sigma, 0.0).transpose() *
100  cov.inverse() *
101  NormalLikelihood2d::Vector(3.0 * sigma, 0.0));
102  const double e2b =
103  exp(-0.5 *
104  NormalLikelihood2d::Vector(0.0, 3.0 * sigma).transpose() *
105  cov.inverse() *
106  NormalLikelihood2d::Vector(0.0, 3.0 * sigma));
107  ASSERT_NEAR(
108  nl(NormalLikelihood2d::Vector(3.0 * sigma, 0.0)), likelihood0 * e2a, 1e-6);
109  ASSERT_NEAR(
110  nl(NormalLikelihood2d::Vector(-3.0 * sigma, 0.0)), likelihood0 * e2a, 1e-6);
111  ASSERT_NEAR(
112  nl(NormalLikelihood2d::Vector(0.0, 3.0 * sigma)), likelihood0 * e2b, 1e-6);
113  ASSERT_NEAR(
114  nl(NormalLikelihood2d::Vector(0.0, -3.0 * sigma)), likelihood0 * e2b, 1e-6);
115 
116  // Check integrated value
117  double sum(0.0);
118  const double step = 0.1;
119  const double step_sq = step * step;
120  for (double i = -100.0; i < 100.0; i += step)
121  {
122  for (double j = -100.0; j < 100.0; j += step)
123  {
124  sum += nl(NormalLikelihood2d::Vector(i, j)) * step_sq;
125  }
126  }
127  ASSERT_NEAR(sum, 1.0, 1e-6);
128  }
129  for (double sigma = 1.0; sigma <= 3.0; sigma += 1.0)
130  {
131  // Check distribution
132  using NormalLikelihood2d = mcl_3dl::NormalLikelihoodNd<double, 2>;
133  NormalLikelihood2d::Matrix cov;
134  cov << sigma, 0,
135  0, sigma;
136  NormalLikelihood2d nl(cov);
137 
138  const double likelihood0 = 1.0 / (M_PI * 2.0 * sqrt(cov.determinant()));
139  const double e1 =
140  exp(-0.5 *
141  NormalLikelihood2d::Vector(sigma, 0.0).transpose() *
142  cov.inverse() *
143  NormalLikelihood2d::Vector(sigma, 0.0));
144  const double e2 =
145  exp(-0.5 *
146  NormalLikelihood2d::Vector(3.0 * sigma, 0.0).transpose() *
147  cov.inverse() *
148  NormalLikelihood2d::Vector(3.0 * sigma, 0.0));
149  for (double r = 0; r < M_PI * 2; r += M_PI / 6)
150  {
151  const auto v1 =
152  Eigen::Rotation2D<double>(r) *
153  NormalLikelihood2d::Vector(sigma, 0.0);
154  ASSERT_NEAR(nl(v1), likelihood0 * e1, 1e-6);
155  const auto v2 =
156  Eigen::Rotation2D<double>(r) *
157  NormalLikelihood2d::Vector(3.0 * sigma, 0.0);
158  ASSERT_NEAR(nl(v2), likelihood0 * e2, 1e-6);
159  }
160  }
161 }
162 
163 int main(int argc, char** argv)
164 {
165  testing::InitGoogleTest(&argc, argv);
166 
167  return RUN_ALL_TESTS();
168 }
int main(int argc, char **argv)
Definition: test_nd.cpp:163
INLINE Rall1d< T, V, S > sqrt(const Rall1d< T, V, S > &arg)
INLINE Rall1d< T, V, S > exp(const Rall1d< T, V, S > &arg)
unsigned int step
TEST(NormalLiklihood, Normality)
Definition: test_nd.cpp:39


mcl_3dl
Author(s): Atsushi Watanabe
autogenerated on Wed May 12 2021 02:16:29