Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Fix omni directional cylindrical test failing #6403

Merged
merged 5 commits into from
Oct 6, 2023
Merged

Fix omni directional cylindrical test failing #6403

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
5 commits
Select commit Hold shift + click to select a range
8b7d67f
changed omnidirectional to be 180°, reprojected backpoints to fix other
FrederikBark Oct 4, 2023
0da04f9
added new object to recognition test to see if objects perpendicular to
FrederikBark Oct 4, 2023
cff2268
updated clang and added new test case for perpendicular object
FrederikBark Oct 5, 2023
183f758
clang changes
FrederikBark Oct 6, 2023
0bb8a06
Merge branch 'master' into fix-omni-directional-cylindrical
FrederikBark Oct 6, 2023
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
9 changes: 5 additions & 4 deletions src/webots/nodes/utils/WbObjectDetection.cpp
View file
Open in desktop
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
Open in desktop
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
Open in desktop
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
Open in desktop
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
Loading