MRPT  2.0.4
CDetectorDoorCrossing.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 "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 
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.insert(in_poseChange);
62  lastObs.insert(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 enough old data yet:
78  out_estimation.enoughInformation = 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_(act->GetRuntimeClass()->derivedFrom(
108  std::dynamic_pointer_cast<CActionRobotMovement2D>(act);
109 
110  action->poseChange->getMean(pos);
111 
112  out_estimation.cumulativeTurning += fabs(pos.phi());
113 
114  // Get the cumulative pose for the current observation:
115  p = p + pos;
116 
117  // Add SF to the grid map:
119  CPose3D pose3D(p);
120  sf->insertObservationsInto(&auxMap, &pose3D);
121  }
122 
123  // 4) Compute the information differece between this
124  // "map patch" and the previous one:
125  // -------------------------------------------------------
126  auxMap.mapByClass<COccupancyGridMap2D>()->computeEntropy(entropy);
127 
128  if (!lastEntropyValid)
129  {
130  out_estimation.enoughInformation = false;
131  }
132  else
133  {
134  // 5) Fill output data
135  // ---------------------------------
136  out_estimation.enoughInformation = true;
137 
138  out_estimation.informationGain = entropy.I - lastEntropy.I;
139  out_estimation.pointsMap = *auxMap.mapByClass<CSimplePointsMap>();
140  }
141 
142  // For next iterations:
144  lastEntropyValid = true;
145 
146  MRPT_END
147 }
unsigned int windowSize
The window size, in (action,observations) pairs;min.
#define MRPT_START
Definition: exceptions.h:241
mrpt::maps::COccupancyGridMap2D::TEntropyInfo entropy
Entropy of current, and last "map patchs".
A cloud of points in 2D or 3D, which can be built from a sequence of laser scans. ...
bool enoughInformation
If this is false, all other output fields must not be taken into account since there is not yet enoug...
mrpt::system::COutputLogger COutputLogger
T::Ptr mapByClass(size_t ith=0) const
Returns the i&#39;th map of a given class (or of a derived class), or empty smart pointer if there is no ...
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:65
#define ASSERT_(f)
Defines an assertion mechanism.
Definition: exceptions.h:120
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:102
double phi() const
Get the phi angle of the 2D pose (in radians)
Definition: CPose2D.h:86
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:51
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:125
void remove(size_t index)
Delete the action or observation stored in the given index.
Definition: CRawlog.cpp:246
void push_back(const MAP_DEFINITION &o)
A class for storing an occupancy grid map.
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:39
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:85
#define MRPT_END
Definition: exceptions.h:245
void insert(CAction &action)
Add an action to the sequence: a collection of just one element is created.
Definition: CRawlog.cpp:57
void clear()
Clear the sequence of actions/observations.
Definition: CRawlog.cpp:28
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...
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:64
mrpt::maps::COccupancyGridMap2D::TEntropyInfo lastEntropy



Page generated by Doxygen 1.8.14 for MRPT 2.0.4 Git: 33de1d0ad Sat Jun 20 11:02:42 2020 +0200 at sáb jun 20 17:35:17 CEST 2020