Main MRPT website > C++ reference for MRPT 1.5.6
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-2017, 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::utils;
21 using namespace mrpt::poses;
22 
23 
24 
25 /*---------------------------------------------------------------
26  Constructor
27  ---------------------------------------------------------------*/
28 CDetectorDoorCrossing::CDetectorDoorCrossing() :
29  COutputLogger("CDetectorDoorCrossing"),
30  options(),
31  lastObs(),
32  entropy(),
33  lastEntropy(),
34  lastEntropyValid(false)
35 {
36  clear();
37 }
38 
39 /*---------------------------------------------------------------
40  clear
41  ---------------------------------------------------------------*/
43 {
44  lastObs.clear();
45  lastEntropyValid = false;
46 }
47 
48 /*---------------------------------------------------------------
49  process
50  ---------------------------------------------------------------*/
52  CActionRobotMovement2D &in_poseChange,
53  CSensoryFrame &in_sf,
54  TDoorCrossingOutParams &out_estimation
55  )
56 {
57  // Variables for generic use:
58  size_t i;
59 
60  out_estimation.cumulativeTurning = 0;
61 
63 
64  // 1) Add new pair to the list:
65  // -----------------------------------------
66  lastObs.addAction( in_poseChange );
67  lastObs.addObservations( in_sf );
68 
69  // 2) Remove oldest pair:
70  // -----------------------------------------
71  ASSERT_( options.windowSize > 1 );
72  ASSERT_( (lastObs.size() % 2) == 0 ); // Assure even size
73 
74  while (lastObs.size()>options.windowSize*2)
75  {
76  lastObs.remove(0);
77  lastObs.remove(0);
78  }
79 
80  if ( lastObs.size() < options.windowSize * 2 )
81  {
82  // Not enought old data yet:
83  out_estimation.enoughtInformation = false;
84  return;
85  }
86 
87  // 3) Build an occupancy grid map with observations
88  // -------------------------------------------------
89  CPose2D p, pos;
90 
91  TSetOfMetricMapInitializers mapInitializer;
92 
93  {
95  mapInitializer.push_back( def );
96  }
97  {
100  mapInitializer.push_back( def );
101  }
102 
103  CMultiMetricMap auxMap( &mapInitializer );
104 
105  for (i=0;i<options.windowSize;i++)
106  {
107  CActionCollectionPtr acts = lastObs.getAsAction( i*2+0 );
108  CActionPtr act = acts->get(0);
109 
110  ASSERT_( act->GetRuntimeClass()->derivedFrom( CLASS_ID( CActionRobotMovement2D ) ) )
111  CActionRobotMovement2DPtr action = CActionRobotMovement2DPtr( act );
112 
113  action->poseChange->getMean(pos);
114 
115  out_estimation.cumulativeTurning+= fabs(pos.phi());
116 
117  // Get the cumulative pose for the current observation:
118  p = p + pos;
119 
120  // Add SF to the grid map:
121  CSensoryFramePtr sf = lastObs.getAsObservations( i*2+1 );
122  CPose3D pose3D(p);
123  sf->insertObservationsInto( &auxMap, &pose3D );
124  }
125 
126  // 4) Compute the information differece between this
127  // "map patch" and the previous one:
128  // -------------------------------------------------------
129  auxMap.m_gridMaps[0]->computeEntropy( entropy );
130 
131  if (!lastEntropyValid)
132  {
133  out_estimation.enoughtInformation = false;
134  }
135  else
136  {
137  // 5) Fill output data
138  // ---------------------------------
139  out_estimation.enoughtInformation = true;
140 
141 
142  out_estimation.informationGain = entropy.I - lastEntropy.I;
143  out_estimation.pointsMap.copyFrom( *auxMap.m_pointsMaps[0] );
144  }
145 
146 
147  // For next iterations:
149  lastEntropyValid = true;
150 
151  MRPT_END
152 
153 }
154 
size_t size() const
Returns the number of actions / observations object in the sequence.
Definition: CRawlog.cpp:83
unsigned int windowSize
The window size, in (action,observations) pairs;min.
const double & phi() const
Get the phi angle of the 2D pose (in radians)
Definition: CPose2D.h:84
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...
float resolution
See COccupancyGridMap2D::COccupancyGridMap2D.
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:45
ProxyFilterContainerByClass< mrpt::maps::COccupancyGridMap2DPtr, TListMaps > m_gridMaps
STL-like proxy to access this kind of maps in maps.
A set of TMetricMapInitializer structures, passed to the constructor CMultiMetricMap::CMultiMetricMap...
Represents a probabilistic 2D movement of the robot mobile base.
#define MRPT_END
struct DETECTORS_IMPEXP mrpt::detectors::CDetectorDoorCrossing::TOptions options
double I
The target variable for absolute "information", defining I(x) = 1 - H(x)
Declares a class for storing a "sensory frame", a set of "observations" taken by the robot approximat...
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.
GLfloat GLfloat p
Definition: glew.h:10113
virtual void copyFrom(const CPointsMap &obj) MRPT_OVERRIDE
Virtual assignment operator, to be implemented in derived classes.
void remove(size_t index)
Delete the action or observation stored in the given index.
Definition: CRawlog.cpp:274
#define CLASS_ID(class_name)
Access to runtime class ID for a defined class name.
Definition: CObject.h:92
void push_back(const MAP_DEFINITION &o)
#define MRPT_START
float informationGain
The gain in information produced by the last observation, in "bits".
A class used to store a 2D pose, including the 2D coordinate point and a heading (phi) angle...
Definition: CPose2D.h:36
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:72
#define ASSERT_(f)
void clear()
Clear the sequence of actions/observations.
Definition: CRawlog.cpp:39
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:76
ProxyFilterContainerByClass< mrpt::maps::CSimplePointsMapPtr, TListMaps > m_pointsMaps
STL-like proxy to access this kind of maps in maps.
This class stores any customizable set of metric maps.
CSensoryFramePtr 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:147
mrpt::maps::COccupancyGridMap2D::TEntropyInfo lastEntropy
CActionCollectionPtr 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:88



Page generated by Doxygen 1.8.6 for MRPT 1.5.6 Git: 4c65e84 Tue Apr 24 08:18:17 2018 +0200 at mar abr 24 08:26:17 CEST 2018