Skip to content

Commit

Permalink
Fix omni directional cylindrical test failing (#6403)
Browse files Browse the repository at this point in the history 
* changed omnidirectional to be 180°, reprojected backpoints to fix other
issue

* added new object to recognition test to see if objects perpendicular to
camera, which are only partially visible are recognized

* updated clang and added new test case for perpendicular object

* clang changes
  • Loading branch information
@FrederikBark
FrederikBark authored Oct 6, 2023
1 parent 677fda7 commit d76e17a
Show file tree
Hide file tree
Showing 4 changed files with 95 additions and 23 deletions.
9 changes: 5 additions & 4 deletions src/webots/nodes/utils/WbObjectDetection.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -37,7 +37,7 @@ WbObjectDetection::WbObjectDetection(WbSolid *device, WbSolid *object, const int
mMaxRange(maxRange),
mOdeGeomData(NULL),
mHorizontalFieldOfView(horizontalFieldOfView),
mIsOmniDirectional(mHorizontalFieldOfView > M_PI_2),
mIsOmniDirectional(mHorizontalFieldOfView > M_PI),
mOcclusion(occlusion) {
if (mOcclusion == ONE_RAY) {
const WbVector3 devicePosition = mDevice->position();
Expand Down Expand Up @@ -322,11 +322,12 @@ bool WbObjectDetection::isWithinBounds(const WbAffinePlane *frustumPlanes, const
return false;
}
}
// add points at the back of the device to ensure the whole object is detected
pointsInFrustum << pointsAtBack;
// move the points in the device referential
for (int i = 0; i < pointsInFrustum.size(); ++i)
pointsInFrustum[i] = deviceInverseRotation * (pointsInFrustum[i] - devicePosition);
// add points at the back of the device to ensure the whole object is detected
pointsInFrustum << pointsAtBack;

double minX = pointsInFrustum[0].x();
double maxX = minX;
double minY = pointsInFrustum[0].y();
Expand Down Expand Up @@ -420,7 +421,7 @@ bool WbObjectDetection::isWithinBounds(const WbAffinePlane *frustumPlanes, const
}

objectRelativePosition = deviceInverseRotation * (objectPosition - devicePosition);
if (!mIsOmniDirectional) {
if (mHorizontalFieldOfView <= M_PI_2) {
// do not recompute the object size and position if partly outside in case of fovX > PI
// (a more complete computation will be needed and currently it seems to work quite well as-is)
objectSize.setY(objectSize.y() - outsidePart[RIGHT] - outsidePart[LEFT]);
Expand Down
2 changes: 1 addition & 1 deletion src/webots/nodes/utils/WbObjectDetection.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -98,7 +98,7 @@ class WbObjectDetection {
QList<double> mRaysCollisionDepth; // rays collision depth
QList<dGeomID> mRayGeoms; // rays that checks collision of this packet
double mHorizontalFieldOfView;
bool mIsOmniDirectional; // is sensor omnidirectional (horizontal FOV > PI/2)
bool mIsOmniDirectional; // is sensor omnidirectional (horizontal FOV > PI)
int mOcclusion;
};

Expand Down
85 changes: 67 additions & 18 deletions tests/api/controllers/camera_recognition/camera_recognition.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -9,15 +9,16 @@
#include "../../../lib/ts_utils.h"

#define TIME_STEP 32
#define VISIBLE_SOLID_NUMBER 6
#define VISIBLE_SOLID_NUMBER 7

// This test is mainly testing the functionalities for a planar camera.
// Some basic tests for spherical and cylindrical cameras are also performed checking mainly
// the number of visible objects and the position on camera.

// objects visible in planar camera
static const char *visible_solid_models[VISIBLE_SOLID_NUMBER] = {
"visible sphere", "visible box", "sub solid", "visible capsule", "composed solid", "visible sphere without BO"};
"visible sphere", "visible box", "sub solid", "visible capsule", "composed solid", "visible sphere without BO",
"perpendicular box"};

static const char *occcluded_solid_model = "occluded box";

Expand Down Expand Up @@ -45,13 +46,16 @@ int main(int argc, char **argv) {
VISIBLE_SOLID_NUMBER + 1, object_number);

object_number = wb_camera_recognition_get_number_of_objects(camera_spherical);
ts_assert_int_equal(object_number, 10, "The spherical camera should initially see %d objects and not %d (with occlusion).", 9,
object_number);
ts_assert_int_equal(object_number, VISIBLE_SOLID_NUMBER + 4,
"The spherical camera should initially see %d objects"
" and not %d (with occlusion).",
VISIBLE_SOLID_NUMBER + 4, object_number);

object_number = wb_camera_recognition_get_number_of_objects(camera_cylindrical);
ts_assert_int_equal(object_number, 8, "The cylindrical camera should initially see %d objects and not %d (with occlusion).",
7, object_number);

ts_assert_int_equal(object_number, VISIBLE_SOLID_NUMBER + 2,
"The cylindrical camera should initially see %d objects"
" and not %d (with occlusion).",
VISIBLE_SOLID_NUMBER + 2, object_number);
// enable occlusion
WbNodeRef recognition_node = wb_supervisor_node_get_from_def("RECOGNITION");
WbFieldRef occlusion_field = wb_supervisor_node_get_field(recognition_node, "occlusion");
Expand All @@ -60,7 +64,7 @@ int main(int argc, char **argv) {
ts_assert_boolean_equal(wb_camera_recognition_has_segmentation(camera),
"The Recognition.segmentation field should be set to TRUE.");
const unsigned char *image = wb_camera_recognition_get_segmentation_image(camera);
ts_assert_boolean_equal(image == NULL, "No segmentation image should be returned if segmentaton is disabled.");
ts_assert_boolean_equal(image == NULL, "No segmentation image should be returned if segmentation is disabled.");

wb_robot_step(TIME_STEP);

Expand Down Expand Up @@ -137,7 +141,7 @@ int main(int argc, char **argv) {
objects[i].position_on_image[0], objects[i].position_on_image[1], expected_position_on_image[0],
expected_position_on_image[1]);
}
// check objct is one of the visible solid
// check object is one of the visible solid
bool found = false;
for (j = 0; j < VISIBLE_SOLID_NUMBER; ++j) {
if (strcmp(objects[i].model, visible_solid_models[j]) == 0)
Expand All @@ -158,6 +162,50 @@ int main(int argc, char **argv) {
ts_assert_double_is_bigger(composed_solid_size, sub_solid_size, "Object: '%s' should have a bigger pixel size than '%s'.",
objects[composed_solid_index].model, objects[sub_solid_index].model);

// check if perpendicular object is recognized correctly
const double perpendicular_box_position[3] = {-0.528470, -1.191827, -0.105723};
const double perpendicular_box_orientation[4] = {0.577352, -0.577349, 0.577350, 2.094393};
// case spherical
object_number = wb_camera_recognition_get_number_of_objects(camera_spherical);
objects = wb_camera_recognition_get_objects(camera_spherical);

for (i = 0; i < object_number; ++i) {
if (strcmp(objects[i].model, "perpendicular box") == 0) {
ts_assert_doubles_in_delta(3, objects[i].position, perpendicular_box_position, 0.001,
"Position of 'perpendicular box' is not correct for spherical camera: found=("
"%f, %f, %f), expected=(%f, %f, %f).",
objects[i].position[0], objects[i].position[1], objects[i].position[2],
perpendicular_box_position[0], perpendicular_box_position[1], perpendicular_box_position[2]);
// orientation
ts_assert_doubles_in_delta(4, objects[i].orientation, perpendicular_box_orientation, 0.001,
"Orientation of 'perpendicular box' is not correct for spherical camera: found=("
"%f, %f, %f, %f), expected=(%f, %f, %f, %f).",
objects[i].orientation[0], objects[i].orientation[1], objects[i].orientation[2],
objects[i].orientation[3], perpendicular_box_orientation[0], perpendicular_box_orientation[1],
perpendicular_box_orientation[2], perpendicular_box_orientation[3]);
}
}
// case cylindrical
object_number = wb_camera_recognition_get_number_of_objects(camera_cylindrical);
objects = wb_camera_recognition_get_objects(camera_cylindrical);
for (i = 0; i < object_number; ++i) {
if (strcmp(objects[i].model, "perpendicular box") == 0) {
// position
ts_assert_doubles_in_delta(3, objects[i].position, perpendicular_box_position, 0.001,
"Position of 'perpendicular box' is not correct for cylindrical camera: found=("
"%f, %f, %f), expected=(%f, %f, %f).",
objects[i].position[0], objects[i].position[1], objects[i].position[2],
perpendicular_box_position[0], perpendicular_box_position[1], perpendicular_box_position[2]);
// orientation
ts_assert_doubles_in_delta(4, objects[i].orientation, perpendicular_box_orientation, 0.001,
"Orientation of 'perpendicular box' is not correct for cylindrical camera: found=("
"%f, %f, %f, %f), expected=(%f, %f, %f, %f).",
objects[i].orientation[0], objects[i].orientation[1], objects[i].orientation[2],
objects[i].orientation[3], perpendicular_box_orientation[0], perpendicular_box_orientation[1],
perpendicular_box_orientation[2], perpendicular_box_orientation[3]);
}
}

wb_robot_step(TIME_STEP);

object_number = wb_camera_recognition_get_number_of_objects(camera);
Expand Down Expand Up @@ -212,6 +260,7 @@ int main(int argc, char **argv) {
const double invisible_capsule_position[3] = {0.369, 1.650, 0.899};
const double invisible_capsule_orientation[4] = {0.577350, -0.577350, -0.577350, 2.094390};
const double invisible_capsule_size[2] = {0.1, 0.1};

object_number = wb_camera_recognition_get_number_of_objects(camera_spherical);
ts_assert_int_equal(object_number, 4,
"The spherical camera should see only 4 objects after removal of the initial objects and not %d.",
Expand Down Expand Up @@ -266,32 +315,32 @@ int main(int argc, char **argv) {
// position
ts_assert_doubles_in_delta(
3, objects[i].position, invisible_capsule_position, 0.001,
"Position of 'invisble capsule' is not correct for spherical camera: found=(%f, %f, %f), expected=(%f, %f, %f).",
"Position of 'invisible capsule' is not correct for spherical camera: found=(%f, %f, %f), expected=(%f, %f, %f).",
objects[i].position[0], objects[i].position[1], objects[i].position[2], invisible_capsule_position[0],
invisible_capsule_position[1], invisible_capsule_position[2]);
// orientation
ts_assert_doubles_in_delta(4, objects[i].orientation, invisible_capsule_orientation, 0.001,
"Orientation of 'invisble capsule' is not correct for spherical camera: found=(%f, %f, %f, "
"Orientation of 'invisible capsule' is not correct for spherical camera: found=(%f, %f, %f, "
"%f), expected=(%f, %f, %f, %f).",
objects[i].orientation[0], objects[i].orientation[1], objects[i].orientation[2],
objects[i].orientation[3], invisible_capsule_orientation[0], invisible_capsule_orientation[1],
invisible_capsule_orientation[2], invisible_capsule_orientation[3]);
// size
ts_assert_doubles_in_delta(
2, objects[i].size, invisible_capsule_size, 0.001,
"Size of 'invisble capsule' is not correct for spherical camera: found=(%f, %f), expected=(%f, %f).",
"Size of 'invisible capsule' is not correct for spherical camera: found=(%f, %f), expected=(%f, %f).",
objects[i].size[0], objects[i].size[1], invisible_capsule_size[0], invisible_capsule_size[1]);
// size on image
int expected_size_on_image[2] = {14, 28};
ts_assert_integers_in_delta(
2, objects[i].size_on_image, expected_size_on_image, 1,
"Size on image of 'invisble capsule' is not correct for spherical camera: found=(%d, %d), expected=(%d, %d).",
"Size on image of 'invisible capsule' is not correct for spherical camera: found=(%d, %d), expected=(%d, %d).",
objects[i].size_on_image[0], objects[i].size_on_image[1], expected_size_on_image[0], expected_size_on_image[1]);
// position on image
int expected_position_on_image[2] = {79, 102};
ts_assert_integers_in_delta(
2, objects[i].position_on_image, expected_position_on_image, 1,
"Position on image of 'invisble capsule' is not correct for spherical camera: found=(%d, %d), expected=(%d, %d).",
"Position on image of 'invisible capsule' is not correct for spherical camera: found=(%d, %d), expected=(%d, %d).",
objects[i].position_on_image[0], objects[i].position_on_image[1], expected_position_on_image[0],
expected_position_on_image[1]);
}
Expand All @@ -315,27 +364,27 @@ int main(int argc, char **argv) {
invisible_capsule_position[1], invisible_capsule_position[2]);
// orientation
ts_assert_doubles_in_delta(4, objects[i].orientation, invisible_capsule_orientation, 0.001,
"Orientation of 'invisble capsule' is not correct for cylindrical camera: found=(%f, %f, %f, "
"Orientation of 'invisible capsule' is not correct for cylindrical camera: found=(%f, %f, %f, "
"%f), expected=(%f, %f, %f, %f).",
objects[i].orientation[0], objects[i].orientation[1], objects[i].orientation[2],
objects[i].orientation[3], invisible_capsule_orientation[0], invisible_capsule_orientation[1],
invisible_capsule_orientation[2], invisible_capsule_orientation[3]);
// size
ts_assert_doubles_in_delta(
2, objects[i].size, invisible_capsule_size, 0.001,
"Size of 'invisble capsule' is not correct for cylindrical camera: found=(%f, %f), expected=(%f, %f).",
"Size of 'invisible capsule' is not correct for cylindrical camera: found=(%f, %f), expected=(%f, %f).",
objects[i].size[0], objects[i].size[1], invisible_capsule_size[0], invisible_capsule_size[1]);
// size on image
int expected_size_on_image[2] = {6, 45};
ts_assert_integers_in_delta(
2, objects[i].size_on_image, expected_size_on_image, 1,
"Size on image of 'invisble capsule' is not correct for cylindrical camera: found=(%d, %d), expected=(%d, %d).",
"Size on image of 'invisible capsule' is not correct for cylindrical camera: found=(%d, %d), expected=(%d, %d).",
objects[i].size_on_image[0], objects[i].size_on_image[1], expected_size_on_image[0], expected_size_on_image[1]);
// position on image
int expected_position_on_image[2] = {12, 88};
ts_assert_integers_in_delta(
2, objects[i].position_on_image, expected_position_on_image, 1,
"Position on image of 'invisble capsule' is not correct for cylindrical camera: found=(%d, %d), expected=(%d, %d).",
"Position on image of 'invisible capsule' is not correct for cylindrical camera: found=(%d, %d), expected=(%d, %d).",
objects[i].position_on_image[0], objects[i].position_on_image[1], expected_position_on_image[0],
expected_position_on_image[1]);

Expand Down
22 changes: 22 additions & 0 deletions tests/api/worlds/camera_recognition.wbt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -288,6 +288,28 @@ Solid {
0 0 1
]
}
Solid {
translation -1.0 1.6 -2.25
rotation 0.599079 0.531234 0.599078 -2.82327
children [
Shape {
appearance Appearance {
material Material {
diffuseColor 0.756863 0.207843 0.207843
}
}
geometry DEF BOX Box {
size 0.9 1.9 0.1
}
}
]
name "solid(10)"
model "perpendicular box"
boundingObject USE BOX
recognitionColors [
0.756863 0.207843 0.207843
]
}
Robot {
translation -0.116489 0.196931 -0.132093
rotation 0.6457879227325559 0.5398879322844559 0.5398879322844559 1.36968
Expand Down

0 comments on commit d76e17a

Please sign in to comment.