From 8b7d67fde6cff29be0cb6dc238c07d1dd3eb1054 Mon Sep 17 00:00:00 2001 From: Frederik Bark Date: Wed, 4 Oct 2023 18:35:26 +0200 Subject: [PATCH 1/4] =?UTF-8?q?changed=20omnidirectional=20to=20be=20180?= =?UTF-8?q?=C2=B0,=20reprojected=20backpoints=20to=20fix=20other=20issue?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/webots/nodes/utils/WbObjectDetection.cpp | 9 +++++---- src/webots/nodes/utils/WbObjectDetection.hpp | 2 +- 2 files changed, 6 insertions(+), 5 deletions(-) diff --git a/src/webots/nodes/utils/WbObjectDetection.cpp b/src/webots/nodes/utils/WbObjectDetection.cpp index 5a56dfebdd6..39206b85ad2 100644 --- a/src/webots/nodes/utils/WbObjectDetection.cpp +++ b/src/webots/nodes/utils/WbObjectDetection.cpp @@ -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(); @@ -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(); @@ -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]); diff --git a/src/webots/nodes/utils/WbObjectDetection.hpp b/src/webots/nodes/utils/WbObjectDetection.hpp index 9c5077b5595..3a1e7033631 100644 --- a/src/webots/nodes/utils/WbObjectDetection.hpp +++ b/src/webots/nodes/utils/WbObjectDetection.hpp @@ -98,7 +98,7 @@ class WbObjectDetection { QList mRaysCollisionDepth; // rays collision depth QList 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; }; From 0da04f9b99a50829a696ce361dd7ce4a3b819e62 Mon Sep 17 00:00:00 2001 From: Frederik Bark Date: Wed, 4 Oct 2023 18:38:32 +0200 Subject: [PATCH 2/4] added new object to recognition test to see if objects perpendicular to camera, which are only partially visible are recognized --- .../camera_recognition/camera_recognition.c | 14 +++++++----- tests/api/worlds/camera_recognition.wbt | 22 +++++++++++++++++++ 2 files changed, 30 insertions(+), 6 deletions(-) diff --git a/tests/api/controllers/camera_recognition/camera_recognition.c b/tests/api/controllers/camera_recognition/camera_recognition.c index 0dba96c8e4b..91b0e7fb9b4 100644 --- a/tests/api/controllers/camera_recognition/camera_recognition.c +++ b/tests/api/controllers/camera_recognition/camera_recognition.c @@ -9,7 +9,7 @@ #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 @@ -17,7 +17,7 @@ // 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"; @@ -45,12 +45,14 @@ 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"); diff --git a/tests/api/worlds/camera_recognition.wbt b/tests/api/worlds/camera_recognition.wbt index 6688ef97eff..d43bdbfb4d7 100644 --- a/tests/api/worlds/camera_recognition.wbt +++ b/tests/api/worlds/camera_recognition.wbt @@ -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 From cff2268c39618e2ce680fb5fb656ad79048ca0a7 Mon Sep 17 00:00:00 2001 From: Frederik Bark Date: Thu, 5 Oct 2023 13:35:39 +0200 Subject: [PATCH 3/4] updated clang and added new test case for perpendicular object --- src/webots/nodes/utils/WbObjectDetection.cpp | 2 +- .../camera_recognition/camera_recognition.c | 81 ++++++++++++++----- 2 files changed, 64 insertions(+), 19 deletions(-) diff --git a/src/webots/nodes/utils/WbObjectDetection.cpp b/src/webots/nodes/utils/WbObjectDetection.cpp index 39206b85ad2..7e2b2ec4a81 100644 --- a/src/webots/nodes/utils/WbObjectDetection.cpp +++ b/src/webots/nodes/utils/WbObjectDetection.cpp @@ -327,7 +327,7 @@ bool WbObjectDetection::isWithinBounds(const WbAffinePlane *frustumPlanes, const // move the points in the device referential for (int i = 0; i < pointsInFrustum.size(); ++i) pointsInFrustum[i] = deviceInverseRotation * (pointsInFrustum[i] - devicePosition); - + double minX = pointsInFrustum[0].x(); double maxX = minX; double minY = pointsInFrustum[0].y(); diff --git a/tests/api/controllers/camera_recognition/camera_recognition.c b/tests/api/controllers/camera_recognition/camera_recognition.c index 91b0e7fb9b4..d8af1f45d6e 100644 --- a/tests/api/controllers/camera_recognition/camera_recognition.c +++ b/tests/api/controllers/camera_recognition/camera_recognition.c @@ -17,7 +17,8 @@ // 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", "perpendicular box"}; + "visible sphere", "visible box", "sub solid", "visible capsule", "composed solid", "visible sphere without BO", + "perpendicular box"}; static const char *occcluded_solid_model = "occluded box"; @@ -45,14 +46,13 @@ 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, VISIBLE_SOLID_NUMBER + 4, - "The spherical camera should initially see %d objects and not %d (with occlusion).", VISIBLE_SOLID_NUMBER + 4, - 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, VISIBLE_SOLID_NUMBER + 2, - "The cylindrical camera should initially see %d objects and not %d (with occlusion).", - VISIBLE_SOLID_NUMBER + 2, 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"); @@ -62,7 +62,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); @@ -139,7 +139,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) @@ -160,6 +160,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); @@ -214,6 +258,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.", @@ -268,12 +313,12 @@ 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], @@ -281,19 +326,19 @@ int main(int argc, char **argv) { // 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]); } @@ -317,7 +362,7 @@ 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], @@ -325,19 +370,19 @@ int main(int argc, char **argv) { // 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]); From 183f758c4da7e3a6e6bb70567a39f084e1983e3a Mon Sep 17 00:00:00 2001 From: Frederik Bark Date: Fri, 6 Oct 2023 10:43:59 +0200 Subject: [PATCH 4/4] clang changes --- .../camera_recognition/camera_recognition.c | 38 ++++++++++--------- 1 file changed, 20 insertions(+), 18 deletions(-) diff --git a/tests/api/controllers/camera_recognition/camera_recognition.c b/tests/api/controllers/camera_recognition/camera_recognition.c index d8af1f45d6e..ac871171b59 100644 --- a/tests/api/controllers/camera_recognition/camera_recognition.c +++ b/tests/api/controllers/camera_recognition/camera_recognition.c @@ -17,7 +17,7 @@ // 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"; @@ -46,14 +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, VISIBLE_SOLID_NUMBER + 4, "The spherical camera should initially see %d objects" - " and not %d (with occlusion).", VISIBLE_SOLID_NUMBER + 4, 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, VISIBLE_SOLID_NUMBER + 2, "The cylindrical camera should initially see %d objects" + 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"); @@ -166,20 +168,20 @@ int main(int argc, char **argv) { // 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]); + "%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], + "%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]); } } @@ -191,15 +193,15 @@ int main(int argc, char **argv) { // 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]); + "%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], + "%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]); } }