camera_sensor_led_detector_algorithm.h
Go to the documentation of this file.
1 
7 #ifndef CAMERA_SENSOR_LED_DETECTOR_ALGORITHM_H
8 #define CAMERA_SENSOR_LED_DETECTOR_ALGORITHM_H
9 
10 namespace argos {
11  class CCameraSensorLEDDetectorAlgorithm;
12 }
13 
14 #include <argos3/core/simulator/entity/embodied_entity.h>
15 #include <argos3/core/simulator/space/positional_indices/positional_index.h>
16 #include <argos3/core/utility/math/ray3.h>
17 #include <argos3/core/utility/math/matrix/transformationmatrix3.h>
18 
19 #include <argos3/plugins/simulator/entities/led_entity.h>
20 #include <argos3/plugins/robots/generic/simulator/camera_sensor_algorithm.h>
21 #include <argos3/plugins/robots/generic/control_interface/ci_camera_sensor_algorithms/ci_camera_sensor_led_detector_algorithm.h>
22 
23 namespace argos {
24 
32 
33  public:
34  class CUpdateOperation : public CBaseUpdateOperation,
35  public CPositionalIndex<CLEDEntity>::COperation {
36 
37  public:
38  /* constructor */
39  CUpdateOperation(const CSquareMatrix<3>& c_projection_matrix,
40  const std::array<CPlane, 6>& arr_frustum_planes,
41  const CTransformationMatrix3& c_world_to_camera_transform,
42  const CVector3& c_camera_location,
43  CCameraSensorLEDDetectorAlgorithm& c_algorithm) :
44  CBaseUpdateOperation(c_projection_matrix,
45  arr_frustum_planes,
46  c_world_to_camera_transform,
47  c_camera_location),
48  m_cAlgorithm(c_algorithm) {
49  m_cOcclusionCheckRay.SetStart(c_camera_location);
50  }
51  /* destructor */
52  virtual ~CUpdateOperation() {}
53  /* operation */
54  bool operator()(CLEDEntity& c_led) {
55  if(c_led.GetColor() == CColor::BLACK) {
56  return true;
57  }
58  const CVector3& cLedPosition = c_led.GetPosition();
59  if(IsPointInsideFrustum(cLedPosition) == false) {
60  return true;
61  }
62  m_cOcclusionCheckRay.SetEnd(cLedPosition);
63  if(GetClosestEmbodiedEntityIntersectedByRay(m_sIntersectionItem, m_cOcclusionCheckRay)) {
64  m_cAlgorithm.AddCheckedRay(true, m_cOcclusionCheckRay);
65  return true;
66  }
67  m_cAlgorithm.AddCheckedRay(false, m_cOcclusionCheckRay);
68  m_cAlgorithm.AddReading(c_led.GetColor(), ProjectOntoSensor(cLedPosition));
69  return true;
70  }
71 
72  private:
73  CRay3 m_cOcclusionCheckRay;
74  SEmbodiedEntityIntersectionItem m_sIntersectionItem;
76  };
77 
78  public:
79 
81 
83 
84  virtual void Init(TConfigurationNode& t_tree);
85 
86  virtual void Update(const CSquareMatrix<3>& c_projection_matrix,
87  const std::array<CPlane, 6>& arr_frustum_planes,
88  const CTransformationMatrix3& c_world_to_camera_transform,
89  const CVector3& c_camera_location,
90  const CVector3& c_bounding_box_position,
91  const CVector3& c_bounding_box_half_extents);
92 
93  void AddCheckedRay(bool b_intersected, const CRay3& c_ray) {
94  if(m_bShowRays) {
95  m_vecCheckedRays.emplace_back(b_intersected, c_ray);
96  }
97  }
98 
99  void AddReading(const CColor& c_color, const CVector2& c_center) {
100  m_vecReadings.emplace_back(c_color, c_center);
101  }
102 
103  private:
104  bool m_bShowRays;
105  CPositionalIndex<CLEDEntity>* m_pcLEDIndex;
106  };
107 }
108 
109 #endif
A 3D vector class.
Definition: vector3.h:29
void AddCheckedRay(bool b_intersected, const CRay3 &c_ray)
static CColor BLACK
Definition: color.h:29
CUpdateOperation(const CSquareMatrix< 3 > &c_projection_matrix, const std::array< CPlane, 6 > &arr_frustum_planes, const CTransformationMatrix3 &c_world_to_camera_transform, const CVector3 &c_camera_location, CCameraSensorLEDDetectorAlgorithm &c_algorithm)
A data structure that contains positional entities.
const CColor & GetColor() const
Returns the current color of the LED.
Definition: led_entity.h:58
ticpp::Element TConfigurationNode
The ARGoS configuration XML node.
const CVector3 & GetPosition() const
void SetEnd(const CVector3 &c_end)
Definition: ray3.h:57
A 2D vector class.
Definition: vector2.h:25
void AddReading(const CColor &c_color, const CVector2 &c_center)
void SetStart(const CVector3 &c_start)
Definition: ray3.h:53
virtual void Update(const CSquareMatrix< 3 > &c_projection_matrix, const std::array< CPlane, 6 > &arr_frustum_planes, const CTransformationMatrix3 &c_world_to_camera_transform, const CVector3 &c_camera_location, const CVector3 &c_bounding_box_position, const CVector3 &c_bounding_box_half_extents)
bool GetClosestEmbodiedEntityIntersectedByRay(SEmbodiedEntityIntersectionItem &s_item, const CRay3 &c_ray)
Returns the closest intersection with an embodied entity to the ray start.
The basic color type.
Definition: color.h:25
This class provides the most general interface to a camera.
The namespace containing all the ARGoS related code.
Definition: ci_actuator.h:12
std::vector< std::pair< bool, CRay3 > > m_vecCheckedRays
virtual void Init(TConfigurationNode &t_tree)
Initializes the resource.