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 }
mrpt::detectors
Definition: CCascadeClassifierDetection.h:17
CDetectorDoorCrossing.h
mrpt::maps::COccupancyGridMap2D::TMapDefinition::resolution
float resolution
Definition: COccupancyGridMap2D.h:1174
mrpt::obs::CSensoryFrame::Ptr
std::shared_ptr< CSensoryFrame > Ptr
Definition: CSensoryFrame.h:56
CMultiMetricMap.h
mrpt::obs::CRawlog::addAction
void addAction(CAction &action)
Add an action to the sequence: a collection of just one element is created.
Definition: CRawlog.cpp:80
mrpt::poses::CPose2D::phi
const double & phi() const
Get the phi angle of the 2D pose (in radians)
Definition: CPose2D.h:82
mrpt::maps::CSimplePointsMap::TMapDefinition
Definition: CSimplePointsMap.h:138
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:119
mrpt::detectors::CDetectorDoorCrossing::TDoorCrossingOutParams
A structure used as output in this method.
Definition: CDetectorDoorCrossing.h:57
mrpt::maps::CMultiMetricMap::m_gridMaps
ProxyFilterContainerByClass< mrpt::maps::COccupancyGridMap2D::Ptr, TListMaps > m_gridMaps
STL-like proxy to access this kind of maps in maps.
Definition: CMultiMetricMap.h:365
mrpt::detectors::CDetectorDoorCrossing::TOptions::gridResolution
float gridResolution
Definition: CDetectorDoorCrossing.h:50
mrpt
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
Definition: CKalmanFilterCapable.h:30
mrpt::utils::COutputLogger
mrpt::system::COutputLogger COutputLogger
Definition: utils/COutputLogger.h:7
mrpt::obs::CActionRobotMovement2D
Represents a probabilistic 2D movement of the robot mobile base.
Definition: CActionRobotMovement2D.h:32
ASSERT_
#define ASSERT_(f)
Defines an assertion mechanism.
Definition: exceptions.h:113
p
GLfloat GLfloat p
Definition: glext.h:6305
mrpt::obs::CActionRobotMovement2D::Ptr
std::shared_ptr< CActionRobotMovement2D > Ptr
Definition: CActionRobotMovement2D.h:34
mrpt::poses
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
Definition: CHierarchicalMapMHPartition.h:25
mrpt::obs
This namespace contains representation of robot actions and observations.
Definition: CParticleFilter.h:17
mrpt::maps::CMultiMetricMap::m_pointsMaps
ProxyFilterContainerByClass< mrpt::maps::CSimplePointsMap::Ptr, TListMaps > m_pointsMaps
STL-like proxy to access this kind of maps in maps.
Definition: CMultiMetricMap.h:362
mrpt::maps::TSetOfMetricMapInitializers
A set of TMetricMapInitializer structures, passed to the constructor CMultiMetricMap::CMultiMetricMap...
Definition: TMetricMapInitializer.h:91
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:149
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:116
mrpt::obs::CSensoryFrame
Declares a class for storing a "sensory frame", a set of "observations" taken by the robot approximat...
Definition: CSensoryFrame.h:54
mrpt::maps::COccupancyGridMap2D::TMapDefinition
Definition: COccupancyGridMap2D.h:1172
mrpt::obs::CRawlog::clear
void clear()
Clear the sequence of actions/observations.
Definition: CRawlog.cpp:32
MRPT_START
#define MRPT_START
Definition: exceptions.h:262
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:40
mrpt::maps::CMultiMetricMap
This class stores any customizable set of metric maps.
Definition: CMultiMetricMap.h:141
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:79
mrpt::obs::CRawlog::size
size_t size() const
Returns the number of actions / observations object in the sequence.
Definition: CRawlog.cpp:88
mrpt::poses::CPose3D
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:88
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:89
mrpt::maps::COccupancyGridMap2D::TEntropyInfo::I
double I
The target variable for absolute "information", defining I(x) = 1 - H(x)
Definition: COccupancyGridMap2D.h:471
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:119
mrpt::detectors::CDetectorDoorCrossing::TDoorCrossingOutParams::pointsMap
mrpt::maps::CSimplePointsMap pointsMap
Definition: CDetectorDoorCrossing.h:86
mrpt::maps::CSimplePointsMap::copyFrom
virtual void copyFrom(const CPointsMap &obj) override
Virtual assignment operator, to be implemented in derived classes
Definition: CSimplePointsMap.cpp:94
detectors-precomp.h
CLASS_ID
#define CLASS_ID(T)
Access to runtime class ID for a defined class name.
Definition: CObject.h:85
mrpt::obs::CRawlog::addObservations
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
MRPT_END
#define MRPT_END
Definition: exceptions.h:266
mrpt::maps::TSetOfMetricMapInitializers::push_back
void push_back(const MAP_DEFINITION &o)
Definition: TMetricMapInitializer.h:99
mrpt::obs::CAction::Ptr
std::shared_ptr< CAction > Ptr
Definition: CAction.h:29
mrpt::detectors::CDetectorDoorCrossing::TDoorCrossingOutParams::cumulativeTurning
float cumulativeTurning
The cumulative turning of the robot in radians for the movements in the "window".
Definition: CDetectorDoorCrossing.h:84
mrpt::maps
Definition: CBeacon.h:24
mrpt::detectors::CDetectorDoorCrossing::TDoorCrossingOutParams::enoughtInformation
bool enoughtInformation
If this is false, all other output fields must not be taken into account since there is not yet enoug...
Definition: CDetectorDoorCrossing.h:64
mrpt::obs::CActionCollection::Ptr
std::shared_ptr< CActionCollection > Ptr
Definition: CActionCollection.h:30
mrpt::detectors::CDetectorDoorCrossing::lastEntropyValid
bool lastEntropyValid
Definition: CDetectorDoorCrossing.h:120
mrpt::detectors::CDetectorDoorCrossing::TOptions::windowSize
unsigned int windowSize
The window size, in (action,observations) pairs;min.
Definition: CDetectorDoorCrossing.h:48
mrpt::obs::CRawlog::remove
void remove(size_t index)
Delete the action or observation stored in the given index.
Definition: CRawlog.cpp:270



Page generated by Doxygen 1.8.17 for MRPT 1.9.9 Git: ad3a9d8ae Tue May 1 23:10:22 2018 -0700 at miƩ 12 jul 2023 10:03:34 CEST