MRPT  2.0.4
test.cpp
Go to the documentation of this file.
1 /* +------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | https://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2020, Individual contributors, see AUTHORS file |
6  | See: https://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See: https://www.mrpt.org/License |
8  +------------------------------------------------------------------------+ */
9 
10 #include <mrpt/bayes.h>
11 #include <mrpt/poses/CPoint2D.h>
12 #include <mrpt/poses/CPose2D.h>
13 #include <mrpt/random.h>
14 #include <mrpt/system/CTicTac.h>
15 #include <mrpt/system/os.h>
16 #include <iostream>
17 
18 using namespace mrpt;
19 using namespace mrpt::bayes;
20 using namespace mrpt::poses;
21 using namespace mrpt::random;
22 using namespace mrpt::system;
23 
24 double SIGMA = 0.05;
25 
26 // The custom class:
27 class CMyRejectionSampling : public CRejectionSamplingCapable<CPose2D>
28 {
29  protected:
30  void RS_drawFromProposal(CPose2D& outSample) override
31  {
32  double ang = getRandomGenerator().drawUniform(-M_PI, M_PI);
33  double R = getRandomGenerator().drawGaussian1D(1.0, SIGMA);
34  outSample.x(1.0f - cos(ang) * R);
35  outSample.y(sin(ang) * R);
36  outSample.phi(getRandomGenerator().drawUniform(-M_PI, M_PI));
37  }
38 
39  /** Returns the NORMALIZED observation likelihood at a given point of the
40  * state space (values in the range [0,1]).
41  */
42  double RS_observationLikelihood(const CPose2D& x) override
43  {
44  return exp(
45  -0.5 * square((x.distanceTo(CPoint2D(0, 0)) - 1.0f) / SIGMA));
46  }
47 };
48 
49 // ------------------------------------------------------
50 // TestRS
51 // ------------------------------------------------------
52 void TestRS()
53 {
55  std::vector<CMyRejectionSampling::TParticle> samples;
56  CTicTac tictac;
57 
58  tictac.Tic();
59  printf("Computing...");
60 
61  RS.rejectionSampling(1000, samples);
62 
63  printf("Ok! %fms\n", 1000 * tictac.Tac());
64 
65  FILE* f = os::fopen("_out_samples.txt", "wt");
66  std::vector<CMyRejectionSampling::TParticle>::iterator it;
67  for (it = samples.begin(); it != samples.end(); it++)
69  f, "%f %f %f %e\n", it->d->x(), it->d->y(), it->d->phi(),
70  it->log_w);
71 
72  os::fclose(f);
73 }
74 
75 // ------------------------------------------------------
76 // MAIN
77 // ------------------------------------------------------
78 int main()
79 {
80  try
81  {
82  TestRS();
83 
84  return 0;
85  }
86  catch (const std::exception& e)
87  {
88  std::cerr << "MRPT error: " << mrpt::exception_to_str(e) << std::endl;
89  return -1;
90  }
91  catch (...)
92  {
93  printf("Untyped excepcion!!");
94  return -1;
95  }
96 }
os.h
CPoint2D.h
mrpt::system::os::fclose
int void fclose(FILE *f)
An OS-independent version of fclose.
Definition: os.cpp:286
mrpt::system::CTicTac
A high-performance stopwatch, with typical resolution of nanoseconds.
Definition: system/CTicTac.h:17
mrpt::poses::CPose2D::phi
double phi() const
Get the phi angle of the 2D pose (in radians)
Definition: CPose2D.h:86
CMyRejectionSampling
Definition: vision_stereo_rectify/test.cpp:27
mrpt::poses::CPoseOrPoint::x
double x() const
Common members of all points & poses classes.
Definition: CPoseOrPoint.h:143
mrpt::bayes::CRejectionSamplingCapable
A base class for implementing rejection sampling in a generic state space.
Definition: CRejectionSamplingCapable.h:29
SIGMA
double SIGMA
Definition: vision_stereo_rectify/test.cpp:24
mrpt
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
Definition: BaseAppDataSource.h:15
R
const float R
Definition: CKinematicChain.cpp:137
mrpt::poses
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
Definition: CHierarchicalMapMHPartition.h:22
mrpt::poses::CPoseOrPoint::distanceTo
double distanceTo(const CPoseOrPoint< OTHERCLASS, DIM2 > &b) const
Returns the Euclidean distance to another pose/point:
Definition: CPoseOrPoint.h:214
CPose2D.h
TestRS
void TestRS()
Definition: vision_stereo_rectify/test.cpp:52
random.h
mrpt::system::CTicTac::Tac
double Tac() noexcept
Stops the stopwatch.
Definition: CTicTac.cpp:87
main
int main()
Definition: vision_stereo_rectify/test.cpp:78
mrpt::random::CRandomGenerator::drawUniform
return_t drawUniform(const double Min, const double Max)
Generate a uniformly distributed pseudo-random number using the MT19937 algorithm,...
Definition: RandomGenerators.h:142
mrpt::poses::CPose2D
A class used to store a 2D pose, including the 2D coordinate point and a heading (phi) angle.
Definition: CPose2D.h:39
mrpt::poses::CPoseOrPoint::y
double y() const
Definition: CPoseOrPoint.h:147
mrpt::system::CTicTac::Tic
void Tic() noexcept
Starts the stopwatch.
Definition: CTicTac.cpp:76
mrpt::square
return_t square(const num_t x)
Inline function for the square of a number.
Definition: core/include/mrpt/core/bits_math.h:23
mrpt::system::os::fprintf
int fprintf(FILE *fil, const char *format,...) noexcept MRPT_printf_format_check(2
An OS-independent version of fprintf.
Definition: os.cpp:419
mrpt::random::getRandomGenerator
CRandomGenerator & getRandomGenerator()
A static instance of a CRandomGenerator class, for use in single-thread applications.
Definition: RandomGenerator.cpp:89
CTicTac.h
M_PI
#define M_PI
Definition: core/include/mrpt/core/bits_math.h:43
mrpt::bayes
The namespace for Bayesian filtering algorithm: different particle filters and Kalman filter algorith...
Definition: CKalmanFilterCapable.h:30
mrpt::random::CRandomGenerator::drawGaussian1D
return_t drawGaussian1D(const double mean, const double std)
Generate a normally distributed pseudo-random number.
Definition: RandomGenerators.h:194
mrpt::bayes::CRejectionSamplingCapable::rejectionSampling
void rejectionSampling(size_t desiredSamples, std::vector< TParticle > &outSamples, size_t timeoutTrials=1000)
Generates a set of N independent samples via rejection sampling.
Definition: CRejectionSamplingCapable.h:48
mrpt::system::os::fopen
FILE * fopen(const char *fileName, const char *mode) noexcept
An OS-independent version of fopen.
Definition: os.cpp:268
mrpt::random
A namespace of pseudo-random numbers generators of diferent distributions.
Definition: random_shuffle.h:18
bayes.h
mrpt::exception_to_str
std::string exception_to_str(const std::exception &e)
Builds a nice textual representation of a nested exception, which if generated using MRPT macros (THR...
Definition: exceptions.cpp:59
mrpt::poses::CPoint2D
A class used to store a 2D point.
Definition: CPoint2D.h:32
mrpt::system
Definition: backtrace.h:14



Page generated by Doxygen 1.8.17 for MRPT 2.0.4 at Sun Jul 19 15:15:43 UTC 2020