8 #include <argos3/core/simulator/simulator.h>
9 #include <argos3/core/simulator/entity/composable_entity.h>
10 #include <argos3/plugins/robots/generic/simulator/camera_sensor_algorithm.h>
18 m_bShowFrustum(false),
19 m_pcEmbodiedEntity(nullptr),
20 m_pcControllableEntity(nullptr) {}
42 for(itCamera = itCamera.begin(&t_tree);
43 itCamera != itCamera.end();
49 std::string strAnchorId;
67 cProjectionMatrix(0,0) = cFocalLength.
GetX();
68 cProjectionMatrix(1,1) = cFocalLength.
GetY();
72 cProjectionMatrix(0,2) = cPrinciplePoint.
GetX();
73 cProjectionMatrix(1,2) = cPrinciplePoint.
GetY();
89 std::vector<CCameraSensorSimulatedAlgorithm*> vecSimulatedAlgorithms;
90 std::vector<CCI_CameraSensorAlgorithm*> vecAlgorithms;
92 for(itAlgorithm = itAlgorithm.begin(&(*itCamera));
93 itAlgorithm != itAlgorithm.end();
101 if(pcCIAlgorithm ==
nullptr) {
103 "\" does not inherit from CCI_CameraSensorAlgorithm");
106 pcCIAlgorithm->Init(*itAlgorithm);
108 vecSimulatedAlgorithms.push_back(pcAlgorithm);
109 vecAlgorithms.push_back(pcCIAlgorithm);
112 m_vecSensors.emplace_back(sAnchor, cOffset, cRange, cProjectionMatrix,
113 cResolution, vecSimulatedAlgorithms);
129 std::vector<std::pair<bool, CRay3> >& vecCheckedRays =
136 CVector3 cCameraLocation, cLookAt, cUp;
138 CVector3 cNearCenter, cNearTopLeft, cNearTopRight, cNearBottomLeft, cNearBottomRight;
139 CVector3 cFarCenter, cFarTopLeft, cFarTopRight, cFarBottomLeft, cFarBottomRight;
140 std::array<CPlane, 6> arrFrustumPlanes;
141 CVector3 cBoundingBoxMinCorner, cBoundingBoxMaxCorner;
142 CVector3 cBoundingBoxPosition, cBoundingBoxHalfExtents;
146 cWorldToAnchorTransform.
SetFromComponents(s_sensor.Anchor.Orientation, s_sensor.Anchor.Position);
147 cWorldToCameraTransform = cWorldToAnchorTransform * s_sensor.Offset;
148 cCameraToWorldTransform = cWorldToCameraTransform.
GetInverse();
155 cZ = cCameraLocation - cLookAt;
163 cNearCenter = cCameraLocation - cZ * s_sensor.Range.GetMin();
164 cFarCenter = cCameraLocation - cZ * s_sensor.Range.GetMax();
165 cNearTopLeft = cNearCenter + (cY * s_sensor.NearPlaneHeight) - (cX * s_sensor.NearPlaneWidth);
166 cNearTopRight = cNearCenter + (cY * s_sensor.NearPlaneHeight) + (cX * s_sensor.NearPlaneWidth);
167 cNearBottomLeft = cNearCenter - (cY * s_sensor.NearPlaneHeight) - (cX * s_sensor.NearPlaneWidth);
168 cNearBottomRight = cNearCenter - (cY * s_sensor.NearPlaneHeight) + (cX * s_sensor.NearPlaneWidth);
169 cFarTopLeft = cFarCenter + (cY * s_sensor.FarPlaneHeight) - (cX * s_sensor.FarPlaneWidth);
170 cFarTopRight = cFarCenter + (cY * s_sensor.FarPlaneHeight) + (cX * s_sensor.FarPlaneWidth);
171 cFarBottomLeft = cFarCenter - (cY * s_sensor.FarPlaneHeight) - (cX * s_sensor.FarPlaneWidth);
172 cFarBottomRight = cFarCenter - (cY * s_sensor.FarPlaneHeight) + (cX * s_sensor.FarPlaneWidth);
175 vecCheckedRays.emplace_back(
false,
CRay3(cNearTopLeft, cNearTopRight));
176 vecCheckedRays.emplace_back(
false,
CRay3(cNearTopRight, cNearBottomRight));
177 vecCheckedRays.emplace_back(
false,
CRay3(cNearBottomRight, cNearBottomLeft));
178 vecCheckedRays.emplace_back(
false,
CRay3(cNearBottomLeft, cNearTopLeft));
179 vecCheckedRays.emplace_back(
false,
CRay3(cFarTopLeft, cFarTopRight));
180 vecCheckedRays.emplace_back(
false,
CRay3(cFarTopRight, cFarBottomRight));
181 vecCheckedRays.emplace_back(
false,
CRay3(cFarBottomRight, cFarBottomLeft));
182 vecCheckedRays.emplace_back(
false,
CRay3(cFarBottomLeft, cFarTopLeft));
183 vecCheckedRays.emplace_back(
false,
CRay3(cNearTopLeft, cFarTopLeft));
184 vecCheckedRays.emplace_back(
false,
CRay3(cNearTopRight, cFarTopRight));
185 vecCheckedRays.emplace_back(
false,
CRay3(cNearBottomRight, cFarBottomRight));
186 vecCheckedRays.emplace_back(
false,
CRay3(cNearBottomLeft, cFarBottomLeft));
190 cBoundingBoxMinCorner = cNearCenter;
191 cBoundingBoxMaxCorner = cNearCenter;
193 cNearTopLeft, cNearTopRight, cNearBottomLeft, cNearBottomRight,
194 cFarTopLeft, cFarTopRight, cFarBottomLeft, cFarBottomRight
196 if(c_point.GetX() > cBoundingBoxMaxCorner.
GetX()) {
197 cBoundingBoxMaxCorner.
SetX(c_point.GetX());
199 if(c_point.GetX() < cBoundingBoxMinCorner.
GetX()) {
200 cBoundingBoxMinCorner.
SetX(c_point.GetX());
202 if(c_point.GetY() > cBoundingBoxMaxCorner.
GetY()) {
203 cBoundingBoxMaxCorner.
SetY(c_point.GetY());
205 if(c_point.GetY() < cBoundingBoxMinCorner.
GetY()) {
206 cBoundingBoxMinCorner.
SetY(c_point.GetY());
208 if(c_point.GetZ() > cBoundingBoxMaxCorner.
GetZ()) {
209 cBoundingBoxMaxCorner.
SetZ(c_point.GetZ());
211 if(c_point.GetZ() < cBoundingBoxMinCorner.
GetZ()) {
212 cBoundingBoxMinCorner.
SetZ(c_point.GetZ());
215 cBoundingBoxMaxCorner *= 0.5;
216 cBoundingBoxMinCorner *= 0.5;
217 cBoundingBoxPosition = (cBoundingBoxMaxCorner + cBoundingBoxMinCorner);
218 cBoundingBoxHalfExtents = (cBoundingBoxMaxCorner - cBoundingBoxMinCorner);
220 arrFrustumPlanes[0].SetFromThreePoints(cNearTopRight, cNearTopLeft, cFarTopLeft);
221 arrFrustumPlanes[1].SetFromThreePoints(cNearBottomLeft, cNearBottomRight, cFarBottomRight);
222 arrFrustumPlanes[2].SetFromThreePoints(cNearTopLeft, cNearBottomLeft, cFarBottomLeft);
223 arrFrustumPlanes[3].SetFromThreePoints(cNearBottomRight, cNearTopRight, cFarBottomRight);
224 arrFrustumPlanes[4].SetFromThreePoints(cNearTopLeft, cNearTopRight, cNearBottomRight);
225 arrFrustumPlanes[5].SetFromThreePoints(cFarTopRight, cFarTopLeft, cFarBottomLeft);
228 pc_algorithm->
Update(s_sensor.ProjectionMatrix,
230 cCameraToWorldTransform,
232 cBoundingBoxPosition,
233 cBoundingBoxHalfExtents);
235 vecCheckedRays.insert(std::end(vecCheckedRays),
246 "cameras",
"default",
247 "Michael Allwright [allsey87@gmail.com]",
249 "A generic multi-camera sensor capable of running various algorithms",
250 "The generic multi-camera sensor can be attached to any composable entity in\n"
251 "ARGoS that contains an embodied entity with at least one anchor. The sensor\n"
252 "can be initialized with a number of cameras each running different algorithms\n"
253 "for detecting different objects in the simulation. The sensor is designed so\n"
254 "that algorithms can project a feature in the simulation on to the virtual\n"
255 "sensor and store its 2D pixel coordinates as a reading. The implementation\n"
256 "of algorithms that behave differently, however, is also possible.\n\n"
257 "REQUIRED XML CONFIGURATION\n\n"
260 " <my_controller ...>\n"
264 " <cameras implementation=\"default\"/>\n"
268 " </my_controller>\n"
270 " </controllers>\n\n"
271 "OPTIONAL XML CONFIGURATION\n\n"
272 "It is possible to draw the frustum of each camera sensor in the OpenGL\n"
273 "visualization. This can be useful for sensor debugging but also to understand\n"
274 "what's wrong in your controller. To turn this functionality on, add the\n"
275 "attribute \"show_frustum\" as follows:\n\n"
278 " <my_controller ...>\n"
282 " <cameras implementation=\"default\" show_frustum=\"true\"/>\n"
286 " </my_controller>\n"
288 " </controllers>\n\n"
289 "To add a camera to the plugin, create a camera node as shown in the following\n"
290 "example. A camera is defined by its range (how close and how far the camera\n"
291 "can see), its anchor and its position and orientation offsets from that\n"
292 "that anchor, its focal length and principle point (which define the\n"
293 "projection matrix), and its resolution.\n\n"
296 " <my_controller ...>\n"
300 " <cameras implementation=\"default\" show_frustum=\"true\">\n"
301 " <camera id=\"camera0\" range=\"0.025:0.25\" anchor=\"origin\"\n"
302 " position=\"0.1,0,0.1\" orientation=\"90,-90,0\"\n"
303 " focal_length=\"800,800\" principle_point=\"320,240\"\n"
304 " resolution=\"640,480\"/>\n"
309 " </my_controller>\n"
311 " </controllers>\n\n"
312 "To run an algorithm on the camera sensor, simply add the algorithm as a node\n"
313 "under the camera node. At the time of writing, three algorithms are available\n"
314 "by default: led_detector, directional_led_detector, and tag_detector. Each of\n"
315 "algorithms requires a medium attribute that specifies the medium where the\n"
316 "target entities are indexed. By setting the show_rays attribute to true, you\n"
317 "can see whether or not a target was partially occluded by another object in\n"
318 "the simulation. For example:\n\n"
321 " <my_controller ...>\n"
325 " <cameras implementation=\"default\" show_frustum=\"true\">\n"
326 " <camera id=\"camera0\" range=\"0.025:0.25\" anchor=\"origin\"\n"
327 " position=\"0.1,0,0.1\" orientation=\"90,-90,0\"\n"
328 " focal_length=\"800,800\" principle_point=\"320,240\"\n"
329 " resolution=\"640,480\">\n"
330 " <led_detector medium=\"leds\" show_rays=\"true\"/>\n"
336 " </my_controller>\n"
An entity that contains a pointer to the user-defined controller.
CSquareMatrix< DIM > GetInverse() const
std::vector< std::pair< bool, CRay3 > > & GetCheckedRays()
Returns the list of checked rays.
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.
#define THROW_ARGOSEXCEPTION(message)
This macro throws an ARGoS exception with the passed message.
Real GetX() const
Returns the x coordinate of this vector.
virtual void Init(TConfigurationNode &t_tree)
Initializes the sensor from the XML configuration tree.
Real GetX() const
Returns the x coordinate of this vector.
CVector3 & Rotate(const CQuaternion &c_quaternion)
Rotates this vector by the given quaternion.
Real GetY() const
Returns the y coordinate of this vector.
CRotationMatrix3 GetRotationMatrix() const
virtual void SetRobot(CComposableEntity &c_entity)
Sets the entity associated to this sensor.
CEmbodiedEntity * m_pcEmbodiedEntity
CVector3 GetTranslationVector() const
ticpp::Element TConfigurationNode
The ARGoS configuration XML node.
Real GetY() const
Returns the y coordinate of this vector.
This entity is a link to a body in the physics engine.
#define THROW_ARGOSEXCEPTION_NESTED(message, nested)
This macro throws an ARGoS exception with the passed message and nesting the passed exception...
CControllableEntity * m_pcControllableEntity
SInterface::TVector m_vecInterfaces
void SetFromComponents(const CRotationMatrix3 &c_rotation, const CVector3 &c_translation)
Basic class for an entity that contains other entities.
const std::vector< std::pair< bool, CRay3 > > & GetCheckedRays() const
static TYPE * New(const std::string &str_label)
Creates a new object of type TYPE
void SetX(const Real f_x)
Sets the x coordinate of this vector.
An anchor related to the body of an entity.
std::vector< SSensor > m_vecSensors
virtual void Init(TConfigurationNode &t_node)
Initializes the sensor from the XML configuration tree.
ticpp::Iterator< ticpp::Element > TConfigurationNodeIterator
The iterator for the ARGoS configuration XML node.
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.
static const CVector3 Z
The z axis.
void SetY(const Real f_y)
Sets the y coordinate of this vector.
CVector3 & CrossProduct(const CVector3 &c_vector3)
Calculates the cross product between this vector and the passed one.
const SAnchor & GetAnchor(const std::string &str_id) const
Returns the wanted anchor as a const reference.
virtual void Update()
Updates the state of the entity associated to this sensor.
The namespace containing all the ARGoS related code.
Real GetZ() const
Returns the z coordinate of this vector.
void SetZ(const Real f_z)
Sets the z coordinate of this vector.
CVector3 & Normalize()
Normalizes this vector.
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
CEntity & GetComponent(const std::string &str_component)
Returns the component with the passed string label.
REGISTER_SENSOR(CEyeBotLightRotZOnlySensor,"eyebot_light","rot_z_only","Carlo Pinciroli [ilpincy@gmail.com]","1.0","The eye-bot light sensor (optimized for 2D).","This sensor accesses a set of light sensors. The sensors all return a value\n""between 0 and 1, where 0 means nothing within range and 1 means the perceived\n""light saturates the sensor. Values between 0 and 1 depend on the distance of\n""the perceived light. Each reading R is calculated with R=(I/x)^2, where x is the\n""distance between a sensor and the light, and I is the reference intensity of the\n""perceived light. The reference intensity corresponds to the minimum distance at\n""which the light saturates a sensor. The reference intensity depends on the\n""individual light, and it is set with the \"intensity\" attribute of the light\n""entity. In case multiple lights are present in the environment, each sensor\n""reading is calculated as the sum of the individual readings due to each light.\n""In other words, light wave interference is not taken into account. In\n""controllers, you must include the ci_light_sensor.h header.\n\n""REQUIRED XML CONFIGURATION\n\n"" <controllers>\n"" ...\n"" <my_controller ...>\n"" ...\n"" <sensors>\n"" ...\n"" <eyebot_light implementation=\"rot_z_only\" />\n"" ...\n"" </sensors>\n"" ...\n"" </my_controller>\n"" ...\n"" </controllers>\n\n""OPTIONAL XML CONFIGURATION\n\n""It is possible to draw the rays shot by the light sensor in the OpenGL\n""visualization. This can be useful for sensor debugging but also to understand\n""what's wrong in your controller. In OpenGL, the rays are drawn in cyan when\n""they are not obstructed and in purple when they are. In case a ray is\n""obstructed, a black dot is drawn where the intersection occurred.\n""To turn this functionality on, add the attribute \"show_rays\" as in this\n""example:\n\n"" <controllers>\n"" ...\n"" <my_controller ...>\n"" ...\n"" <sensors>\n"" ...\n"" <eyebot_light implementation=\"rot_z_only\"\n"" show_rays=\"true\" />\n"" ...\n"" </sensors>\n"" ...\n"" </my_controller>\n"" ...\n"" </controllers>\n\n""It is possible to add uniform noise to the sensors, thus matching the\n""characteristics of a real robot better. This can be done with the attribute\n""\"noise_level\", whose allowed range is in [-1,1] and is added to the calculated\n""reading. The final sensor reading is always normalized in the [0-1] range.\n\n"" <controllers>\n"" ...\n"" <my_controller ...>\n"" ...\n"" <sensors>\n"" ...\n"" <eyebot_light implementation=\"rot_z_only\"\n"" noise_level=\"0.1\" />\n"" ...\n"" </sensors>\n"" ...\n"" </my_controller>\n"" ...\n"" </controllers>\n\n""OPTIONAL XML CONFIGURATION\n\n""None.\n","Usable")