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 }
mrpt::detectors
Definition: CCascadeClassifierDetection.h:14
CDetectorDoorCrossing.h
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::maps::COccupancyGridMap2D::TMapDefinition::resolution
float resolution
Definition: COccupancyGridMap2D.h:1152
mrpt::obs::CSensoryFrame::Ptr
std::shared_ptr< mrpt::obs ::CSensoryFrame > Ptr
Definition: CSensoryFrame.h:53
CMultiMetricMap.h
mrpt::poses::CPose2D::phi
double phi() const
Get the phi angle of the 2D pose (in radians)
Definition: CPose2D.h:86
mrpt::maps::CSimplePointsMap::TMapDefinition
Definition: CSimplePointsMap.h:135
mrpt::detectors::CDetectorDoorCrossing::process
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.
Definition: CDetectorDoorCrossing.cpp:48
mrpt::detectors::CDetectorDoorCrossing::lastEntropy
mrpt::maps::COccupancyGridMap2D::TEntropyInfo lastEntropy
Definition: CDetectorDoorCrossing.h:104
mrpt::detectors::CDetectorDoorCrossing::TDoorCrossingOutParams
A structure used as output in this method.
Definition: CDetectorDoorCrossing.h:47
mrpt::obs::CActionCollection::Ptr
std::shared_ptr< mrpt::obs ::CActionCollection > Ptr
Definition: CActionCollection.h:28
mrpt::detectors::CDetectorDoorCrossing::TOptions::gridResolution
float gridResolution
Definition: CDetectorDoorCrossing.h:41
mrpt
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
Definition: BaseAppDataSource.h:15
mrpt::utils::COutputLogger
mrpt::system::COutputLogger COutputLogger
Definition: utils/COutputLogger.h:5
mrpt::obs::CActionRobotMovement2D
Represents a probabilistic 2D movement of the robot mobile base.
Definition: CActionRobotMovement2D.h:30
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
mrpt::obs::CActionRobotMovement2D::Ptr
std::shared_ptr< mrpt::obs ::CActionRobotMovement2D > Ptr
Definition: CActionRobotMovement2D.h:32
mrpt::obs
This namespace contains representation of robot actions and observations.
Definition: CParticleFilter.h:17
mrpt::obs::CRawlog::insert
void insert(CAction &action)
Add an action to the sequence: a collection of just one element is created.
Definition: CRawlog.cpp:57
mrpt::maps::TSetOfMetricMapInitializers
A set of TMetricMapInitializer structures, passed to the constructor CMultiMetricMap::CMultiMetricMap...
Definition: TMetricMapInitializer.h:89
mrpt::obs::CRawlog::getAsObservations
CSensoryFrame::Ptr getAsObservations(size_t index) const
Returns the i'th element in the sequence, as being an action, where index=0 is the first object.
Definition: CRawlog.cpp:125
mrpt::detectors::CDetectorDoorCrossing::lastObs
mrpt::obs::CRawlog lastObs
The last observations and consecutive actions are stored here: Indexes (0,1) is the earlier (act,...
Definition: CDetectorDoorCrossing.h:101
mrpt::obs::CSensoryFrame
Declares a class for storing a "sensory frame", a set of "observations" taken by the robot approximat...
Definition: CSensoryFrame.h:51
mrpt::maps::COccupancyGridMap2D::TMapDefinition
Definition: COccupancyGridMap2D.h:1149
mrpt::obs::CRawlog::clear
void clear()
Clear the sequence of actions/observations.
Definition: CRawlog.cpp:28
MRPT_START
#define MRPT_START
Definition: exceptions.h:241
CPosePDF.h
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::detectors::CDetectorDoorCrossing::clear
void clear()
Reset the detector, i.e.
Definition: CDetectorDoorCrossing.cpp:39
mrpt::detectors::CDetectorDoorCrossing::TDoorCrossingOutParams::informationGain
float informationGain
The gain in information produced by the last observation, in "bits".
Definition: CDetectorDoorCrossing.h:64
mrpt::obs::CRawlog::size
size_t size() const
Returns the number of actions / observations object in the sequence.
Definition: CRawlog.cpp:64
mrpt::poses::CPose3D
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:85
mrpt::detectors::CDetectorDoorCrossing::TDoorCrossingOutParams::enoughInformation
bool enoughInformation
If this is false, all other output fields must not be taken into account since there is not yet enoug...
Definition: CDetectorDoorCrossing.h:55
mrpt::obs::CRawlog::getAsAction
CActionCollection::Ptr getAsAction(size_t index) const
Returns the i'th element in the sequence, as being actions, where index=0 is the first object.
Definition: CRawlog.cpp:65
mrpt::maps::COccupancyGridMap2D::TEntropyInfo::I
double I
The target variable for absolute "information", defining I(x) = 1 - H(x)
Definition: COccupancyGridMap2D.h:449
mrpt::detectors::CDetectorDoorCrossing::options
struct mrpt::detectors::CDetectorDoorCrossing::TOptions options
mrpt::detectors::CDetectorDoorCrossing::entropy
mrpt::maps::COccupancyGridMap2D::TEntropyInfo entropy
Entropy of current, and last "map patchs".
Definition: CDetectorDoorCrossing.h:104
mrpt::maps::CSimplePointsMap
A cloud of points in 2D or 3D, which can be built from a sequence of laser scans.
Definition: CSimplePointsMap.h:30
mrpt::detectors::CDetectorDoorCrossing::TDoorCrossingOutParams::pointsMap
mrpt::maps::CSimplePointsMap pointsMap
Definition: CDetectorDoorCrossing.h:71
detectors-precomp.h
CLASS_ID
#define CLASS_ID(T)
Access to runtime class ID for a defined class name.
Definition: CObject.h:102
MRPT_END
#define MRPT_END
Definition: exceptions.h:245
mrpt::maps::TSetOfMetricMapInitializers::push_back
void push_back(const MAP_DEFINITION &o)
Definition: TMetricMapInitializer.h:97
mrpt::obs::CAction::Ptr
std::shared_ptr< CAction > Ptr
Definition: CAction.h:26
mrpt::maps::COccupancyGridMap2D
A class for storing an occupancy grid map.
Definition: COccupancyGridMap2D.h:53
mrpt::detectors::CDetectorDoorCrossing::TDoorCrossingOutParams::cumulativeTurning
float cumulativeTurning
The cumulative turning of the robot in radians for the movements in the "window".
Definition: CDetectorDoorCrossing.h:69
mrpt::maps
Definition: CBeacon.h:21
mrpt::detectors::CDetectorDoorCrossing::lastEntropyValid
bool lastEntropyValid
Definition: CDetectorDoorCrossing.h:105
mrpt::detectors::CDetectorDoorCrossing::TOptions::windowSize
unsigned int windowSize
The window size, in (action,observations) pairs;min.
Definition: CDetectorDoorCrossing.h:40
mrpt::obs::CRawlog::remove
void remove(size_t index)
Delete the action or observation stored in the given index.
Definition: CRawlog.cpp:246



Page generated by Doxygen 1.8.17 for MRPT 2.0.4 at Fri Jul 17 08:43:33 UTC 2020