Main MRPT website > C++ reference for MRPT 1.9.9
CDetectorDoorCrossing.cpp
Go to the documentation of this file.
1 /* +------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | http://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2018, Individual contributors, see AUTHORS file |
6  | See: http://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See details in http://www.mrpt.org/License |
8  +------------------------------------------------------------------------+ */
9 
10 #include "detectors-precomp.h" // Precompiled headers
11 
14 #include <mrpt/poses/CPosePDF.h>
15 
16 using namespace mrpt;
17 using namespace mrpt::obs;
18 using namespace mrpt::maps;
19 using namespace mrpt::detectors;
20 using namespace mrpt::poses;
21 
22 /*---------------------------------------------------------------
23  Constructor
24  ---------------------------------------------------------------*/
25 CDetectorDoorCrossing::CDetectorDoorCrossing()
26  : COutputLogger("CDetectorDoorCrossing"),
27  options(),
28  lastObs(),
29  entropy(),
30  lastEntropy(),
31  lastEntropyValid(false)
32 {
33  clear();
34 }
35 
36 /*---------------------------------------------------------------
37  clear
38  ---------------------------------------------------------------*/
40 {
41  lastObs.clear();
42  lastEntropyValid = false;
43 }
44 
45 /*---------------------------------------------------------------
46  process
47  ---------------------------------------------------------------*/
49  CActionRobotMovement2D& in_poseChange, CSensoryFrame& in_sf,
50  TDoorCrossingOutParams& out_estimation)
51 {
52  // Variables for generic use:
53  size_t i;
54 
55  out_estimation.cumulativeTurning = 0;
56 
58 
59  // 1) Add new pair to the list:
60  // -----------------------------------------
61  lastObs.addAction(in_poseChange);
62  lastObs.addObservations(in_sf);
63 
64  // 2) Remove oldest pair:
65  // -----------------------------------------
67  ASSERT_((lastObs.size() % 2) == 0); // Assure even size
68 
69  while (lastObs.size() > options.windowSize * 2)
70  {
71  lastObs.remove(0);
72  lastObs.remove(0);
73  }
74 
75  if (lastObs.size() < options.windowSize * 2)
76  {
77  // Not enought old data yet:
78  out_estimation.enoughtInformation = false;
79  return;
80  }
81 
82  // 3) Build an occupancy grid map with observations
83  // -------------------------------------------------
84  CPose2D p, pos;
85 
86  TSetOfMetricMapInitializers mapInitializer;
87 
88  {
90  mapInitializer.push_back(def);
91  }
92  {
95  mapInitializer.push_back(def);
96  }
97 
98  CMultiMetricMap auxMap(&mapInitializer);
99 
100  for (i = 0; i < options.windowSize; i++)
101  {
102  CActionCollection::Ptr acts = lastObs.getAsAction(i * 2 + 0);
103  CAction::Ptr act = acts->get(0);
104 
105  ASSERT_(
106  act->GetRuntimeClass()->derivedFrom(
109  std::dynamic_pointer_cast<CActionRobotMovement2D>(act);
110 
111  action->poseChange->getMean(pos);
112 
113  out_estimation.cumulativeTurning += fabs(pos.phi());
114 
115  // Get the cumulative pose for the current observation:
116  p = p + pos;
117 
118  // Add SF to the grid map:
120  CPose3D pose3D(p);
121  sf->insertObservationsInto(&auxMap, &pose3D);
122  }
123 
124  // 4) Compute the information differece between this
125  // "map patch" and the previous one:
126  // -------------------------------------------------------
127  auxMap.m_gridMaps[0]->computeEntropy(entropy);
128 
129  if (!lastEntropyValid)
130  {
131  out_estimation.enoughtInformation = false;
132  }
133  else
134  {
135  // 5) Fill output data
136  // ---------------------------------
137  out_estimation.enoughtInformation = true;
138 
139  out_estimation.informationGain = entropy.I - lastEntropy.I;
140  out_estimation.pointsMap.copyFrom(*auxMap.m_pointsMaps[0]);
141  }
142 
143  // For next iterations:
145  lastEntropyValid = true;
146 
147  MRPT_END
148 }
unsigned int windowSize
The window size, in (action,observations) pairs;min.
#define MRPT_START
Definition: exceptions.h:262
ProxyFilterContainerByClass< mrpt::maps::CSimplePointsMap::Ptr, TListMaps > m_pointsMaps
STL-like proxy to access this kind of maps in maps.
mrpt::maps::COccupancyGridMap2D::TEntropyInfo entropy
Entropy of current, and last "map patchs".
bool enoughtInformation
If this is false, all other output fields must not be taken into account since there is not yet enoug...
mrpt::system::COutputLogger COutputLogger
void addObservations(CSensoryFrame &observations)
Add a set of observations to the sequence; the object is duplicated, so the original one can be free ...
Definition: CRawlog.cpp:38
A set of TMetricMapInitializer structures, passed to the constructor CMultiMetricMap::CMultiMetricMap...
CActionCollection::Ptr getAsAction(size_t index) const
Returns the i&#39;th element in the sequence, as being actions, where index=0 is the first object...
Definition: CRawlog.cpp:89
#define ASSERT_(f)
Defines an assertion mechanism.
Definition: exceptions.h:113
Represents a probabilistic 2D movement of the robot mobile base.
#define CLASS_ID(T)
Access to runtime class ID for a defined class name.
Definition: CObject.h:85
double I
The target variable for absolute "information", defining I(x) = 1 - H(x)
struct mrpt::detectors::CDetectorDoorCrossing::TOptions options
This namespace contains representation of robot actions and observations.
Declares a class for storing a "sensory frame", a set of "observations" taken by the robot approximat...
Definition: CSensoryFrame.h:54
ProxyFilterContainerByClass< mrpt::maps::COccupancyGridMap2D::Ptr, TListMaps > m_gridMaps
STL-like proxy to access this kind of maps in maps.
void process(mrpt::obs::CActionRobotMovement2D &in_poseChange, mrpt::obs::CSensoryFrame &in_sf, TDoorCrossingOutParams &out_estimation)
The main method, where a new action/observation pair is added to the list.
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
CSensoryFrame::Ptr getAsObservations(size_t index) const
Returns the i&#39;th element in the sequence, as being an action, where index=0 is the first object...
Definition: CRawlog.cpp:149
void remove(size_t index)
Delete the action or observation stored in the given index.
Definition: CRawlog.cpp:270
virtual void copyFrom(const CPointsMap &obj) override
Virtual assignment operator, to be implemented in derived classes.
void push_back(const MAP_DEFINITION &o)
float informationGain
The gain in information produced by the last observation, in "bits".
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
mrpt::containers::deepcopy_poly_ptr< mrpt::poses::CPosePDF::Ptr > poseChange
The 2D pose change probabilistic estimation.
A class used to store a 2D pose, including the 2D coordinate point and a heading (phi) angle...
Definition: CPose2D.h:40
void clear()
Reset the detector, i.e.
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:88
const double & phi() const
Get the phi angle of the 2D pose (in radians)
Definition: CPose2D.h:82
#define MRPT_END
Definition: exceptions.h:266
void clear()
Clear the sequence of actions/observations.
Definition: CRawlog.cpp:32
float cumulativeTurning
The cumulative turning of the robot in radians for the movements in the "window". ...
mrpt::obs::CRawlog lastObs
The last observations and consecutive actions are stored here: Indexes (0,1) is the earlier (act...
void addAction(CAction &action)
Add an action to the sequence: a collection of just one element is created.
Definition: CRawlog.cpp:80
This class stores any customizable set of metric maps.
size_t size() const
Returns the number of actions / observations object in the sequence.
Definition: CRawlog.cpp:88
GLfloat GLfloat p
Definition: glext.h:6305
mrpt::maps::COccupancyGridMap2D::TEntropyInfo lastEntropy



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: ad3a9d8ae Tue May 1 23:10:22 2018 -0700 at lun oct 28 00:14:14 CET 2019