MRPT  2.0.3
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 
14 #include <mrpt/random.h>
16 #include <mrpt/system/CTicTac.h>
17 #include <mrpt/system/os.h>
18 #include <iostream>
19 
20 using namespace mrpt;
21 using namespace mrpt::slam;
22 using namespace mrpt::maps;
23 using namespace mrpt::system;
24 using namespace mrpt::random;
25 using namespace mrpt::poses;
26 using namespace mrpt::config;
27 using namespace std;
28 
29 float SIGMA = 0.03f;
30 
31 // ------------------------------------------------------
32 // TestRS
33 // ------------------------------------------------------
34 void TestRS()
35 {
37 
38  // Load the map:
39  CMultiMetricMap map;
41  mapInit.loadFromConfigFile(CConfigFile("_demo_map.ini"), "MetricMap");
42  map.setListOfMaps(mapInit);
43 
44  // Create a dummy observation:
47  obs.stdError = SIGMA;
48 
49  meas.beaconID = 0;
50  meas.sensedDistance = 2.1f;
51  meas.sensorLocationOnRobot = CPoint3D(0, 0, 0);
52  obs.sensedData.push_back(meas);
53 
54  meas.beaconID = 1;
55  meas.sensedDistance = 3.1f;
56  meas.sensorLocationOnRobot = CPoint3D(0, 0, 0);
57  obs.sensedData.push_back(meas);
58 
59  meas.beaconID = 2;
60  meas.sensedDistance = 1.1f;
61  meas.sensorLocationOnRobot = CPoint3D(0, 0, 0);
62  // obs.sensedData.push_back( meas );
63 
64  // Rejection Sampling:
66  vector<CRejectionSamplingRangeOnlyLocalization::TParticle> samples;
67  CTicTac tictac;
68 
69  // Set data:
70  auto lmMap = map.mapByClass<CLandmarksMap>();
71  ASSERT_(lmMap);
72 
73  printf("Preparing...");
74  tictac.Tic();
75  CPose2D dumPose(0, 0, 0);
76  RS.setParams(*lmMap, obs, SIGMA, dumPose);
77  printf("Ok! %fms\n", 1000 * tictac.Tac());
78 
79  printf("Computing...");
80  tictac.Tic();
81  RS.rejectionSampling(1000, samples, 1000);
82  printf("Ok! %fms\n", 1000 * tictac.Tac());
83 
84  FILE* f = os::fopen("_out_samples.txt", "wt");
85  vector<CRejectionSamplingRangeOnlyLocalization::TParticle>::iterator it;
86  for (it = samples.begin(); it != samples.end(); it++)
88  f, "%f %f %f %e\n", it->d->x(), it->d->y(), it->d->phi(),
89  it->log_w);
90 
91  os::fclose(f);
92 }
93 
94 // ------------------------------------------------------
95 // MAIN
96 // ------------------------------------------------------
97 int main()
98 {
99  try
100  {
101  TestRS();
102 
103  return 0;
104  }
105  catch (exception& e)
106  {
107  std::cerr << "MRPT error: " << mrpt::exception_to_str(e) << std::endl;
108  return -1;
109  }
110  catch (...)
111  {
112  printf("Untyped excepcion!!");
113  return -1;
114  }
115 }
mrpt::config
Definition: config/CConfigFile.h:14
os.h
mrpt::obs::CObservationBeaconRanges::TMeasurement
Each one of the measurements.
Definition: CObservationBeaconRanges.h:38
mrpt::obs::CObservationBeaconRanges::TMeasurement::sensedDistance
float sensedDistance
The sensed range itself (in meters).
Definition: CObservationBeaconRanges.h:50
mrpt::system::os::fclose
int void fclose(FILE *f)
An OS-independent version of fclose.
Definition: os.cpp:275
mrpt::maps::CMultiMetricMap::mapByClass
T::Ptr mapByClass(size_t ith=0) const
Returns the i'th map of a given class (or of a derived class), or empty smart pointer if there is no ...
Definition: CMultiMetricMap.h:156
mrpt::slam::CRejectionSamplingRangeOnlyLocalization::setParams
bool setParams(const mrpt::maps::CLandmarksMap &beaconsMap, const mrpt::obs::CObservationBeaconRanges &observation, float sigmaRanges, const mrpt::poses::CPose2D &oldPose, float robot_z=0, bool autoCheckAngleRanges=true)
The parameters used in the generation of random samples:
Definition: CRejectionSamplingRangeOnlyLocalization.cpp:120
mrpt::system::CTicTac
A high-performance stopwatch, with typical resolution of nanoseconds.
Definition: system/CTicTac.h:17
CMultiMetricMap.h
CConfigFile.h
SIGMA
double SIGMA
Definition: vision_stereo_rectify/test.cpp:24
mrpt::utils::CConfigFile
mrpt::config::CConfigFile CConfigFile
Definition: utils/CConfigFile.h:5
mrpt
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
Definition: BaseAppDataSource.h:15
mrpt::slam::CRejectionSamplingRangeOnlyLocalization
An implementation of rejection sampling for generating 2D robot pose from range-only measurements wit...
Definition: CRejectionSamplingRangeOnlyLocalization.h:34
ASSERT_
#define ASSERT_(f)
Defines an assertion mechanism.
Definition: exceptions.h:120
mrpt::poses
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
Definition: CHierarchicalMapMHPartition.h:22
TestRS
void TestRS()
Definition: vision_stereo_rectify/test.cpp:52
random.h
mrpt::random::CRandomGenerator::randomize
void randomize(const uint32_t seed)
Initialize the PRNG from the given random seed.
Definition: RandomGenerator.cpp:92
mrpt::system::CTicTac::Tac
double Tac() noexcept
Stops the stopwatch.
Definition: CTicTac.cpp:86
mrpt::maps::TSetOfMetricMapInitializers
A set of TMetricMapInitializer structures, passed to the constructor CMultiMetricMap::CMultiMetricMap...
Definition: TMetricMapInitializer.h:89
mrpt::obs::CObservationBeaconRanges::sensedData
std::deque< TMeasurement > sensedData
The list of observed ranges.
Definition: CObservationBeaconRanges.h:56
main
int main()
Definition: vision_stereo_rectify/test.cpp:78
CLandmarksMap.h
mrpt::obs::CObservationBeaconRanges
Declares a class derived from "CObservation" that represents one (or more) range measurements to labe...
Definition: CObservationBeaconRanges.h:24
mrpt::obs::CObservationBeaconRanges::stdError
float stdError
The "sigma" of the sensor, assuming a zero-mean Gaussian noise model.
Definition: CObservationBeaconRanges.h:35
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::maps::CMultiMetricMap
This class stores any customizable set of metric maps.
Definition: CMultiMetricMap.h:120
mrpt::obs::CObservationBeaconRanges::TMeasurement::beaconID
int32_t beaconID
The ID of the sensed beacon (or INVALID_BEACON_ID if unknown)
Definition: CObservationBeaconRanges.h:52
mrpt::maps::TSetOfMetricMapInitializers::loadFromConfigFile
void loadFromConfigFile(const mrpt::config::CConfigFileBase &source, const std::string &sectionName) override
Loads the configuration for the set of internal maps from a textual definition in an INI-like file.
Definition: TMetricMapInitializer.cpp:69
mrpt::system::CTicTac::Tic
void Tic() noexcept
Starts the stopwatch.
Definition: CTicTac.cpp:75
mrpt::maps::CMultiMetricMap::setListOfMaps
void setListOfMaps(const mrpt::maps::TSetOfMetricMapInitializers &init)
Sets the list of internal map according to the passed list of map initializers (current maps will be ...
Definition: CMultiMetricMap.cpp:113
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:408
CObservationBeaconRanges.h
mrpt::random::getRandomGenerator
CRandomGenerator & getRandomGenerator()
A static instance of a CRandomGenerator class, for use in single-thread applications.
Definition: RandomGenerator.cpp:89
CTicTac.h
CRejectionSamplingRangeOnlyLocalization.h
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:257
mrpt::maps::CLandmarksMap
A class for storing a map of 3D probabilistic landmarks.
Definition: CLandmarksMap.h:74
mrpt::random
A namespace of pseudo-random numbers generators of diferent distributions.
Definition: random_shuffle.h:18
mrpt::maps
Definition: CBeacon.h:21
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::CPoint3D
A class used to store a 3D point.
Definition: CPoint3D.h:31
mrpt::slam
Definition: CMultiMetricMapPDF.h:26
mrpt::obs::CObservationBeaconRanges::TMeasurement::sensorLocationOnRobot
mrpt::poses::CPoint3D sensorLocationOnRobot
Position of the sensor on the robot.
Definition: CObservationBeaconRanges.h:48
mrpt::system
Definition: backtrace.h:14



Page generated by Doxygen 1.8.17 for MRPT 2.0.3 at Fri May 29 13:06:46 UTC 2020