1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
|
/* +------------------------------------------------------------------------+
| Mobile Robot Programming Toolkit (MRPT) |
| https://www.mrpt.org/ |
| |
| Copyright (c) 2005-2023, Individual contributors, see AUTHORS file |
| See: https://www.mrpt.org/Authors - All rights reserved. |
| Released under BSD License. See: https://www.mrpt.org/License |
+------------------------------------------------------------------------+ */
#include <mrpt/bayes.h>
#include <mrpt/poses/CPoint2D.h>
#include <mrpt/poses/CPose2D.h>
#include <mrpt/random.h>
#include <mrpt/system/CTicTac.h>
#include <mrpt/system/os.h>
#include <iostream>
using namespace mrpt;
using namespace mrpt::bayes;
using namespace mrpt::poses;
using namespace mrpt::random;
using namespace mrpt::system;
double SIGMA = 0.05;
// The custom class:
class CMyRejectionSampling : public CRejectionSamplingCapable<CPose2D>
{
protected:
void RS_drawFromProposal(CPose2D& outSample) override
{
double ang = getRandomGenerator().drawUniform(-M_PI, M_PI);
double R = getRandomGenerator().drawGaussian1D(1.0, SIGMA);
outSample.x(1.0f - cos(ang) * R);
outSample.y(sin(ang) * R);
outSample.phi(getRandomGenerator().drawUniform(-M_PI, M_PI));
}
/** Returns the NORMALIZED observation likelihood at a given point of the
* state space (values in the range [0,1]).
*/
double RS_observationLikelihood(const CPose2D& x) override
{
return exp(
-0.5 * square((x.distanceTo(CPoint2D(0, 0)) - 1.0f) / SIGMA));
}
};
// ------------------------------------------------------
// TestRS
// ------------------------------------------------------
void TestRS()
{
CMyRejectionSampling RS;
std::vector<CMyRejectionSampling::TParticle> samples;
CTicTac tictac;
tictac.Tic();
printf("Computing...");
RS.rejectionSampling(1000, samples);
printf("Ok! %fms\n", 1000 * tictac.Tac());
FILE* f = os::fopen("_out_samples.txt", "wt");
std::vector<CMyRejectionSampling::TParticle>::iterator it;
for (it = samples.begin(); it != samples.end(); it++)
os::fprintf(
f, "%f %f %f %e\n", it->d->x(), it->d->y(), it->d->phi(),
it->log_w);
os::fclose(f);
}
// ------------------------------------------------------
// MAIN
// ------------------------------------------------------
int main()
{
try
{
TestRS();
return 0;
}
catch (const std::exception& e)
{
std::cerr << "MRPT error: " << mrpt::exception_to_str(e) << std::endl;
return -1;
}
catch (...)
{
printf("Untyped excepcion!!");
return -1;
}
}
|
