camera_sensor_tag_detector_algorithm.cpp
Go to the documentation of this file.
1 
8 
9 #include <argos3/core/simulator/simulator.h>
10 #include <argos3/core/utility/math/matrix/squarematrix.h>
11 #include <argos3/core/utility/math/matrix/transformationmatrix3.h>
12 
13 #include <argos3/plugins/simulator/media/tag_medium.h>
14 #include <argos3/plugins/simulator/entities/tag_entity.h>
15 
16 namespace argos {
17 
18  /****************************************/
19  /****************************************/
20 
22  m_bShowRays(false),
23  m_pcTagIndex(nullptr) {}
24 
25  /****************************************/
26  /****************************************/
27 
29  try {
30  /* Parent class init */
32  /* Show rays? */
33  GetNodeAttributeOrDefault(t_tree, "show_rays", m_bShowRays, m_bShowRays);
34  /* Get tag medium from id specified in the XML */
35  std::string strMedium;
36  GetNodeAttribute(t_tree, "medium", strMedium);
37  m_pcTagIndex = &(CSimulator::GetInstance().GetMedium<CTagMedium>(strMedium).GetIndex());
38  }
39  catch(CARGoSException& ex) {
40  THROW_ARGOSEXCEPTION_NESTED("Error initializing the tag detector algorithm", ex);
41  }
42  }
43 
44  /****************************************/
45  /****************************************/
46 
48  const std::array<CPlane, 6>& arr_frustum_planes,
49  const CTransformationMatrix3& c_world_to_camera_transform,
50  const CVector3& c_camera_location,
51  const CVector3& c_bounding_box_position,
52  const CVector3& c_bounding_box_half_extents) {
53  /* Define a class for the update operation */
54  CUpdateOperation cUpdateOperation(c_projection_matrix, arr_frustum_planes,
55  c_world_to_camera_transform, c_camera_location,
56  *this);
57  /* Clear out readings from last update */
58  m_vecReadings.clear();
59  /* Clear out checked rays from last update */
60  m_vecCheckedRays.clear();
61  /* Run the operation */
62  m_pcTagIndex->ForEntitiesInBoxRange(c_bounding_box_position,
63  c_bounding_box_half_extents,
64  cUpdateOperation);
65  }
66 
67  /****************************************/
68  /****************************************/
69 
71  "tag_detector",
72  "Michael Allwright [allsey87@gmail.com]",
73  "1.0",
74  "This algorithm detects nearby tags seen by the camera and\n"
75  "returns the coordinates of their corners to the sensor",
76  "This algorithm detects nearby tags seen by the camera and\n"
77  "returns the coordinates of their corners to the sensor",
78  "Under development");
79 }
A 3D vector class.
Definition: vector3.h:29
void GetNodeAttributeOrDefault(TConfigurationNode &t_node, const std::string &str_attribute, T &t_buffer, const T &t_default)
Returns the value of a node's attribute, or the passed default value.
virtual void Init(TConfigurationNode &t_tree)
Initializes the resource.
ticpp::Element TConfigurationNode
The ARGoS configuration XML node.
#define THROW_ARGOSEXCEPTION_NESTED(message, nested)
This macro throws an ARGoS exception with the passed message and nesting the passed exception...
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)
virtual void Init(TConfigurationNode &t_node)
Initializes the resource.
void GetNodeAttribute(TConfigurationNode &t_node, const std::string &str_attribute, T &t_buffer)
Returns the value of a node's attribute.
The exception that wraps all errors in ARGoS.
This class provides the most general interface to a camera.
T & GetMedium(const std::string &str_id)
Returns a reference to a medium.
Definition: simulator.h:129
The namespace containing all the ARGoS related code.
Definition: ci_actuator.h:12
REGISTER_CAMERA_SENSOR_ALGORITHM(CCameraSensorDirectionalLEDDetectorAlgorithm,"directional_led_detector","Michael Allwright [allsey87@gmail.com]","1.0","This algorithm detects nearby LEDs seen by the camera and\n""returns the X and Y coordinates on the sensor","This algorithm detects nearby LEDs seen by the camera and\n""returns the X and Y coordinates on the sensor","Under development")
std::vector< std::pair< bool, CRay3 > > m_vecCheckedRays
static CSimulator & GetInstance()
Returns the instance to the CSimulator class.
Definition: simulator.cpp:78