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 }
virtual void Init(TConfigurationNode &t_tree)
Initializes the resource.
void GetNodeAttribute(TConfigurationNode &t_node, const std::string &str_attribute, T &t_buffer)
Returns the value of a node&#39;s attribute.
A 3D vector class.
Definition: vector3.h:29
#define THROW_ARGOSEXCEPTION_NESTED(message, nested)
This macro throws an ARGoS exception with the passed message and nesting the passed exception...
void GetNodeAttributeOrDefault(TConfigurationNode &t_node, const std::string &str_attribute, T &t_buffer, const T &t_default)
Returns the value of a node&#39;s attribute, or the passed default value.
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\ "returns the X and Y coordinates on the sensor", "This algorithm detects nearby LEDs seen by the camera and\" "returns the X and Y coordinates on the sensor", "Under development")
The exception that wraps all errors in ARGoS.
virtual void Init(TConfigurationNode &t_node)
Initializes the resource.
ticpp::Element TConfigurationNode
The ARGoS configuration XML node.
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)
static CSimulator & GetInstance()
Returns the instance to the CSimulator class.
Definition: simulator.cpp:78
std::vector< std::pair< bool, CRay3 > > m_vecCheckedRays
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