camera_sensor_directional_led_detector_algorithm.h
Go to the documentation of this file.
1 
7 #ifndef CAMERA_SENSOR_DIRECTIONAL_LED_DETECTOR_ALGORITHM_H
8 #define CAMERA_SENSOR_DIRECTIONAL_LED_DETECTOR_ALGORITHM_H
9 
10 namespace argos {
11  class CCameraSensorDirectionalLEDDetectorAlgorithm;
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/directional_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_directional_led_detector_algorithm.h>
22 
23 namespace argos {
24 
32 
33  public:
34  class CUpdateOperation : public CBaseUpdateOperation,
35  public CPositionalIndex<CDirectionalLEDEntity>::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,
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 */
55  if(c_led.GetColor() == CColor::BLACK) {
56  return true;
57  }
58  if(GetAngleWithCamera(c_led) > c_led.GetObservableAngle()) {
59  return true;
60  }
61  const CVector3& cLedPosition = c_led.GetPosition();
62  if(IsPointInsideFrustum(cLedPosition) == false) {
63  return true;
64  }
65  m_cOcclusionCheckRay.SetEnd(cLedPosition);
66  if(GetClosestEmbodiedEntityIntersectedByRay(m_sIntersectionItem, m_cOcclusionCheckRay)) {
67  m_cAlgorithm.AddCheckedRay(true, m_cOcclusionCheckRay);
68  return true;
69  }
70  m_cAlgorithm.AddCheckedRay(false, m_cOcclusionCheckRay);
71  m_cAlgorithm.AddReading(c_led.GetColor(), ProjectOntoSensor(cLedPosition));
72  return true;
73  }
74 
75  private:
76  CRay3 m_cOcclusionCheckRay;
77  SEmbodiedEntityIntersectionItem m_sIntersectionItem;
79  };
80 
81  public:
82 
84 
86 
87  virtual void Init(TConfigurationNode& t_tree);
88 
89  virtual void Update(const CSquareMatrix<3>& c_projection_matrix,
90  const std::array<CPlane, 6>& arr_frustum_planes,
91  const CTransformationMatrix3& c_world_to_camera_transform,
92  const CVector3& c_camera_location,
93  const CVector3& c_bounding_box_position,
94  const CVector3& c_bounding_box_half_extents);
95 
96  void AddCheckedRay(bool b_intersected, const CRay3& c_ray) {
97  if(m_bShowRays) {
98  m_vecCheckedRays.emplace_back(b_intersected, c_ray);
99  }
100  }
101 
102  void AddReading(const CColor& c_color, const CVector2& c_center) {
103  m_vecReadings.emplace_back(c_color, c_center);
104  }
105 
106  private:
107  bool m_bShowRays;
109  };
110 }
111 
112 #endif
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, CCameraSensorDirectionalLEDDetectorAlgorithm &c_algorithm)
void SetEnd(const CVector3 &c_end)
Definition: ray3.h:57
const CVector3 & GetPosition() const
A 3D vector class.
Definition: vector3.h:29
A data structure that contains positional entities.
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)
const CColor & GetColor() const
Returns the current color of the LED.
bool GetClosestEmbodiedEntityIntersectedByRay(SEmbodiedEntityIntersectionItem &s_item, const CRay3 &c_ray)
Returns the closest intersection with an embodied entity to the ray start.
ticpp::Element TConfigurationNode
The ARGoS configuration XML node.
virtual void Init(TConfigurationNode &t_tree)
Initializes the resource.
The basic color type.
Definition: color.h:25
std::vector< std::pair< bool, CRay3 > > m_vecCheckedRays
const CRadians & GetObservableAngle() const
Returns the observable angle of the LED.
This class provides the most general interface to a camera.
static CColor BLACK
Definition: color.h:29
void SetStart(const CVector3 &c_start)
Definition: ray3.h:53
A 2D vector class.
Definition: vector2.h:25
The namespace containing all the ARGoS related code.
Definition: ci_actuator.h:12