camera_sensor_algorithm.h
Go to the documentation of this file.
1 
7 #ifndef CAMERAS_SENSOR_ALGORITHM_H
8 #define CAMERAS_SENSOR_ALGORITHM_H
9 
10 namespace argos {
11  class CCameraSensorSimulatedAlgorithm;
12 }
13 
14 #include <argos3/core/utility/plugins/factory.h>
15 #include <argos3/core/utility/math/vector3.h>
16 #include <argos3/core/utility/math/plane.h>
17 #include <argos3/core/utility/math/matrix/transformationmatrix3.h>
18 #include <argos3/core/simulator/entity/positional_entity.h>
19 #include <array>
20 
21 namespace argos {
22 
24 
25  public:
26 
28 
29  public:
30 
31  CBaseUpdateOperation(const CSquareMatrix<3>& c_projection_matrix,
32  const std::array<CPlane, 6>& arr_frustum_planes,
33  const CTransformationMatrix3& c_world_to_camera_transform,
34  const CVector3& c_camera_location) :
35  m_cProjectionMatrix(c_projection_matrix),
36  m_arrFrustumPlanes(arr_frustum_planes),
37  m_cWorldToCameraTransform(c_world_to_camera_transform),
38  m_cCameraLocation(c_camera_location) {}
39 
40  virtual ~CBaseUpdateOperation() {}
41 
43  CVector3 cEntityToCamera(m_cCameraLocation - c_entity.GetPosition());
44  CVector3 cEntityDirection(CVector3::Z);
45  cEntityDirection.Rotate(c_entity.GetOrientation());
46  Real fDotProduct = cEntityDirection.DotProduct(cEntityToCamera);
47  return ACos(fDotProduct / (cEntityDirection.Length() * cEntityToCamera.Length()));
48  }
49 
50  CVector2 ProjectOntoSensor(const CVector3& c_vector) const {
51  CVector3 cCameraToEntityTranslation(m_cWorldToCameraTransform * c_vector);
52  /* this could be avoided if CVector3 inherited from CMatrix<3,1> */
53  CMatrix<3,1> cCameraToEntityTranslationMatrix;
54  cCameraToEntityTranslationMatrix(0,0) =
55  cCameraToEntityTranslation.GetX() / cCameraToEntityTranslation.GetZ();
56  cCameraToEntityTranslationMatrix(1,0) =
57  cCameraToEntityTranslation.GetY() / cCameraToEntityTranslation.GetZ();
58  cCameraToEntityTranslationMatrix(2,0) = 1.0f;
59  /* get image coordinates */
60  CMatrix<3,1> cImageCoordinates(m_cProjectionMatrix * cCameraToEntityTranslationMatrix);
61  /* return as vector2 */
62  return CVector2(cImageCoordinates(0,0), cImageCoordinates(1,0));
63  }
64 
65  bool IsPointInsideFrustum(const CVector3& c_point) const {
66  for(const CPlane& c_plane : m_arrFrustumPlanes) {
67  if(c_plane.GetNormal().DotProduct(c_point - c_plane.GetPosition()) < 0.0) {
68  return false;
69  }
70  }
71  return true;
72  }
73 
74  protected:
75 
77  const std::array<CPlane, 6>& m_arrFrustumPlanes;
80  };
81 
82  public:
83 
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) = 0;
92 
93  const std::vector<std::pair<bool, CRay3> >& GetCheckedRays() const {
94  return m_vecCheckedRays;
95  }
96 
97  protected:
98 
99  std::vector<std::pair<bool, CRay3> > m_vecCheckedRays;
100 
101  };
102 }
103 
104 #define REGISTER_CAMERA_SENSOR_ALGORITHM(CLASSNAME, \
105  LABEL, \
106  AUTHOR, \
107  VERSION, \
108  BRIEF_DESCRIPTION, \
109  LONG_DESCRIPTION, \
110  STATUS) \
111  REGISTER_SYMBOL(CCameraSensorSimulatedAlgorithm, \
112  CLASSNAME, \
113  LABEL, \
114  AUTHOR, \
115  VERSION, \
116  BRIEF_DESCRIPTION, \
117  LONG_DESCRIPTION, \
118  STATUS)
119 
120 #endif
CRadians GetAngleWithCamera(const CPositionalEntity &c_entity) const
A 3D vector class.
Definition: vector3.h:29
float Real
Collects all ARGoS code.
Definition: datatypes.h:39
Real GetX() const
Returns the x coordinate of this vector.
Definition: vector3.h:93
Real GetY() const
Returns the y coordinate of this vector.
Definition: vector3.h:109
CBaseUpdateOperation(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 CQuaternion & GetOrientation() const
const CVector3 & GetPosition() const
It defines the basic type CRadians, used to store an angle value in radians.
Definition: angles.h:42
const std::vector< std::pair< bool, CRay3 > > & GetCheckedRays() const
A 2D vector class.
Definition: vector2.h:25
static const CVector3 Z
The z axis.
Definition: vector3.h:40
The namespace containing all the ARGoS related code.
Definition: ci_actuator.h:12
Real GetZ() const
Returns the z coordinate of this vector.
Definition: vector3.h:125
CRadians ACos(Real f_value)
Computes the arccosine of the passed value.
Definition: angles.h:622
std::vector< std::pair< bool, CRay3 > > m_vecCheckedRays
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)=0