camera_sensor_tag_detector_algorithm.h
Go to the documentation of this file.
1 
7 #ifndef CAMERA_SENSOR_TAG_DETECTOR_ALGORITHM_H
8 #define CAMERA_SENSOR_TAG_DETECTOR_ALGORITHM_H
9 
10 namespace argos {
11  class CCameraSensorTagDetectorAlgorithm;
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/tag_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_tag_detector_algorithm.h>
22 
23 namespace argos {
24 
32 
33  public:
35  public CPositionalIndex<CTagEntity>::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_camera_to_world_transform,
42  const CVector3& c_camera_location,
43  CCameraSensorTagDetectorAlgorithm& c_algorithm) :
44  CBaseUpdateOperation(c_projection_matrix,
45  arr_frustum_planes,
46  c_camera_to_world_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  virtual bool operator()(CTagEntity& c_tag) {
55  if(GetAngleWithCamera(c_tag) > c_tag.GetObservableAngle()) {
56  return true;
57  }
58  std::transform(std::begin(m_arrTagCornerOffsets),
59  std::end(m_arrTagCornerOffsets),
60  std::begin(m_arrTagCorners),
61  [&c_tag] (const CVector3& c_tag_corner_offset) {
62  CVector3 cCorner(c_tag_corner_offset * c_tag.GetSideLength());
63  cCorner.Rotate(c_tag.GetOrientation());
64  return (cCorner + c_tag.GetPosition());
65  });
66  for(const CVector3& c_corner : m_arrTagCorners) {
67  if(IsPointInsideFrustum(c_corner) == false) {
68  /* corner is not inside the frustum */
69  return true;
70  }
71  }
72  for(const CVector3& c_corner : m_arrTagCorners) {
73  m_cOcclusionCheckRay.SetEnd(c_corner);
74  if(GetClosestEmbodiedEntityIntersectedByRay(m_sIntersectionItem, m_cOcclusionCheckRay)) {
75  /* corner is occluded */
76  m_cAlgorithm.AddCheckedRay(true, m_cOcclusionCheckRay);
77  return true;
78  }
79  else {
80  m_cAlgorithm.AddCheckedRay(false, m_cOcclusionCheckRay);
81  }
82  }
83  std::transform(std::begin(m_arrTagCorners),
84  std::end(m_arrTagCorners),
85  std::begin(m_arrTagCornerPixels),
86  [this] (const CVector3& c_tag_corner) {
87  return ProjectOntoSensor(c_tag_corner);
88  });
89  const CVector2& cCenterPixel = ProjectOntoSensor(c_tag.GetPosition());
90  const std::string& strPayload = c_tag.GetPayload();
91  m_cAlgorithm.AddReading(strPayload, cCenterPixel, m_arrTagCornerPixels);
92  return true;
93  }
94 
95  private:
96  /* This order is consistent with the AprilTags algorithm */
97  const std::array<CVector3, 4> m_arrTagCornerOffsets = {{
98  {-0.5, -0.5, 0},
99  {-0.5, 0.5, 0},
100  { 0.5, 0.5, 0},
101  { 0.5, -0.5, 0},
102  }};
103  std::array<CVector3, 4> m_arrTagCorners;
104  std::array<CVector2, 4> m_arrTagCornerPixels;
105  CRay3 m_cOcclusionCheckRay;
106  SEmbodiedEntityIntersectionItem m_sIntersectionItem;
107  CCameraSensorTagDetectorAlgorithm& m_cAlgorithm;
108  };
109 
110  public:
111 
113 
115 
116  virtual void Init(TConfigurationNode& t_tree);
117 
118  virtual void Update(const CSquareMatrix<3>& c_projection_matrix,
119  const std::array<CPlane, 6>& arr_frustum_planes,
120  const CTransformationMatrix3& c_camera_to_world_transform,
121  const CVector3& c_camera_location,
122  const CVector3& c_bounding_box_position,
123  const CVector3& c_bounding_box_half_extents);
124 
125  void AddCheckedRay(bool b_intersected, const CRay3& c_ray) {
126  if(m_bShowRays) {
127  m_vecCheckedRays.emplace_back(b_intersected, c_ray);
128  }
129  }
130 
131  void AddReading(const std::string& str_payload,
132  const CVector2& c_center_pixel,
133  const std::array<CVector2, 4>& arr_corner_pixels) {
134  m_vecReadings.emplace_back(str_payload, c_center_pixel, arr_corner_pixels);
135  }
136 
141  inline bool IsShowRays() {
142  return m_bShowRays;
143  }
144 
149  inline void SetShowRays(bool b_show_rays) {
150  m_bShowRays = b_show_rays;
151  }
152 
153  private:
154  bool m_bShowRays;
155  CPositionalIndex<CTagEntity>* m_pcTagIndex;
156  };
157 }
158 
159 #endif
The namespace containing all the ARGoS related code.
Definition: ci_actuator.h:12
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.
const CQuaternion & GetOrientation() const
const CVector3 & GetPosition() const
A data structure that contains positional entities.
void SetEnd(const CVector3 &c_end)
Definition: ray3.h:57
void SetStart(const CVector3 &c_start)
Definition: ray3.h:53
A 2D vector class.
Definition: vector2.h:27
A 3D vector class.
Definition: vector3.h:31
CVector3 & Rotate(const CQuaternion &c_quaternion)
Rotates this vector by the given quaternion.
Definition: vector3.cpp:23
std::vector< std::pair< bool, CRay3 > > m_vecCheckedRays
CRadians GetAngleWithCamera(const CPositionalEntity &c_entity) const
This class provides the most general interface to a camera.
virtual void Init(TConfigurationNode &t_tree)
Initializes the resource.
void SetShowRays(bool b_show_rays)
Sets whether or not the rays must be shown in the GUI.
void AddReading(const std::string &str_payload, const CVector2 &c_center_pixel, const std::array< CVector2, 4 > &arr_corner_pixels)
bool IsShowRays()
Returns true if the rays must be shown in the GUI.
void AddCheckedRay(bool b_intersected, const CRay3 &c_ray)
virtual void Update(const CSquareMatrix< 3 > &c_projection_matrix, const std::array< CPlane, 6 > &arr_frustum_planes, const CTransformationMatrix3 &c_camera_to_world_transform, const CVector3 &c_camera_location, const CVector3 &c_bounding_box_position, const CVector3 &c_bounding_box_half_extents)
CUpdateOperation(const CSquareMatrix< 3 > &c_projection_matrix, const std::array< CPlane, 6 > &arr_frustum_planes, const CTransformationMatrix3 &c_camera_to_world_transform, const CVector3 &c_camera_location, CCameraSensorTagDetectorAlgorithm &c_algorithm)
const std::string & GetPayload() const
Returns the current payload of the tag.
Definition: tag_entity.h:60
const CRadians & GetObservableAngle() const
Returns the observable angle of the tag.
Definition: tag_entity.h:68
Real GetSideLength() const
Returns the side length of the tag.
Definition: tag_entity.h:76