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 
13 #include <mrpt/poses/CPosePDFSOG.h>
15 #include <mrpt/system/os.h>
16 #include <iostream>
17 
18 using namespace mrpt;
19 using namespace mrpt::poses;
20 using namespace mrpt::opengl;
21 using namespace mrpt::gui;
22 using namespace mrpt::io;
23 using namespace mrpt::system;
24 using namespace mrpt::serialization;
25 using namespace std;
26 
27 /* ------------------------------------------------------------------------
28  Test_SoG_Merge
29  ------------------------------------------------------------------------ */
30 void Test_SoG_Merge()
31 {
32  CPosePDFSOG pdf;
33 
35  m.mean = CPose2D(1.1, -0.1, -2.0_deg);
36 
37  m.cov.setIdentity();
38  m.cov(0, 0) = m.cov(1, 1) = square(0.1);
39  m.cov(2, 2) = square(2.0_deg);
40  m.log_w = 0;
41 
42  pdf.clear();
43  pdf.push_back(m);
44 
45  m.mean = CPose2D(1.1, 0.1, 2.0_deg);
46  pdf.push_back(m);
47 
48  m.mean = CPose2D(2, 0, 20.0_deg);
49  pdf.push_back(m);
50 
51  cout << "Initial PDF: mean: " << pdf.getMeanVal() << endl;
52  cout << pdf.getCovariance() << endl << endl;
53 
54 #if MRPT_HAS_WXWIDGETS
55  CDisplayWindow3D win_before("Before merge");
56  CDisplayWindow3D win_after("After merge");
57 #endif
58 
59  {
60  COpenGLScene scene;
61  CSetOfObjects::Ptr o = CSetOfObjects::Create();
62  pdf.getAs3DObject(o);
63  scene.insert(o);
64  scene.insert(CGridPlaneXY::Create(-5, 5, -5, 5, 0, 1));
65 
66  CFileGZOutputStream f("sog_before.3Dscene");
67  archiveFrom(f) << scene;
68 
69 #if MRPT_HAS_WXWIDGETS
70  COpenGLScene::Ptr sc = win_before.get3DSceneAndLock();
71  *sc = scene;
72  win_before.unlockAccess3DScene();
73  win_before.setCameraZoom(5);
74  win_before.setCameraPointingToPoint(1, 0, 0);
75  win_before.forceRepaint();
76 #endif
77  }
78 
79  cout << "Merging...";
80  pdf.mergeModes(0.9, true);
81  cout << " # modes after: " << pdf.size() << endl;
82 
83  cout << "Final PDF: mean: " << pdf.getMeanVal() << endl;
84  cout << pdf.getCovariance() << endl << endl;
85 
86  {
87  COpenGLScene scene;
88  CSetOfObjects::Ptr o = CSetOfObjects::Create();
89  pdf.getAs3DObject(o);
90  scene.insert(o);
91  scene.insert(CGridPlaneXY::Create(-5, 5, -5, 5, 0, 1));
92 
93  CFileGZOutputStream f("sog_after.3Dscene");
94  archiveFrom(f) << scene;
95 
96 #if MRPT_HAS_WXWIDGETS
97  COpenGLScene::Ptr sc = win_after.get3DSceneAndLock();
98  *sc = scene;
99  win_after.unlockAccess3DScene();
100  win_after.setCameraZoom(5);
101  win_after.setCameraPointingToPoint(1, 0, 0);
102  win_after.forceRepaint();
103 #endif
104  }
105 
106 #if MRPT_HAS_WXWIDGETS
107  cout << "Push any key to exit..." << endl;
109 #endif
110 }
111 
112 // ------------------------------------------------------
113 // MAIN
114 // ------------------------------------------------------
115 int main(int argc, char** argv)
116 {
117  try
118  {
119  Test_SoG_Merge();
120 
121  return 0;
122  }
123  catch (const std::exception& e)
124  {
125  std::cerr << "MRPT error: " << mrpt::exception_to_str(e) << std::endl;
126  return -1;
127  }
128  catch (...)
129  {
130  printf("Untyped exception!");
131  return -1;
132  }
133 }
os.h
mrpt::poses::CPosePDFSOG::clear
void clear()
Clear the list of modes.
Definition: CPosePDFSOG.cpp:40
mrpt::poses::CPosePDF::getAs3DObject
void getAs3DObject(OPENGL_SETOFOBJECTSPTR &out_obj) const
Returns a 3D representation of this PDF (it doesn't clear the current contents of out_obj,...
Definition: CPosePDF.h:113
mrpt::io
Definition: img/CImage.h:24
mrpt::opengl::CSetOfObjects::Ptr
std::shared_ptr< mrpt::opengl ::CSetOfObjects > Ptr
Definition: CSetOfObjects.h:28
mrpt::opengl::COpenGLScene::insert
void insert(const CRenderizable::Ptr &newObject, const std::string &viewportName=std::string("main"))
Insert a new object into the scene, in the given viewport (by default, into the "main" viewport).
Definition: COpenGLScene.cpp:177
CPosePDFSOG.h
mrpt::opengl::COpenGLScene
This class allows the user to create, load, save, and render 3D scenes using OpenGL primitives.
Definition: COpenGLScene.h:56
CFileGZOutputStream.h
mrpt::math::MatrixBase::setIdentity
void setIdentity()
Definition: MatrixBase.h:57
mrpt
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
Definition: BaseAppDataSource.h:15
mrpt::poses::CPosePDFSOG::push_back
void push_back(const TGaussianMode &m)
Inserts a copy of the given mode into the SOG.
Definition: CPosePDFSOG.h:118
mrpt::poses
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
Definition: CHierarchicalMapMHPartition.h:22
mrpt::poses::CPosePDFSOG::TGaussianMode
The struct for each mode:
Definition: CPosePDFSOG.h:41
CDisplayWindow3D.h
main
int main()
Definition: vision_stereo_rectify/test.cpp:78
mrpt::poses::CPosePDFSOG::TGaussianMode::mean
CPose2D mean
Definition: CPosePDFSOG.h:44
mrpt::io::CFileGZOutputStream
Saves data to a file and transparently compress the data using the given compression level.
Definition: io/CFileGZOutputStream.h:26
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
argv
const char * argv[]
Definition: RawlogGrabberApp_unittest.cpp:40
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::math::CProbabilityDensityFunction::getMeanVal
type_value getMeanVal() const
Returns the mean, or mathematical expectation of the probability density distribution (PDF).
Definition: CProbabilityDensityFunction.h:77
mrpt::serialization
Definition: aligned_serialization.h:13
mrpt::gui
Classes for creating GUI windows for 2D and 3D visualization.
Definition: about_box.h:14
mrpt::poses::CPosePDFSOG::TGaussianMode::cov
mrpt::math::CMatrixDouble33 cov
Definition: CPosePDFSOG.h:45
mrpt::opengl::COpenGLScene::Ptr
std::shared_ptr< mrpt::opengl ::COpenGLScene > Ptr
Definition: COpenGLScene.h:58
mrpt::serialization::archiveFrom
CArchiveStreamBase< STREAM > archiveFrom(STREAM &s)
Helper function to create a templatized wrapper CArchive object for a: MRPT's CStream,...
Definition: CArchive.h:592
mrpt::poses::CPosePDFSOG::size
size_t size() const
Return the number of Gaussian modes.
Definition: CPosePDFSOG.h:85
mrpt::poses::CPosePDFSOG::mergeModes
void mergeModes(double max_KLd=0.5, bool verbose=false)
Merge very close modes so the overall number of modes is reduced while preserving the total distribut...
Definition: CPosePDFSOG.cpp:516
argc
const int argc
Definition: RawlogGrabberApp_unittest.cpp:41
mrpt::system::os::getch
int getch() noexcept
An OS-independent version of getch, which waits until a key is pushed.
Definition: os.cpp:370
mrpt::math::CProbabilityDensityFunction::getCovariance
void getCovariance(mrpt::math::CMatrixDouble &cov) const
Returns the estimate of the covariance matrix (STATE_LEN x STATE_LEN covariance matrix)
Definition: CProbabilityDensityFunction.h:88
Test_SoG_Merge
void Test_SoG_Merge()
Definition: vision_stereo_rectify/test.cpp:30
CArchive.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
CGridPlaneXY.h
mrpt::opengl
The namespace for 3D scene representation and rendering.
Definition: CGlCanvasBase.h:13
mrpt::gui::CDisplayWindow3D
A graphical user interface (GUI) for efficiently rendering 3D scenes in real-time.
Definition: CDisplayWindow3D.h:117
mrpt::system
Definition: backtrace.h:14
mrpt::poses::CPosePDFSOG
Declares a class that represents a Probability Density function (PDF) of a 2D pose .
Definition: CPosePDFSOG.h:34
mrpt::poses::CPosePDFSOG::TGaussianMode::log_w
double log_w
The log-weight.
Definition: CPosePDFSOG.h:49



Page generated by Doxygen 1.8.17 for MRPT 2.0.3 at Fri May 15 23:51:15 UTC 2020