From 9c0f77c4cb6074b98fe64498ac4a4b9e5298374d Mon Sep 17 00:00:00 2001 From: AmrElsersy Date: Wed, 16 Jun 2021 21:41:59 +0200 Subject: [PATCH 01/31] Add BoundingBox Sensor Signed-off-by: AmrElsersy --- .../sensors/BoundingBoxCameraSensor.hh | 119 +++++ src/BoundingBoxCameraSensor.cc | 442 ++++++++++++++++++ src/CMakeLists.txt | 9 + test/integration/CMakeLists.txt | 2 + 4 files changed, 572 insertions(+) create mode 100644 include/ignition/sensors/BoundingBoxCameraSensor.hh create mode 100644 src/BoundingBoxCameraSensor.cc diff --git a/include/ignition/sensors/BoundingBoxCameraSensor.hh b/include/ignition/sensors/BoundingBoxCameraSensor.hh new file mode 100644 index 00000000..91a8c28e --- /dev/null +++ b/include/ignition/sensors/BoundingBoxCameraSensor.hh @@ -0,0 +1,119 @@ +/* + * Copyright (C) 2021 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * +*/ + +#ifndef IGNITION_SENSORS_BOUNDINGBOXCAMERASENSOR2_HH_ +#define IGNITION_SENSORS_BOUNDINGBOXCAMERASENSOR2_HH_ + +#include +#include +#include +#include +#include +#include +#include +#include + +#include "ignition/msgs.hh" +#include "ignition/transport/Node.hh" +#include "ignition/transport/Publisher.hh" + +#include "ignition/rendering/BoundingBoxCamera.hh" +#include "ignition/sensors/CameraSensor.hh" +#include "ignition/sensors/Export.hh" +#include "ignition/sensors/Sensor.hh" + +namespace ignition +{ + namespace sensors + { + // Inline bracket to help doxygen filtering. + inline namespace IGNITION_SENSORS_VERSION_NAMESPACE { + // forward declarations + class BoundingBoxCameraSensorPrivate; + + /// \brief BoundingBox camera sensor class. + /// + /// This class creates BoundingBox image from an ignition rendering scene. + /// The scene must be created in advance and given to Manager::Init(). + /// It offers both an ignition-transport interface and a direct C++ API + /// to access the image data. The API works by setting a callback to be + /// called with image data. + class BoundingBoxCameraSensor : public CameraSensor + { + /// \brief constructor + public: BoundingBoxCameraSensor(); + + /// \brief destructor + public: virtual ~BoundingBoxCameraSensor(); + + /// \brief Load the sensor based on data from an sdf::Sensor object. + /// \param[in] _sdf SDF Sensor parameters. + /// \return true if loading was successful + public: virtual bool Load(const sdf::Sensor &_sdf) override; + + /// \brief Load the sensor with SDF parameters. + /// \param[in] _sdf SDF Sensor parameters. + /// \return true if loading was successful + public: virtual bool Load(sdf::ElementPtr _sdf) override; + + /// \brief Initialize values in the sensor + /// \return True on success + public: virtual bool Init() override; + + /// \brief Force the sensor to generate data + /// \param[in] _now The current time + /// \return true if the update was successfull + public: virtual bool Update( + const std::chrono::steady_clock::duration &_now) override; + + /// \brief Get the rendering BoundingBox camera + /// \return BoundingBox camera pointer + public: virtual rendering::BoundingBoxCameraPtr BoundingBoxCamera(); + + /// \brief Callback on new bounding boxes from bounding boxes camera + /// \param[in] _boxes Detected bounding boxes from the camera + public: void OnNewBoundingBoxes( + const std::vector &boxes); + + /// \brief Set the rendering scene. + /// \param[in] _scene Pointer to the scene + public: virtual void SetScene( + ignition::rendering::ScenePtr _scene) override; + + /// \brief Get image width. + /// \return width of the image + public: virtual unsigned int ImageWidth() const override; + + /// \brief Get image height. + /// \return height of the image + public: virtual unsigned int ImageHeight() const override; + + /// \brief Create a camera in a scene + /// \return True on success. + private: bool CreateCamera(); + + IGN_COMMON_WARN_IGNORE__DLL_INTERFACE_MISSING + /// \brief Data pointer for private data + /// \internal + private: std::unique_ptr dataPtr; + IGN_COMMON_WARN_RESUME__DLL_INTERFACE_MISSING + }; + } + } +} + +#endif diff --git a/src/BoundingBoxCameraSensor.cc b/src/BoundingBoxCameraSensor.cc new file mode 100644 index 00000000..8953e03f --- /dev/null +++ b/src/BoundingBoxCameraSensor.cc @@ -0,0 +1,442 @@ +/* + * Copyright (C) 2021 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * +*/ + +#include +#include + +#include "ignition/common/Console.hh" +#include "ignition/common/Profiler.hh" +#include "ignition/common/Image.hh" +#include "ignition/sensors/RenderingEvents.hh" +#include "ignition/sensors/SensorFactory.hh" + +#include "ignition/sensors/BoundingBoxCameraSensor.hh" +#include "ignition/rendering/BoundingBoxCamera.hh" + +#include "ignition/transport/Node.hh" +#include "ignition/transport/Publisher.hh" +#include "ignition/msgs.hh" + + +using namespace ignition; +using namespace sensors; +using namespace rendering; + +class ignition::sensors::BoundingBoxCameraSensorPrivate +{ + /// \brief SDF Sensor DOM Object + public: sdf::Sensor sdfSensor; + + /// \brief True if Load() has been called and was successful + public: bool initialized = false; + + /// \brief Rendering BoundingBox Camera + public: rendering::BoundingBoxCameraPtr boundingboxCamera {nullptr}; + + /// \brief Rendering RGB Camera to draw boxes on it and publish + /// its image (just for visualization) + public: rendering::CameraPtr camera {nullptr}; + + /// \brief Node to create publisher + public: transport::Node node; + + /// \brief Publisher to publish Image msg with drawn boxes + public: transport::Node::Publisher imagePublisher; + + /// \brief Publisher to publish BoundingBoxes msg + public: transport::Node::Publisher boxesPublisher; + + /// \brief Image msg with drawn boxes on it + public: msgs::Image imageMsg; + + /// \brief Bounding boxes msg + public: msgs::BoundingBoxes boxesMsg; + + /// \brief Topic to publish the BoundingBox image + public: std::string topicBoundingBoxes = ""; + + /// \brief Topic to publish the BoundingBox image + public: std::string topicImage = ""; + + /// \brief True when the sensor published image with drawn boxes + /// False when the sensor publish only the boxes + public: bool IsPublishedImage {false}; + + /// \brief Buffer contains the BoundingBox map data + public: std::vector boundingBoxes; + + /// \brief RGB Image to draw boxes on it + public: rendering::Image image; + + /// \brief Buffer contains the BoundingBox map data + public: unsigned char *imageBuffer {nullptr}; + + /// \brief Connection to the new BoundingBox frames data + public: common::ConnectionPtr newBoundingBoxConnection; + + /// \brief Connection to the Manager's scene change event. + public: common::ConnectionPtr sceneChangeConnection; + + /// \brief Just a mutex for thread safety + public: std::mutex mutex; + + /// \brief BoundingBoxes type + public: BoundingBoxType type {BoundingBoxType::VisibleBox}; +}; + +////////////////////////////////////////////////// +BoundingBoxCameraSensor::BoundingBoxCameraSensor() + : CameraSensor(), dataPtr(new BoundingBoxCameraSensorPrivate) +{ +} + +///////////////////////////////////////////////// +BoundingBoxCameraSensor::~BoundingBoxCameraSensor() +{ + if (this->dataPtr->imageBuffer) + delete this->dataPtr->imageBuffer; +} + +///////////////////////////////////////////////// +bool BoundingBoxCameraSensor::Init() +{ + return CameraSensor::Init(); +} + +///////////////////////////////////////////////// +bool BoundingBoxCameraSensor::Load(sdf::ElementPtr _sdf) +{ + sdf::Sensor sdfSensor; + sdfSensor.Load(_sdf); + return this->Load(sdfSensor); +} + +///////////////////////////////////////////////// +bool BoundingBoxCameraSensor::Load(const sdf::Sensor &_sdf) +{ + std::lock_guard lock(this->dataPtr->mutex); + + // BoundingBox Type + sdf::ElementPtr sdfElement = _sdf.Element(); + if (sdfElement->HasElement("box_type")) + { + std::string type = sdfElement->Get("box_type"); + + // convert type to lowercase + std::for_each(type.begin(), type.end(), [](char & c){ + c = std::tolower(c); + }); + + if (type == "full" || type == "full_box") + this->dataPtr->type = BoundingBoxType::FullBox; + else if (type == "visible" || type == "visible_box") + this->dataPtr->type = BoundingBoxType::VisibleBox; + } + + if (!Sensor::Load(_sdf)) + { + return false; + } + + // Check if this is the right type + if (_sdf.Type() != sdf::SensorType::BOUNDINGBOX_CAMERA) + { + ignerr << "Attempting to a load a BoundingBox Camera sensor, but received " + << "a " << _sdf.TypeStr() << std::endl; + return false; + } + + if (_sdf.CameraSensor() == nullptr) + { + ignerr << "Attempting to a load a BoundingBox Camera sensor, but received " + << "a null sensor." << std::endl; + return false; + } + + this->dataPtr->sdfSensor = _sdf; + + this->dataPtr->topicBoundingBoxes = this->Topic(); + this->dataPtr->topicImage = this->Topic() + "_image"; + + this->dataPtr->imagePublisher = + this->dataPtr->node.Advertise( + this->dataPtr->topicImage); + + this->dataPtr->boxesPublisher = + this->dataPtr->node.Advertise( + this->dataPtr->topicBoundingBoxes); + + if (!this->dataPtr->imagePublisher) + { + ignerr << "Unable to create publisher on topic[" + << this->dataPtr->topicImage << "].\n"; + return false; + } + + if (!this->dataPtr->boxesPublisher) + { + ignerr << "Unable to create publisher on topic[" + << this->dataPtr->topicBoundingBoxes << "].\n"; + return false; + } + + if (!this->AdvertiseInfo()) + return false; + + if (this->Scene()) + { + this->CreateCamera(); + } + + this->dataPtr->sceneChangeConnection = + RenderingEvents::ConnectSceneChangeCallback( + std::bind(&BoundingBoxCameraSensor::SetScene, this, + std::placeholders::_1)); + + this->dataPtr->initialized = true; + + return true; +} + +///////////////////////////////////////////////// +void BoundingBoxCameraSensor::SetScene( + ignition::rendering::ScenePtr _scene) +{ + std::lock_guard lock(this->dataPtr->mutex); + + // APIs make it possible for the scene pointer to change + if (this->Scene() != _scene) + { + this->dataPtr->boundingboxCamera = nullptr; + this->dataPtr->camera = nullptr; + RenderingSensor::SetScene(_scene); + + if (this->dataPtr->initialized) + this->CreateCamera(); + } +} + +///////////////////////////////////////////////// +bool BoundingBoxCameraSensor::CreateCamera() +{ + auto sdfCamera = this->dataPtr->sdfSensor.CameraSensor(); + if (!sdfCamera) + { + ignerr << "Unable to access camera SDF element\n"; + return false; + } + + // Camera Info Msg + this->PopulateInfo(sdfCamera); + + if (!this->dataPtr->camera) + { + // Create rendering camera + this->dataPtr->boundingboxCamera = + this->Scene()->CreateBoundingBoxCamera(this->Name()); + + this->dataPtr->camera = this->Scene()->CreateCamera( + this->Name() + "_rgbCamera"); + } + + auto width = sdfCamera->ImageWidth(); + auto height = sdfCamera->ImageHeight(); + + // Set Camera Properties + this->dataPtr->camera->SetImageFormat(rendering::PF_R8G8B8); + this->dataPtr->camera->SetImageWidth(width); + this->dataPtr->camera->SetImageHeight(height); + this->dataPtr->camera->SetVisibilityMask(sdfCamera->VisibilityMask()); + this->dataPtr->camera->SetNearClipPlane(sdfCamera->NearClip()); + this->dataPtr->camera->SetFarClipPlane(sdfCamera->FarClip()); + this->dataPtr->camera->SetNearClipPlane(0.01); + this->dataPtr->camera->SetFarClipPlane(1000); + math::Angle angle = sdfCamera->HorizontalFov(); + if (angle < 0.01 || angle > IGN_PI*2) + { + ignerr << "Invalid horizontal field of view [" << angle << "]\n"; + return false; + } + double aspectRatio = static_cast(width)/height; + this->dataPtr->camera->SetAspectRatio(aspectRatio); + this->dataPtr->camera->SetHFOV(angle); + + this->dataPtr->boundingboxCamera->SetImageWidth(width); + this->dataPtr->boundingboxCamera->SetImageHeight(height); + this->dataPtr->boundingboxCamera->SetNearClipPlane(sdfCamera->NearClip()); + this->dataPtr->boundingboxCamera->SetFarClipPlane(sdfCamera->FarClip()); + this->dataPtr->boundingboxCamera->SetNearClipPlane(0.01); + this->dataPtr->boundingboxCamera->SetFarClipPlane(1000); + this->dataPtr->boundingboxCamera->SetImageFormat(PixelFormat::PF_R8G8B8); + this->dataPtr->boundingboxCamera->SetAspectRatio(aspectRatio); + this->dataPtr->boundingboxCamera->SetHFOV(angle); + this->dataPtr->boundingboxCamera->SetVisibilityMask( + sdfCamera->VisibilityMask()); + this->dataPtr->boundingboxCamera->SetBoundingBoxType(this->dataPtr->type); + + // Add the camera to the scene + this->Scene()->RootVisual()->AddChild(this->dataPtr->camera); + this->Scene()->RootVisual()->AddChild(this->dataPtr->boundingboxCamera); + + // Add the rendering sensor to handle its render, no need to add the + // rgb camera as we handle its render via Capture() function + this->AddSensor(this->dataPtr->boundingboxCamera); + + // Connection to receive the BoundingBox buffer + this->dataPtr->newBoundingBoxConnection = + this->dataPtr->boundingboxCamera->ConnectNewBoundingBoxes( + std::bind(&BoundingBoxCameraSensor::OnNewBoundingBoxes, this, + std::placeholders::_1)); + + this->dataPtr->image = this->dataPtr->camera->CreateImage(); + + return true; +} + +///////////////////////////////////////////////// +rendering::BoundingBoxCameraPtr BoundingBoxCameraSensor::BoundingBoxCamera() +{ + return this->dataPtr->boundingboxCamera; +} + +///////////////////////////////////////////////// +void BoundingBoxCameraSensor::OnNewBoundingBoxes( + const std::vector &boxes) +{ + std::lock_guard lock(this->dataPtr->mutex); + this->dataPtr->boundingBoxes.clear(); + for (auto box : boxes) + this->dataPtr->boundingBoxes.push_back(box); +} + +////////////////////////////////////////////////// +bool BoundingBoxCameraSensor::Update( + const std::chrono::steady_clock::duration &_now) +{ + IGN_PROFILE("BoundingBoxCameraSensor::Update"); + if (!this->dataPtr->initialized) + { + ignerr << "Not initialized, update ignored.\n"; + return false; + } + + if (!this->dataPtr->boundingboxCamera || !this->dataPtr->camera) + { + ignerr << "Camera doesn't exist.\n"; + return false; + } + + // don't render if there is no subscribers + if (!this->dataPtr->imagePublisher.HasConnections() && + !this->dataPtr->boxesPublisher.HasConnections()) + { + return false; + } + + // Render the bounding box camera + this->Render(); + + // Publish image only if there is subscribers for it + this->dataPtr->IsPublishedImage = + this->dataPtr->imagePublisher.HasConnections(); + + if (this->dataPtr->IsPublishedImage) + { + // The sensor updates only the bounding box camera with its pose + // as it has the same name, so make rgb camera with the same pose + this->dataPtr->camera->SetWorldPose( + this->dataPtr->boundingboxCamera->WorldPose()); + + // Render the rgb camera + this->dataPtr->camera->Capture(this->dataPtr->image); + + // Draw bounding boxes + for (auto box : this->dataPtr->boundingBoxes) + { + this->dataPtr->imageBuffer = this->dataPtr->image.Data(); + + this->dataPtr->boundingboxCamera->DrawBoundingBox( + this->dataPtr->imageBuffer, box); + } + + auto width = this->dataPtr->camera->ImageWidth(); + auto height = this->dataPtr->camera->ImageHeight(); + + // Create Image message + this->dataPtr->imageMsg.set_width(width); + this->dataPtr->imageMsg.set_height(height); + // format + this->dataPtr->imageMsg.set_step( + width * rendering::PixelUtil::BytesPerPixel(rendering::PF_R8G8B8)); + this->dataPtr->imageMsg.set_pixel_format_type( + msgs::PixelFormatType::RGB_INT8); + // time stamp + auto stamp = this->dataPtr->imageMsg.mutable_header()->mutable_stamp(); + *stamp = msgs::Convert(_now); + auto frame = this->dataPtr->imageMsg.mutable_header()->add_data(); + frame->set_key("frame_id"); + frame->add_value(this->Name()); + // image data + this->dataPtr->imageMsg.set_data(this->dataPtr->imageBuffer, + rendering::PixelUtil::MemorySize(rendering::PF_R8G8B8, + width, height)); + + this->AddSequence(this->dataPtr->imageMsg.mutable_header(), "rgbImage"); + this->dataPtr->imagePublisher.Publish(this->dataPtr->imageMsg); + } + + // Create BoundingBoxes message + for (auto box : this->dataPtr->boundingBoxes) + { + // box data + auto boxMsg = this->dataPtr->boxesMsg.add_box(); + boxMsg->set_minx(box.minX); + boxMsg->set_miny(box.minY); + boxMsg->set_maxx(box.maxX); + boxMsg->set_maxy(box.maxY); + boxMsg->set_label(box.label); + } + // time stamp + auto stampBoxes = this->dataPtr->boxesMsg.mutable_header()->mutable_stamp(); + *stampBoxes = msgs::Convert(_now); + auto frameBoxes = this->dataPtr->boxesMsg.mutable_header()->add_data(); + frameBoxes->set_key("frame_id"); + frameBoxes->add_value(this->Name()); + + std::lock_guard lock(this->dataPtr->mutex); + + // Publish + this->PublishInfo(_now); + this->AddSequence(this->dataPtr->boxesMsg.mutable_header(), "boundingboxes"); + this->dataPtr->boxesPublisher.Publish(this->dataPtr->boxesMsg); + + return true; +} + +///////////////////////////////////////////////// +unsigned int BoundingBoxCameraSensor::ImageHeight() const +{ + return this->dataPtr->camera->ImageHeight(); +} + +///////////////////////////////////////////////// +unsigned int BoundingBoxCameraSensor::ImageWidth() const +{ + return this->dataPtr->camera->ImageWidth(); +} + +IGN_SENSORS_REGISTER_SENSOR(BoundingBoxCameraSensor) diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index 1afdb7c0..18f67e05 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -137,6 +137,15 @@ target_link_libraries(${thermal_camera_target} ignition-transport${IGN_TRANSPORT_VER}::ignition-transport${IGN_TRANSPORT_VER} ) +set(boundingbox_camera_sources BoundingBoxCameraSensor.cc) +ign_add_component(boundingbox_camera SOURCES ${boundingbox_camera_sources} GET_TARGET_NAME boundingbox_camera_target) +target_link_libraries(${boundingbox_camera_target} + PRIVATE + ${camera_target} + ignition-msgs${IGN_MSGS_VER}::ignition-msgs${IGN_MSGS_VER} + ignition-transport${IGN_TRANSPORT_VER}::ignition-transport${IGN_TRANSPORT_VER} + ) + # Build the unit tests. ign_build_tests(TYPE UNIT SOURCES ${gtest_sources} LIB_DEPS ${rendering_target}) diff --git a/test/integration/CMakeLists.txt b/test/integration/CMakeLists.txt index 2c5de9fe..754a008f 100644 --- a/test/integration/CMakeLists.txt +++ b/test/integration/CMakeLists.txt @@ -1,6 +1,7 @@ set(TEST_TYPE "INTEGRATION") set(dri_tests + boundingbox_camera_plugin.cc camera_plugin.cc depth_camera_plugin.cc gpu_lidar_sensor_plugin.cc @@ -33,6 +34,7 @@ if (DRI_TESTS) ${dri_tests} LIB_DEPS ${IGNITION-TRANSPORT_LIBRARIES} + # ${PROJECT_LIBRARY_TARGET_NAME}-boundingbox_camera ${PROJECT_LIBRARY_TARGET_NAME}-depth_camera ${PROJECT_LIBRARY_TARGET_NAME}-camera ${PROJECT_LIBRARY_TARGET_NAME}-lidar From 207f3bb8cd1dfa8ae1c40f8ab4472b0cb287e27a Mon Sep 17 00:00:00 2001 From: AmrElsersy Date: Thu, 17 Jun 2021 00:48:51 +0200 Subject: [PATCH 02/31] Add Integration Test Signed-off-by: AmrElsersy --- src/BoundingBoxCameraSensor.cc | 9 +- test/integration/CMakeLists.txt | 2 +- test/integration/boundingbox_camera_plugin.cc | 297 ++++++++++++++++++ .../boundingbox_camera_sensor_builtin.sdf | 21 ++ 4 files changed, 325 insertions(+), 4 deletions(-) create mode 100644 test/integration/boundingbox_camera_plugin.cc create mode 100644 test/integration/boundingbox_camera_sensor_builtin.sdf diff --git a/src/BoundingBoxCameraSensor.cc b/src/BoundingBoxCameraSensor.cc index 8953e03f..3e252e0e 100644 --- a/src/BoundingBoxCameraSensor.cc +++ b/src/BoundingBoxCameraSensor.cc @@ -379,26 +379,29 @@ bool BoundingBoxCameraSensor::Update( // Create Image message this->dataPtr->imageMsg.set_width(width); this->dataPtr->imageMsg.set_height(height); - // format + // Format this->dataPtr->imageMsg.set_step( width * rendering::PixelUtil::BytesPerPixel(rendering::PF_R8G8B8)); this->dataPtr->imageMsg.set_pixel_format_type( msgs::PixelFormatType::RGB_INT8); - // time stamp + // Time stamp auto stamp = this->dataPtr->imageMsg.mutable_header()->mutable_stamp(); *stamp = msgs::Convert(_now); auto frame = this->dataPtr->imageMsg.mutable_header()->add_data(); frame->set_key("frame_id"); frame->add_value(this->Name()); - // image data + // Image data this->dataPtr->imageMsg.set_data(this->dataPtr->imageBuffer, rendering::PixelUtil::MemorySize(rendering::PF_R8G8B8, width, height)); + // Publish this->AddSequence(this->dataPtr->imageMsg.mutable_header(), "rgbImage"); this->dataPtr->imagePublisher.Publish(this->dataPtr->imageMsg); } + this->dataPtr->boxesMsg.Clear(); + // Create BoundingBoxes message for (auto box : this->dataPtr->boundingBoxes) { diff --git a/test/integration/CMakeLists.txt b/test/integration/CMakeLists.txt index 754a008f..3686336d 100644 --- a/test/integration/CMakeLists.txt +++ b/test/integration/CMakeLists.txt @@ -34,7 +34,7 @@ if (DRI_TESTS) ${dri_tests} LIB_DEPS ${IGNITION-TRANSPORT_LIBRARIES} - # ${PROJECT_LIBRARY_TARGET_NAME}-boundingbox_camera + ${PROJECT_LIBRARY_TARGET_NAME}-boundingbox_camera ${PROJECT_LIBRARY_TARGET_NAME}-depth_camera ${PROJECT_LIBRARY_TARGET_NAME}-camera ${PROJECT_LIBRARY_TARGET_NAME}-lidar diff --git a/test/integration/boundingbox_camera_plugin.cc b/test/integration/boundingbox_camera_plugin.cc new file mode 100644 index 00000000..7ec78bdd --- /dev/null +++ b/test/integration/boundingbox_camera_plugin.cc @@ -0,0 +1,297 @@ +/* + * Copyright (C) 2021 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * +*/ + +#include + +#include +#include +#include + +#include +#include +#include +#include +#include + +#include "test_config.h" // NOLINT(build/include) +#include "TransportTestTools.hh" + +using namespace ignition; +using namespace rendering; + +class BoundingBoxCameraSensorTest: public testing::Test, + public testing::WithParamInterface +{ + // Create a BoundingBox Camera sensor from a SDF and gets a image message + public: void BoxesWithBuiltinSDF(const std::string &_renderEngine); +}; + +/// \brief mutex for thread safety +std::mutex g_mutex; + +/// \brief bounding boxes from the camera +std::vector g_boxes; + +/// \brief counter of received boundingboxes msg +int g_counter = 0; + +/// \brief callback to receive boxes from the camera +void OnNewBoundingBoxes(const msgs::BoundingBoxes &boxes) +{ + g_mutex.lock(); + g_boxes.clear(); + + int size = boxes.box_size(); + for (int i = 0; i < size; i++) + { + auto boxMsg = boxes.box(i); + BoundingBox box; + box.minX = boxMsg.minx(); + box.minY = boxMsg.miny(); + box.maxX = boxMsg.maxx(); + box.maxY = boxMsg.maxy(); + box.label = boxMsg.label(); + g_boxes.push_back(box); + } + g_counter++; + g_mutex.unlock(); +} + +/// \brief wait till you read the published frame +void WaitForNewFrame() +{ + auto waitTime = std::chrono::duration_cast< std::chrono::milliseconds >( + std::chrono::duration< double >(0.001)); + int counter = 0; + // wait for the counter to increase + for (int sleep = 0; sleep < 300 && counter == 0; ++sleep) + { + g_mutex.lock(); + counter = g_counter; + g_mutex.unlock(); + std::this_thread::sleep_for(waitTime); + } + EXPECT_GT(counter, 0); +} + +/// \brief Build the scene with 3 boxes 2 overlapping boxes which 1 +/// is behind the other, and the 3rd box is invisible behind them +void BuildScene(rendering::ScenePtr scene) +{ + math::Vector3d occludedPosition(4, 1, 0); + math::Vector3d frontPosition(2, 0, 0); + math::Vector3d invisiblePosition(5, 0, 0); + + rendering::VisualPtr root = scene->RootVisual(); + + // create front box visual (the smaller box) + rendering::VisualPtr occludedBox = scene->CreateVisual(); + occludedBox->AddGeometry(scene->CreateBox()); + occludedBox->SetOrigin(0.0, 0.0, 0.0); + occludedBox->SetLocalPosition(occludedPosition); + occludedBox->SetLocalRotation(0, 0, 0); + occludedBox->SetUserData("label", 1); + root->AddChild(occludedBox); + + // create occluded box visual (the bigger box) + rendering::VisualPtr frontBox = scene->CreateVisual(); + frontBox->AddGeometry(scene->CreateBox()); + frontBox->SetOrigin(0.0, 0.0, 0.0); + frontBox->SetLocalPosition(frontPosition); + frontBox->SetLocalRotation(0, 0, 0); + frontBox->SetUserData("label", 2); + root->AddChild(frontBox); + + rendering::VisualPtr invisibleBox = scene->CreateVisual(); + invisibleBox->AddGeometry(scene->CreateBox()); + invisibleBox->SetOrigin(0.0, 0.0, 0.0); + invisibleBox->SetLocalPosition(invisiblePosition); + invisibleBox->SetLocalRotation(0, 0, 0); + invisibleBox->SetUserData("label", 2); + root->AddChild(invisibleBox); +} + +///////////////////////////////////////////////// +void BoundingBoxCameraSensorTest::BoxesWithBuiltinSDF( + const std::string &_renderEngine) +{ + std::string path = ignition::common::joinPaths(PROJECT_SOURCE_PATH, "test", + "integration", "boundingbox_camera_sensor_builtin.sdf"); + sdf::SDFPtr doc(new sdf::SDF()); + sdf::init(doc); + ASSERT_TRUE(sdf::readFile(path, doc)); + ASSERT_NE(nullptr, doc->Root()); + ASSERT_TRUE(doc->Root()->HasElement("model")); + auto modelPtr = doc->Root()->GetElement("model"); + ASSERT_TRUE(modelPtr->HasElement("link")); + auto linkPtr = modelPtr->GetElement("link"); + ASSERT_TRUE(linkPtr->HasElement("sensor")); + auto sensorPtr = linkPtr->GetElement("sensor"); + ASSERT_TRUE(sensorPtr->HasElement("camera")); + auto cameraPtr = sensorPtr->GetElement("camera"); + ASSERT_TRUE(cameraPtr->HasElement("image")); + auto imagePtr = cameraPtr->GetElement("image"); + + unsigned int width = imagePtr->Get("width"); + unsigned int height = imagePtr->Get("height"); + + EXPECT_EQ(width, 320); + EXPECT_EQ(height, 240); + + // Setup ign-rendering with an empty scene + auto *engine = ignition::rendering::engine(_renderEngine); + if (!engine) + { + igndbg << "Engine '" << _renderEngine + << "' is not supported" << std::endl; + return; + } + + ignition::rendering::ScenePtr scene = engine->CreateScene("scene"); + BuildScene(scene); + + ignition::sensors::Manager mgr; + + sdf::Sensor sdfSensor; + sdfSensor.Load(sensorPtr); + + std::string type = sdfSensor.TypeStr(); + EXPECT_EQ(type, "boundingbox_camera"); + + ignition::sensors::BoundingBoxCameraSensor *sensor = + mgr.CreateSensor(sdfSensor); + + EXPECT_NE(sensor, nullptr); + sensor->SetScene(scene); + + EXPECT_EQ(width, sensor->ImageWidth()); + EXPECT_EQ(height, sensor->ImageHeight()); + + auto camera = sensor->BoundingBoxCamera(); + EXPECT_NE(camera, nullptr); + + camera->SetLocalPosition(0.0, 0.0, 0.0); + camera->SetLocalRotation(0.0, 0.0, 0.0); + camera->SetAspectRatio(1.333); + camera->SetHFOV(IGN_PI / 2); + camera->SetBoundingBoxType(BoundingBoxType::VisibleBox); + + EXPECT_EQ(camera->Type(), BoundingBoxType::VisibleBox); + EXPECT_EQ(camera->ImageWidth(), width); + EXPECT_EQ(camera->ImageHeight(), height); + + // Get the Msg + std::string topic = + "/test/integration/BoundingBoxCameraPlugin_boxesWithBuiltinSDF"; + WaitForMessageTestHelper helper(topic); + + // Update once to create image + mgr.RunOnce(std::chrono::steady_clock::duration::zero()); + + EXPECT_TRUE(helper.WaitForMessage()) << helper; + + // subscribe to the BoundingBox camera topic + ignition::transport::Node node; + node.Subscribe(topic, &OnNewBoundingBoxes); + + // wait for a few BoundingBox camera boxes + mgr.RunOnce(std::chrono::steady_clock::duration::zero(), true); + // wait for a new boxes + WaitForNewFrame(); + + // accepted error with +/- in pixels in comparing the box coordinates + int margin_error = 1; + + // Visible box test + g_mutex.lock(); + g_counter = 0; + + // check if the invisible 3rd box is not exist + EXPECT_EQ(g_boxes.size(), size_t(2)); + + BoundingBox occludedBox = g_boxes[0]; + BoundingBox frontBox = g_boxes[1]; + + unsigned int occludedLabel = 1; + unsigned int frontLabel = 2; + + // hard-coded comparasion with acceptable error + EXPECT_NEAR(occludedBox.minX, 91, margin_error); + EXPECT_NEAR(occludedBox.minY, 97, margin_error); + EXPECT_NEAR(occludedBox.maxX, 106, margin_error); + EXPECT_NEAR(occludedBox.maxY, 142, margin_error); + EXPECT_EQ(occludedBox.label, occludedLabel); + + EXPECT_NEAR(frontBox.minX, 107, margin_error); + EXPECT_NEAR(frontBox.minY, 67, margin_error); + EXPECT_NEAR(frontBox.maxX, 212, margin_error); + EXPECT_NEAR(frontBox.maxY, 172, margin_error); + EXPECT_EQ(frontBox.label, frontLabel); + + g_mutex.unlock(); + + // Full Boxes Type Test + camera->SetBoundingBoxType(BoundingBoxType::FullBox); + + // wait for bounding boxes + mgr.RunOnce(std::chrono::steady_clock::duration::zero(), true); + WaitForNewFrame(); + + // test + g_mutex.lock(); + g_counter = 0; + + // check the hidden box + EXPECT_EQ(g_boxes.size(), size_t(2)); + BoundingBox occludedFullBox = g_boxes[0]; + BoundingBox frontFullBox = g_boxes[1]; + + // coordinates of partially occluded object is bigger + EXPECT_NEAR(occludedFullBox.minX, 91, margin_error); + EXPECT_NEAR(occludedFullBox.minY, 97, margin_error); + EXPECT_NEAR(occludedFullBox.maxX, 142, margin_error); + EXPECT_NEAR(occludedFullBox.maxY, 142, margin_error); + EXPECT_EQ(occludedFullBox.label, occludedLabel); + + EXPECT_NEAR(frontFullBox.minX, 105, margin_error); + EXPECT_NEAR(frontFullBox.minY, 65, margin_error); + EXPECT_NEAR(frontFullBox.maxX, 214, margin_error); + EXPECT_NEAR(frontFullBox.maxY, 174, margin_error); + EXPECT_EQ(frontFullBox.label, frontLabel); + g_mutex.unlock(); + + // Clean up + engine->DestroyScene(scene); + ignition::rendering::unloadEngine(engine->Name()); +} + +////////////////////////////////////////////////// +TEST_P(BoundingBoxCameraSensorTest, BoxesWithBuiltinSDF) +{ + BoxesWithBuiltinSDF(GetParam()); +} + +INSTANTIATE_TEST_CASE_P(BoundingBoxCameraSensor, BoundingBoxCameraSensorTest, + RENDER_ENGINE_VALUES, ignition::rendering::PrintToStringParam()); + +////////////////////////////////////////////////// +int main(int argc, char **argv) +{ + ignition::common::Console::SetVerbosity(4); + ::testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/test/integration/boundingbox_camera_sensor_builtin.sdf b/test/integration/boundingbox_camera_sensor_builtin.sdf new file mode 100644 index 00000000..b133767d --- /dev/null +++ b/test/integration/boundingbox_camera_sensor_builtin.sdf @@ -0,0 +1,21 @@ + + + + + + 10 + /test/integration/BoundingBoxCameraPlugin_boxesWithBuiltinSDF + + + 320 + 240 + + + 0.1 + 1000.0 + + + + + + \ No newline at end of file From 6429fb9bb85ade0c3b0ccf2a868eeb14311a471c Mon Sep 17 00:00:00 2001 From: AmrElsersy Date: Thu, 15 Jul 2021 08:17:04 +0200 Subject: [PATCH 03/31] Edit format 2D boxes Signed-off-by: AmrElsersy --- src/BoundingBoxCameraSensor.cc | 73 ++- test/integration/boundingbox_camera_plugin.cc | 594 +++++++++--------- 2 files changed, 351 insertions(+), 316 deletions(-) diff --git a/src/BoundingBoxCameraSensor.cc b/src/BoundingBoxCameraSensor.cc index 3e252e0e..3b4f8c17 100644 --- a/src/BoundingBoxCameraSensor.cc +++ b/src/BoundingBoxCameraSensor.cc @@ -63,8 +63,11 @@ class ignition::sensors::BoundingBoxCameraSensorPrivate /// \brief Image msg with drawn boxes on it public: msgs::Image imageMsg; - /// \brief Bounding boxes msg - public: msgs::BoundingBoxes boxesMsg; + /// \brief 2D axis aligned bounding boxes msg + public: msgs::AnnotatedAxisAligned2DBox_V boxes2DMsg; + + /// \brief 3D oreinted bounding boxes msg + public: msgs::AnnotatedOriented3DBox_V boxes3DMsg; /// \brief Topic to publish the BoundingBox image public: std::string topicBoundingBoxes = ""; @@ -95,7 +98,7 @@ class ignition::sensors::BoundingBoxCameraSensorPrivate public: std::mutex mutex; /// \brief BoundingBoxes type - public: BoundingBoxType type {BoundingBoxType::VisibleBox}; + public: BoundingBoxType type {BoundingBoxType::VisibleBox2D}; }; ////////////////////////////////////////////////// @@ -141,10 +144,18 @@ bool BoundingBoxCameraSensor::Load(const sdf::Sensor &_sdf) c = std::tolower(c); }); - if (type == "full" || type == "full_box") - this->dataPtr->type = BoundingBoxType::FullBox; - else if (type == "visible" || type == "visible_box") - this->dataPtr->type = BoundingBoxType::VisibleBox; + if (type == "full_2d" || type == "full_box_2d") + this->dataPtr->type = BoundingBoxType::FullBox2D; + else if (type == "2d" || type == "visible_2d" + || type == "visible_box_2d") + this->dataPtr->type = BoundingBoxType::VisibleBox2D; + else if (type == "3d") + this->dataPtr->type = BoundingBoxType::Box3D; + else + { + ignerr << "Unknown bounding box type " << type << std::endl; + return false; + } } if (!Sensor::Load(_sdf)) @@ -176,9 +187,20 @@ bool BoundingBoxCameraSensor::Load(const sdf::Sensor &_sdf) this->dataPtr->node.Advertise( this->dataPtr->topicImage); - this->dataPtr->boxesPublisher = - this->dataPtr->node.Advertise( + if (this->dataPtr->type == BoundingBoxType::Box3D) + { + this->dataPtr->boxesPublisher = + this->dataPtr->node.Advertise< + ignition::msgs::AnnotatedOriented3DBox_V>( + this->dataPtr->topicBoundingBoxes); + } + else + { + this->dataPtr->boxesPublisher = + this->dataPtr->node.Advertise< + ignition::msgs::AnnotatedAxisAligned2DBox_V>( this->dataPtr->topicBoundingBoxes); + } if (!this->dataPtr->imagePublisher) { @@ -400,23 +422,36 @@ bool BoundingBoxCameraSensor::Update( this->dataPtr->imagePublisher.Publish(this->dataPtr->imageMsg); } - this->dataPtr->boxesMsg.Clear(); + if (this->dataPtr->type == BoundingBoxType::Box3D) + this->dataPtr->boxes3DMsg.Clear(); + else + this->dataPtr->boxes2DMsg.Clear(); // Create BoundingBoxes message for (auto box : this->dataPtr->boundingBoxes) { // box data - auto boxMsg = this->dataPtr->boxesMsg.add_box(); - boxMsg->set_minx(box.minX); - boxMsg->set_miny(box.minY); - boxMsg->set_maxx(box.maxX); - boxMsg->set_maxy(box.maxY); + auto boxMsg = this->dataPtr->boxes2DMsg.add_boxes(); + + auto axisAlignedBox = new msgs::AxisAligned2DBox(); + auto min_corner = new msgs::Vector2d(); + auto max_corner = new msgs::Vector2d(); + + min_corner->set_x(box.center.X() - box.size.X() / 2); + min_corner->set_y(box.center.Y() - box.size.Y() / 2); + + max_corner->set_x(box.center.X() + box.size.X() / 2); + max_corner->set_y(box.center.Y() + box.size.Y() / 2); + + axisAlignedBox->set_allocated_min_corner(min_corner); + axisAlignedBox->set_allocated_max_corner(max_corner); + boxMsg->set_allocated_box(axisAlignedBox); boxMsg->set_label(box.label); } // time stamp - auto stampBoxes = this->dataPtr->boxesMsg.mutable_header()->mutable_stamp(); + auto stampBoxes = this->dataPtr->boxes2DMsg.mutable_header()->mutable_stamp(); *stampBoxes = msgs::Convert(_now); - auto frameBoxes = this->dataPtr->boxesMsg.mutable_header()->add_data(); + auto frameBoxes = this->dataPtr->boxes2DMsg.mutable_header()->add_data(); frameBoxes->set_key("frame_id"); frameBoxes->add_value(this->Name()); @@ -424,8 +459,8 @@ bool BoundingBoxCameraSensor::Update( // Publish this->PublishInfo(_now); - this->AddSequence(this->dataPtr->boxesMsg.mutable_header(), "boundingboxes"); - this->dataPtr->boxesPublisher.Publish(this->dataPtr->boxesMsg); + this->AddSequence(this->dataPtr->boxes2DMsg.mutable_header(), "boundingboxes"); + this->dataPtr->boxesPublisher.Publish(this->dataPtr->boxes2DMsg); return true; } diff --git a/test/integration/boundingbox_camera_plugin.cc b/test/integration/boundingbox_camera_plugin.cc index 7ec78bdd..884871bf 100644 --- a/test/integration/boundingbox_camera_plugin.cc +++ b/test/integration/boundingbox_camera_plugin.cc @@ -1,297 +1,297 @@ -/* - * Copyright (C) 2021 Open Source Robotics Foundation - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - * -*/ - -#include - -#include -#include -#include - -#include -#include -#include -#include -#include - -#include "test_config.h" // NOLINT(build/include) -#include "TransportTestTools.hh" - -using namespace ignition; -using namespace rendering; - -class BoundingBoxCameraSensorTest: public testing::Test, - public testing::WithParamInterface -{ - // Create a BoundingBox Camera sensor from a SDF and gets a image message - public: void BoxesWithBuiltinSDF(const std::string &_renderEngine); -}; - -/// \brief mutex for thread safety -std::mutex g_mutex; - -/// \brief bounding boxes from the camera -std::vector g_boxes; - -/// \brief counter of received boundingboxes msg -int g_counter = 0; - -/// \brief callback to receive boxes from the camera -void OnNewBoundingBoxes(const msgs::BoundingBoxes &boxes) -{ - g_mutex.lock(); - g_boxes.clear(); - - int size = boxes.box_size(); - for (int i = 0; i < size; i++) - { - auto boxMsg = boxes.box(i); - BoundingBox box; - box.minX = boxMsg.minx(); - box.minY = boxMsg.miny(); - box.maxX = boxMsg.maxx(); - box.maxY = boxMsg.maxy(); - box.label = boxMsg.label(); - g_boxes.push_back(box); - } - g_counter++; - g_mutex.unlock(); -} - -/// \brief wait till you read the published frame -void WaitForNewFrame() -{ - auto waitTime = std::chrono::duration_cast< std::chrono::milliseconds >( - std::chrono::duration< double >(0.001)); - int counter = 0; - // wait for the counter to increase - for (int sleep = 0; sleep < 300 && counter == 0; ++sleep) - { - g_mutex.lock(); - counter = g_counter; - g_mutex.unlock(); - std::this_thread::sleep_for(waitTime); - } - EXPECT_GT(counter, 0); -} - -/// \brief Build the scene with 3 boxes 2 overlapping boxes which 1 -/// is behind the other, and the 3rd box is invisible behind them -void BuildScene(rendering::ScenePtr scene) -{ - math::Vector3d occludedPosition(4, 1, 0); - math::Vector3d frontPosition(2, 0, 0); - math::Vector3d invisiblePosition(5, 0, 0); - - rendering::VisualPtr root = scene->RootVisual(); - - // create front box visual (the smaller box) - rendering::VisualPtr occludedBox = scene->CreateVisual(); - occludedBox->AddGeometry(scene->CreateBox()); - occludedBox->SetOrigin(0.0, 0.0, 0.0); - occludedBox->SetLocalPosition(occludedPosition); - occludedBox->SetLocalRotation(0, 0, 0); - occludedBox->SetUserData("label", 1); - root->AddChild(occludedBox); - - // create occluded box visual (the bigger box) - rendering::VisualPtr frontBox = scene->CreateVisual(); - frontBox->AddGeometry(scene->CreateBox()); - frontBox->SetOrigin(0.0, 0.0, 0.0); - frontBox->SetLocalPosition(frontPosition); - frontBox->SetLocalRotation(0, 0, 0); - frontBox->SetUserData("label", 2); - root->AddChild(frontBox); - - rendering::VisualPtr invisibleBox = scene->CreateVisual(); - invisibleBox->AddGeometry(scene->CreateBox()); - invisibleBox->SetOrigin(0.0, 0.0, 0.0); - invisibleBox->SetLocalPosition(invisiblePosition); - invisibleBox->SetLocalRotation(0, 0, 0); - invisibleBox->SetUserData("label", 2); - root->AddChild(invisibleBox); -} - -///////////////////////////////////////////////// -void BoundingBoxCameraSensorTest::BoxesWithBuiltinSDF( - const std::string &_renderEngine) -{ - std::string path = ignition::common::joinPaths(PROJECT_SOURCE_PATH, "test", - "integration", "boundingbox_camera_sensor_builtin.sdf"); - sdf::SDFPtr doc(new sdf::SDF()); - sdf::init(doc); - ASSERT_TRUE(sdf::readFile(path, doc)); - ASSERT_NE(nullptr, doc->Root()); - ASSERT_TRUE(doc->Root()->HasElement("model")); - auto modelPtr = doc->Root()->GetElement("model"); - ASSERT_TRUE(modelPtr->HasElement("link")); - auto linkPtr = modelPtr->GetElement("link"); - ASSERT_TRUE(linkPtr->HasElement("sensor")); - auto sensorPtr = linkPtr->GetElement("sensor"); - ASSERT_TRUE(sensorPtr->HasElement("camera")); - auto cameraPtr = sensorPtr->GetElement("camera"); - ASSERT_TRUE(cameraPtr->HasElement("image")); - auto imagePtr = cameraPtr->GetElement("image"); - - unsigned int width = imagePtr->Get("width"); - unsigned int height = imagePtr->Get("height"); - - EXPECT_EQ(width, 320); - EXPECT_EQ(height, 240); - - // Setup ign-rendering with an empty scene - auto *engine = ignition::rendering::engine(_renderEngine); - if (!engine) - { - igndbg << "Engine '" << _renderEngine - << "' is not supported" << std::endl; - return; - } - - ignition::rendering::ScenePtr scene = engine->CreateScene("scene"); - BuildScene(scene); - - ignition::sensors::Manager mgr; - - sdf::Sensor sdfSensor; - sdfSensor.Load(sensorPtr); - - std::string type = sdfSensor.TypeStr(); - EXPECT_EQ(type, "boundingbox_camera"); - - ignition::sensors::BoundingBoxCameraSensor *sensor = - mgr.CreateSensor(sdfSensor); - - EXPECT_NE(sensor, nullptr); - sensor->SetScene(scene); - - EXPECT_EQ(width, sensor->ImageWidth()); - EXPECT_EQ(height, sensor->ImageHeight()); - - auto camera = sensor->BoundingBoxCamera(); - EXPECT_NE(camera, nullptr); - - camera->SetLocalPosition(0.0, 0.0, 0.0); - camera->SetLocalRotation(0.0, 0.0, 0.0); - camera->SetAspectRatio(1.333); - camera->SetHFOV(IGN_PI / 2); - camera->SetBoundingBoxType(BoundingBoxType::VisibleBox); - - EXPECT_EQ(camera->Type(), BoundingBoxType::VisibleBox); - EXPECT_EQ(camera->ImageWidth(), width); - EXPECT_EQ(camera->ImageHeight(), height); - - // Get the Msg - std::string topic = - "/test/integration/BoundingBoxCameraPlugin_boxesWithBuiltinSDF"; - WaitForMessageTestHelper helper(topic); - - // Update once to create image - mgr.RunOnce(std::chrono::steady_clock::duration::zero()); - - EXPECT_TRUE(helper.WaitForMessage()) << helper; - - // subscribe to the BoundingBox camera topic - ignition::transport::Node node; - node.Subscribe(topic, &OnNewBoundingBoxes); - - // wait for a few BoundingBox camera boxes - mgr.RunOnce(std::chrono::steady_clock::duration::zero(), true); - // wait for a new boxes - WaitForNewFrame(); - - // accepted error with +/- in pixels in comparing the box coordinates - int margin_error = 1; - - // Visible box test - g_mutex.lock(); - g_counter = 0; - - // check if the invisible 3rd box is not exist - EXPECT_EQ(g_boxes.size(), size_t(2)); - - BoundingBox occludedBox = g_boxes[0]; - BoundingBox frontBox = g_boxes[1]; - - unsigned int occludedLabel = 1; - unsigned int frontLabel = 2; - - // hard-coded comparasion with acceptable error - EXPECT_NEAR(occludedBox.minX, 91, margin_error); - EXPECT_NEAR(occludedBox.minY, 97, margin_error); - EXPECT_NEAR(occludedBox.maxX, 106, margin_error); - EXPECT_NEAR(occludedBox.maxY, 142, margin_error); - EXPECT_EQ(occludedBox.label, occludedLabel); - - EXPECT_NEAR(frontBox.minX, 107, margin_error); - EXPECT_NEAR(frontBox.minY, 67, margin_error); - EXPECT_NEAR(frontBox.maxX, 212, margin_error); - EXPECT_NEAR(frontBox.maxY, 172, margin_error); - EXPECT_EQ(frontBox.label, frontLabel); - - g_mutex.unlock(); - - // Full Boxes Type Test - camera->SetBoundingBoxType(BoundingBoxType::FullBox); - - // wait for bounding boxes - mgr.RunOnce(std::chrono::steady_clock::duration::zero(), true); - WaitForNewFrame(); - - // test - g_mutex.lock(); - g_counter = 0; - - // check the hidden box - EXPECT_EQ(g_boxes.size(), size_t(2)); - BoundingBox occludedFullBox = g_boxes[0]; - BoundingBox frontFullBox = g_boxes[1]; - - // coordinates of partially occluded object is bigger - EXPECT_NEAR(occludedFullBox.minX, 91, margin_error); - EXPECT_NEAR(occludedFullBox.minY, 97, margin_error); - EXPECT_NEAR(occludedFullBox.maxX, 142, margin_error); - EXPECT_NEAR(occludedFullBox.maxY, 142, margin_error); - EXPECT_EQ(occludedFullBox.label, occludedLabel); - - EXPECT_NEAR(frontFullBox.minX, 105, margin_error); - EXPECT_NEAR(frontFullBox.minY, 65, margin_error); - EXPECT_NEAR(frontFullBox.maxX, 214, margin_error); - EXPECT_NEAR(frontFullBox.maxY, 174, margin_error); - EXPECT_EQ(frontFullBox.label, frontLabel); - g_mutex.unlock(); - - // Clean up - engine->DestroyScene(scene); - ignition::rendering::unloadEngine(engine->Name()); -} - -////////////////////////////////////////////////// -TEST_P(BoundingBoxCameraSensorTest, BoxesWithBuiltinSDF) -{ - BoxesWithBuiltinSDF(GetParam()); -} - -INSTANTIATE_TEST_CASE_P(BoundingBoxCameraSensor, BoundingBoxCameraSensorTest, - RENDER_ENGINE_VALUES, ignition::rendering::PrintToStringParam()); - -////////////////////////////////////////////////// -int main(int argc, char **argv) -{ - ignition::common::Console::SetVerbosity(4); - ::testing::InitGoogleTest(&argc, argv); - return RUN_ALL_TESTS(); -} +// /* +// * Copyright (C) 2021 Open Source Robotics Foundation +// * +// * Licensed under the Apache License, Version 2.0 (the "License"); +// * you may not use this file except in compliance with the License. +// * You may obtain a copy of the License at +// * +// * http://www.apache.org/licenses/LICENSE-2.0 +// * +// * Unless required by applicable law or agreed to in writing, software +// * distributed under the License is distributed on an "AS IS" BASIS, +// * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// * See the License for the specific language governing permissions and +// * limitations under the License. +// * +// */ + +// #include + +// #include +// #include +// #include + +// #include +// #include +// #include +// #include +// #include + +// #include "test_config.h" // NOLINT(build/include) +// #include "TransportTestTools.hh" + +// using namespace ignition; +// using namespace rendering; + +// class BoundingBoxCameraSensorTest: public testing::Test, +// public testing::WithParamInterface +// { +// // Create a BoundingBox Camera sensor from a SDF and gets a image message +// public: void BoxesWithBuiltinSDF(const std::string &_renderEngine); +// }; + +// /// \brief mutex for thread safety +// std::mutex g_mutex; + +// /// \brief bounding boxes from the camera +// std::vector g_boxes; + +// /// \brief counter of received boundingboxes msg +// int g_counter = 0; + +// /// \brief callback to receive boxes from the camera +// void OnNewBoundingBoxes(const msgs::BoundingBoxes &boxes) +// { +// g_mutex.lock(); +// g_boxes.clear(); + +// int size = boxes.box_size(); +// for (int i = 0; i < size; i++) +// { +// auto boxMsg = boxes.box(i); +// BoundingBox box; +// box.minX = boxMsg.minx(); +// box.minY = boxMsg.miny(); +// box.maxX = boxMsg.maxx(); +// box.maxY = boxMsg.maxy(); +// box.label = boxMsg.label(); +// g_boxes.push_back(box); +// } +// g_counter++; +// g_mutex.unlock(); +// } + +// /// \brief wait till you read the published frame +// void WaitForNewFrame() +// { +// auto waitTime = std::chrono::duration_cast< std::chrono::milliseconds >( +// std::chrono::duration< double >(0.001)); +// int counter = 0; +// // wait for the counter to increase +// for (int sleep = 0; sleep < 300 && counter == 0; ++sleep) +// { +// g_mutex.lock(); +// counter = g_counter; +// g_mutex.unlock(); +// std::this_thread::sleep_for(waitTime); +// } +// EXPECT_GT(counter, 0); +// } + +// /// \brief Build the scene with 3 boxes 2 overlapping boxes which 1 +// /// is behind the other, and the 3rd box is invisible behind them +// void BuildScene(rendering::ScenePtr scene) +// { +// math::Vector3d occludedPosition(4, 1, 0); +// math::Vector3d frontPosition(2, 0, 0); +// math::Vector3d invisiblePosition(5, 0, 0); + +// rendering::VisualPtr root = scene->RootVisual(); + +// // create front box visual (the smaller box) +// rendering::VisualPtr occludedBox = scene->CreateVisual(); +// occludedBox->AddGeometry(scene->CreateBox()); +// occludedBox->SetOrigin(0.0, 0.0, 0.0); +// occludedBox->SetLocalPosition(occludedPosition); +// occludedBox->SetLocalRotation(0, 0, 0); +// occludedBox->SetUserData("label", 1); +// root->AddChild(occludedBox); + +// // create occluded box visual (the bigger box) +// rendering::VisualPtr frontBox = scene->CreateVisual(); +// frontBox->AddGeometry(scene->CreateBox()); +// frontBox->SetOrigin(0.0, 0.0, 0.0); +// frontBox->SetLocalPosition(frontPosition); +// frontBox->SetLocalRotation(0, 0, 0); +// frontBox->SetUserData("label", 2); +// root->AddChild(frontBox); + +// rendering::VisualPtr invisibleBox = scene->CreateVisual(); +// invisibleBox->AddGeometry(scene->CreateBox()); +// invisibleBox->SetOrigin(0.0, 0.0, 0.0); +// invisibleBox->SetLocalPosition(invisiblePosition); +// invisibleBox->SetLocalRotation(0, 0, 0); +// invisibleBox->SetUserData("label", 2); +// root->AddChild(invisibleBox); +// } + +// ///////////////////////////////////////////////// +// void BoundingBoxCameraSensorTest::BoxesWithBuiltinSDF( +// const std::string &_renderEngine) +// { +// std::string path = ignition::common::joinPaths(PROJECT_SOURCE_PATH, "test", +// "integration", "boundingbox_camera_sensor_builtin.sdf"); +// sdf::SDFPtr doc(new sdf::SDF()); +// sdf::init(doc); +// ASSERT_TRUE(sdf::readFile(path, doc)); +// ASSERT_NE(nullptr, doc->Root()); +// ASSERT_TRUE(doc->Root()->HasElement("model")); +// auto modelPtr = doc->Root()->GetElement("model"); +// ASSERT_TRUE(modelPtr->HasElement("link")); +// auto linkPtr = modelPtr->GetElement("link"); +// ASSERT_TRUE(linkPtr->HasElement("sensor")); +// auto sensorPtr = linkPtr->GetElement("sensor"); +// ASSERT_TRUE(sensorPtr->HasElement("camera")); +// auto cameraPtr = sensorPtr->GetElement("camera"); +// ASSERT_TRUE(cameraPtr->HasElement("image")); +// auto imagePtr = cameraPtr->GetElement("image"); + +// unsigned int width = imagePtr->Get("width"); +// unsigned int height = imagePtr->Get("height"); + +// EXPECT_EQ(width, 320); +// EXPECT_EQ(height, 240); + +// // Setup ign-rendering with an empty scene +// auto *engine = ignition::rendering::engine(_renderEngine); +// if (!engine) +// { +// igndbg << "Engine '" << _renderEngine +// << "' is not supported" << std::endl; +// return; +// } + +// ignition::rendering::ScenePtr scene = engine->CreateScene("scene"); +// BuildScene(scene); + +// ignition::sensors::Manager mgr; + +// sdf::Sensor sdfSensor; +// sdfSensor.Load(sensorPtr); + +// std::string type = sdfSensor.TypeStr(); +// EXPECT_EQ(type, "boundingbox_camera"); + +// ignition::sensors::BoundingBoxCameraSensor *sensor = +// mgr.CreateSensor(sdfSensor); + +// EXPECT_NE(sensor, nullptr); +// sensor->SetScene(scene); + +// EXPECT_EQ(width, sensor->ImageWidth()); +// EXPECT_EQ(height, sensor->ImageHeight()); + +// auto camera = sensor->BoundingBoxCamera(); +// EXPECT_NE(camera, nullptr); + +// camera->SetLocalPosition(0.0, 0.0, 0.0); +// camera->SetLocalRotation(0.0, 0.0, 0.0); +// camera->SetAspectRatio(1.333); +// camera->SetHFOV(IGN_PI / 2); +// camera->SetBoundingBoxType(BoundingBoxType::VisibleBox); + +// EXPECT_EQ(camera->Type(), BoundingBoxType::VisibleBox); +// EXPECT_EQ(camera->ImageWidth(), width); +// EXPECT_EQ(camera->ImageHeight(), height); + +// // Get the Msg +// std::string topic = +// "/test/integration/BoundingBoxCameraPlugin_boxesWithBuiltinSDF"; +// WaitForMessageTestHelper helper(topic); + +// // Update once to create image +// mgr.RunOnce(std::chrono::steady_clock::duration::zero()); + +// EXPECT_TRUE(helper.WaitForMessage()) << helper; + +// // subscribe to the BoundingBox camera topic +// ignition::transport::Node node; +// node.Subscribe(topic, &OnNewBoundingBoxes); + +// // wait for a few BoundingBox camera boxes +// mgr.RunOnce(std::chrono::steady_clock::duration::zero(), true); +// // wait for a new boxes +// WaitForNewFrame(); + +// // accepted error with +/- in pixels in comparing the box coordinates +// int margin_error = 1; + +// // Visible box test +// g_mutex.lock(); +// g_counter = 0; + +// // check if the invisible 3rd box is not exist +// EXPECT_EQ(g_boxes.size(), size_t(2)); + +// BoundingBox occludedBox = g_boxes[0]; +// BoundingBox frontBox = g_boxes[1]; + +// unsigned int occludedLabel = 1; +// unsigned int frontLabel = 2; + +// // hard-coded comparasion with acceptable error +// EXPECT_NEAR(occludedBox.minX, 91, margin_error); +// EXPECT_NEAR(occludedBox.minY, 97, margin_error); +// EXPECT_NEAR(occludedBox.maxX, 106, margin_error); +// EXPECT_NEAR(occludedBox.maxY, 142, margin_error); +// EXPECT_EQ(occludedBox.label, occludedLabel); + +// EXPECT_NEAR(frontBox.minX, 107, margin_error); +// EXPECT_NEAR(frontBox.minY, 67, margin_error); +// EXPECT_NEAR(frontBox.maxX, 212, margin_error); +// EXPECT_NEAR(frontBox.maxY, 172, margin_error); +// EXPECT_EQ(frontBox.label, frontLabel); + +// g_mutex.unlock(); + +// // Full Boxes Type Test +// camera->SetBoundingBoxType(BoundingBoxType::FullBox); + +// // wait for bounding boxes +// mgr.RunOnce(std::chrono::steady_clock::duration::zero(), true); +// WaitForNewFrame(); + +// // test +// g_mutex.lock(); +// g_counter = 0; + +// // check the hidden box +// EXPECT_EQ(g_boxes.size(), size_t(2)); +// BoundingBox occludedFullBox = g_boxes[0]; +// BoundingBox frontFullBox = g_boxes[1]; + +// // coordinates of partially occluded object is bigger +// EXPECT_NEAR(occludedFullBox.minX, 91, margin_error); +// EXPECT_NEAR(occludedFullBox.minY, 97, margin_error); +// EXPECT_NEAR(occludedFullBox.maxX, 142, margin_error); +// EXPECT_NEAR(occludedFullBox.maxY, 142, margin_error); +// EXPECT_EQ(occludedFullBox.label, occludedLabel); + +// EXPECT_NEAR(frontFullBox.minX, 105, margin_error); +// EXPECT_NEAR(frontFullBox.minY, 65, margin_error); +// EXPECT_NEAR(frontFullBox.maxX, 214, margin_error); +// EXPECT_NEAR(frontFullBox.maxY, 174, margin_error); +// EXPECT_EQ(frontFullBox.label, frontLabel); +// g_mutex.unlock(); + +// // Clean up +// engine->DestroyScene(scene); +// ignition::rendering::unloadEngine(engine->Name()); +// } + +// ////////////////////////////////////////////////// +// TEST_P(BoundingBoxCameraSensorTest, BoxesWithBuiltinSDF) +// { +// BoxesWithBuiltinSDF(GetParam()); +// } + +// INSTANTIATE_TEST_CASE_P(BoundingBoxCameraSensor, BoundingBoxCameraSensorTest, +// RENDER_ENGINE_VALUES, ignition::rendering::PrintToStringParam()); + +// ////////////////////////////////////////////////// +// int main(int argc, char **argv) +// { +// ignition::common::Console::SetVerbosity(4); +// ::testing::InitGoogleTest(&argc, argv); +// return RUN_ALL_TESTS(); +// } From 8a4ea0dbee4a6a278d42bb851e4e61ef249b0046 Mon Sep 17 00:00:00 2001 From: AmrElsersy Date: Thu, 15 Jul 2021 08:51:07 +0200 Subject: [PATCH 04/31] Add 3D Boxes Signed-off-by: AmrElsersy --- src/BoundingBoxCameraSensor.cc | 98 +++++++++++++++++++++++++--------- 1 file changed, 74 insertions(+), 24 deletions(-) diff --git a/src/BoundingBoxCameraSensor.cc b/src/BoundingBoxCameraSensor.cc index 3b4f8c17..750e0c4f 100644 --- a/src/BoundingBoxCameraSensor.cc +++ b/src/BoundingBoxCameraSensor.cc @@ -423,44 +423,94 @@ bool BoundingBoxCameraSensor::Update( } if (this->dataPtr->type == BoundingBoxType::Box3D) + { this->dataPtr->boxes3DMsg.Clear(); + + // Create 3D boxes message + for (auto box : this->dataPtr->boundingBoxes) + { + // box data + auto annotated_box = this->dataPtr->boxes3DMsg.add_annotated_box(); + + auto oriented3DBox = new msgs::Oriented3DBox(); + auto center = new msgs::Vector3d(); + auto size = new msgs::Vector3d(); + auto rotation = new msgs::Quaternion(); + + center->set_x(box.center.X()); + center->set_y(box.center.Y()); + center->set_z(box.center.Z()); + + size->set_x(box.size.X()); + size->set_y(box.size.Y()); + size->set_z(box.size.Z()); + + rotation->set_x(box.oreintation.X()); + rotation->set_y(box.oreintation.Y()); + rotation->set_z(box.oreintation.Z()); + rotation->set_w(box.oreintation.W()); + + oriented3DBox->set_allocated_center(center); + oriented3DBox->set_allocated_orientation(rotation); + oriented3DBox->set_allocated_boxsize(size); + + annotated_box->set_allocated_box(oriented3DBox); + annotated_box->set_label(box.label); + } + // time stamp + auto stampBoxes = this->dataPtr->boxes3DMsg.mutable_header()->mutable_stamp(); + *stampBoxes = msgs::Convert(_now); + auto frameBoxes = this->dataPtr->boxes3DMsg.mutable_header()->add_data(); + frameBoxes->set_key("frame_id"); + frameBoxes->add_value(this->Name()); + } else + { this->dataPtr->boxes2DMsg.Clear(); - // Create BoundingBoxes message - for (auto box : this->dataPtr->boundingBoxes) - { - // box data - auto boxMsg = this->dataPtr->boxes2DMsg.add_boxes(); + // Create 2D boxes message + for (auto box : this->dataPtr->boundingBoxes) + { + // box data + auto annotated_box = this->dataPtr->boxes2DMsg.add_annotated_box(); - auto axisAlignedBox = new msgs::AxisAligned2DBox(); - auto min_corner = new msgs::Vector2d(); - auto max_corner = new msgs::Vector2d(); + auto axisAlignedBox = new msgs::AxisAligned2DBox(); + auto min_corner = new msgs::Vector2d(); + auto max_corner = new msgs::Vector2d(); - min_corner->set_x(box.center.X() - box.size.X() / 2); - min_corner->set_y(box.center.Y() - box.size.Y() / 2); + min_corner->set_x(box.center.X() - box.size.X() / 2); + min_corner->set_y(box.center.Y() - box.size.Y() / 2); - max_corner->set_x(box.center.X() + box.size.X() / 2); - max_corner->set_y(box.center.Y() + box.size.Y() / 2); + max_corner->set_x(box.center.X() + box.size.X() / 2); + max_corner->set_y(box.center.Y() + box.size.Y() / 2); - axisAlignedBox->set_allocated_min_corner(min_corner); - axisAlignedBox->set_allocated_max_corner(max_corner); - boxMsg->set_allocated_box(axisAlignedBox); - boxMsg->set_label(box.label); + axisAlignedBox->set_allocated_min_corner(min_corner); + axisAlignedBox->set_allocated_max_corner(max_corner); + annotated_box->set_allocated_box(axisAlignedBox); + annotated_box->set_label(box.label); + } + // time stamp + auto stampBoxes = this->dataPtr->boxes2DMsg.mutable_header()->mutable_stamp(); + *stampBoxes = msgs::Convert(_now); + auto frameBoxes = this->dataPtr->boxes2DMsg.mutable_header()->add_data(); + frameBoxes->set_key("frame_id"); + frameBoxes->add_value(this->Name()); } - // time stamp - auto stampBoxes = this->dataPtr->boxes2DMsg.mutable_header()->mutable_stamp(); - *stampBoxes = msgs::Convert(_now); - auto frameBoxes = this->dataPtr->boxes2DMsg.mutable_header()->add_data(); - frameBoxes->set_key("frame_id"); - frameBoxes->add_value(this->Name()); std::lock_guard lock(this->dataPtr->mutex); // Publish this->PublishInfo(_now); - this->AddSequence(this->dataPtr->boxes2DMsg.mutable_header(), "boundingboxes"); - this->dataPtr->boxesPublisher.Publish(this->dataPtr->boxes2DMsg); + if (this->dataPtr->type == BoundingBoxType::Box3D) + { + this->AddSequence(this->dataPtr->boxes3DMsg.mutable_header(), "boundingboxes"); + this->dataPtr->boxesPublisher.Publish(this->dataPtr->boxes3DMsg); + } + else + { + this->AddSequence(this->dataPtr->boxes2DMsg.mutable_header(), "boundingboxes"); + this->dataPtr->boxesPublisher.Publish(this->dataPtr->boxes2DMsg); + } return true; } From 907302b617f645c2cf5ec835c69737485521593c Mon Sep 17 00:00:00 2001 From: AmrElsersy Date: Thu, 15 Jul 2021 13:02:34 +0200 Subject: [PATCH 05/31] Testing 3D Signed-off-by: AmrElsersy --- src/BoundingBoxCameraSensor.cc | 12 +- .../boundingbox_3d_camera_sensor_builtin.sdf | 22 + test/integration/boundingbox_camera_plugin.cc | 754 +++++++++++------- 3 files changed, 491 insertions(+), 297 deletions(-) create mode 100644 test/integration/boundingbox_3d_camera_sensor_builtin.sdf diff --git a/src/BoundingBoxCameraSensor.cc b/src/BoundingBoxCameraSensor.cc index 750e0c4f..b4776009 100644 --- a/src/BoundingBoxCameraSensor.cc +++ b/src/BoundingBoxCameraSensor.cc @@ -458,7 +458,8 @@ bool BoundingBoxCameraSensor::Update( annotated_box->set_label(box.label); } // time stamp - auto stampBoxes = this->dataPtr->boxes3DMsg.mutable_header()->mutable_stamp(); + auto stampBoxes = + this->dataPtr->boxes3DMsg.mutable_header()->mutable_stamp(); *stampBoxes = msgs::Convert(_now); auto frameBoxes = this->dataPtr->boxes3DMsg.mutable_header()->add_data(); frameBoxes->set_key("frame_id"); @@ -490,7 +491,8 @@ bool BoundingBoxCameraSensor::Update( annotated_box->set_label(box.label); } // time stamp - auto stampBoxes = this->dataPtr->boxes2DMsg.mutable_header()->mutable_stamp(); + auto stampBoxes = + this->dataPtr->boxes2DMsg.mutable_header()->mutable_stamp(); *stampBoxes = msgs::Convert(_now); auto frameBoxes = this->dataPtr->boxes2DMsg.mutable_header()->add_data(); frameBoxes->set_key("frame_id"); @@ -503,12 +505,14 @@ bool BoundingBoxCameraSensor::Update( this->PublishInfo(_now); if (this->dataPtr->type == BoundingBoxType::Box3D) { - this->AddSequence(this->dataPtr->boxes3DMsg.mutable_header(), "boundingboxes"); + this->AddSequence( + this->dataPtr->boxes3DMsg.mutable_header(), "boundingboxes"); this->dataPtr->boxesPublisher.Publish(this->dataPtr->boxes3DMsg); } else { - this->AddSequence(this->dataPtr->boxes2DMsg.mutable_header(), "boundingboxes"); + this->AddSequence( + this->dataPtr->boxes2DMsg.mutable_header(), "boundingboxes"); this->dataPtr->boxesPublisher.Publish(this->dataPtr->boxes2DMsg); } diff --git a/test/integration/boundingbox_3d_camera_sensor_builtin.sdf b/test/integration/boundingbox_3d_camera_sensor_builtin.sdf new file mode 100644 index 00000000..7c1ae648 --- /dev/null +++ b/test/integration/boundingbox_3d_camera_sensor_builtin.sdf @@ -0,0 +1,22 @@ + + + + + + 10 + /test/integration/BoundingBoxCameraPlugin_boxesWithBuiltinSDF + 3d + + + 320 + 240 + + + 0.1 + 1000.0 + + + + + + \ No newline at end of file diff --git a/test/integration/boundingbox_camera_plugin.cc b/test/integration/boundingbox_camera_plugin.cc index 884871bf..49405fbf 100644 --- a/test/integration/boundingbox_camera_plugin.cc +++ b/test/integration/boundingbox_camera_plugin.cc @@ -1,297 +1,465 @@ -// /* -// * Copyright (C) 2021 Open Source Robotics Foundation -// * -// * Licensed under the Apache License, Version 2.0 (the "License"); -// * you may not use this file except in compliance with the License. -// * You may obtain a copy of the License at -// * -// * http://www.apache.org/licenses/LICENSE-2.0 -// * -// * Unless required by applicable law or agreed to in writing, software -// * distributed under the License is distributed on an "AS IS" BASIS, -// * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// * See the License for the specific language governing permissions and -// * limitations under the License. -// * -// */ - -// #include - -// #include -// #include -// #include - -// #include -// #include -// #include -// #include -// #include - -// #include "test_config.h" // NOLINT(build/include) -// #include "TransportTestTools.hh" - -// using namespace ignition; -// using namespace rendering; - -// class BoundingBoxCameraSensorTest: public testing::Test, -// public testing::WithParamInterface +/* + * Copyright (C) 2021 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * +*/ + +#include + +#include +#include +#include + +#include +#include +#include +#include +#include + +#include "test_config.h" // NOLINT(build/include) +#include "TransportTestTools.hh" + +using namespace ignition; +using namespace rendering; + +class BoundingBoxCameraSensorTest: public testing::Test, + public testing::WithParamInterface +{ + // Create a BoundingBox Camera sensor from a SDF and gets a image message + public: void BoxesWithBuiltinSDF(const std::string &_renderEngine); + + public: void Boxes3DWithBuiltinSDF(const std::string &renderEngine); +}; + +/// \brief mutex for thread safety +std::mutex g_mutex; + +/// \brief bounding boxes from the camera +std::vector g_boxes; + +/// \brief counter of received boundingboxes msg +int g_counter = 0; + +/// \brief callback to receive 2d boxes from the camera +void OnNewBoundingBoxes(const msgs::AnnotatedAxisAligned2DBox_V &boxes) +{ + g_mutex.lock(); + g_boxes.clear(); + + int size = boxes.annotated_box_size(); + for (int i = 0; i < size; i++) + { + BoundingBox box(BoundingBoxType::VisibleBox2D); + auto annotated_box = boxes.annotated_box(i); + auto axisAlignedBox = annotated_box.box(); + auto min_corner = axisAlignedBox.min_corner(); + auto max_corner = axisAlignedBox.max_corner(); + + box.size.X(max_corner.x() - min_corner.x()); + box.size.Y(max_corner.y() - min_corner.y()); + box.size.Z(0); + + box.center.X(min_corner.x() + box.size.X() / 2); + box.center.Y(min_corner.y() + box.size.Y() / 2); + box.center.Z(0); + + box.oreintation.Set(0, 0, 0, 0); + box.label = annotated_box.label(); + g_boxes.push_back(box); + } + g_counter++; + g_mutex.unlock(); +} + +/// \brief callback to receive 3D boxes from the camera +void OnNew3DBoundingBoxes(const msgs::AnnotatedOriented3DBox_V &boxes) +{ + g_mutex.lock(); + g_boxes.clear(); + + int size = boxes.annotated_box_size(); + for (int i = 0; i < size; i++) + { + BoundingBox box(BoundingBoxType::VisibleBox2D); + auto annotated_box = boxes.annotated_box(i); + auto orientedBox = annotated_box.box(); + // center + box.center.Set(orientedBox.center().x(), orientedBox.center().y(), + orientedBox.center().z()); + // orientation + auto orientation = orientedBox.orientation(); + box.oreintation.Set(orientation.x(), orientation.y(), + orientation.z(), orientation.w()); + // size + auto boxSize = orientedBox.boxsize(); + box.size.Set(boxSize.x(), boxSize.y(), boxSize.z()); + + box.label = annotated_box.label(); + g_boxes.push_back(box); + } + g_counter++; + g_mutex.unlock(); + std::cout << "Received boxes " << g_counter << std::endl; +} + +/// \brief wait till you read the published frame +void WaitForNewFrame() +{ + auto waitTime = std::chrono::duration_cast< std::chrono::milliseconds >( + std::chrono::duration< double >(0.001)); + int counter = 0; + // wait for the counter to increase + for (int sleep = 0; sleep < 300 && counter == 0; ++sleep) + { + g_mutex.lock(); + counter = g_counter; + g_mutex.unlock(); + std::this_thread::sleep_for(waitTime); + } + EXPECT_GT(counter, 0); +} + +/// \brief Build the scene with 3 boxes 2 overlapping boxes which 1 +/// is behind the other, and the 3rd box is invisible behind them +void BuildScene(rendering::ScenePtr scene) +{ + math::Vector3d occludedPosition(4, 1, 0); + math::Vector3d frontPosition(2, 0, 0); + math::Vector3d invisiblePosition(5, 0, 0); + + rendering::VisualPtr root = scene->RootVisual(); + + // create front box visual (the smaller box) + rendering::VisualPtr occludedBox = scene->CreateVisual(); + occludedBox->AddGeometry(scene->CreateBox()); + occludedBox->SetOrigin(0.0, 0.0, 0.0); + occludedBox->SetLocalPosition(occludedPosition); + occludedBox->SetLocalRotation(0, 0, 0); + occludedBox->SetUserData("label", 1); + root->AddChild(occludedBox); + + // create occluded box visual (the bigger box) + rendering::VisualPtr frontBox = scene->CreateVisual(); + frontBox->AddGeometry(scene->CreateBox()); + frontBox->SetOrigin(0.0, 0.0, 0.0); + frontBox->SetLocalPosition(frontPosition); + frontBox->SetLocalRotation(0, 0, 0); + frontBox->SetUserData("label", 2); + root->AddChild(frontBox); + + rendering::VisualPtr invisibleBox = scene->CreateVisual(); + invisibleBox->AddGeometry(scene->CreateBox()); + invisibleBox->SetOrigin(0.0, 0.0, 0.0); + invisibleBox->SetLocalPosition(invisiblePosition); + invisibleBox->SetLocalRotation(0, 0, 0); + invisibleBox->SetUserData("label", 2); + root->AddChild(invisibleBox); +} + +///////////////////////////////////////////////// +void BoundingBoxCameraSensorTest::BoxesWithBuiltinSDF( + const std::string &_renderEngine) +{ + std::string path = ignition::common::joinPaths(PROJECT_SOURCE_PATH, "test", + "integration", "boundingbox_camera_sensor_builtin.sdf"); + sdf::SDFPtr doc(new sdf::SDF()); + sdf::init(doc); + ASSERT_TRUE(sdf::readFile(path, doc)); + ASSERT_NE(nullptr, doc->Root()); + ASSERT_TRUE(doc->Root()->HasElement("model")); + auto modelPtr = doc->Root()->GetElement("model"); + ASSERT_TRUE(modelPtr->HasElement("link")); + auto linkPtr = modelPtr->GetElement("link"); + ASSERT_TRUE(linkPtr->HasElement("sensor")); + auto sensorPtr = linkPtr->GetElement("sensor"); + ASSERT_TRUE(sensorPtr->HasElement("camera")); + auto cameraPtr = sensorPtr->GetElement("camera"); + ASSERT_TRUE(cameraPtr->HasElement("image")); + auto imagePtr = cameraPtr->GetElement("image"); + + unsigned int width = imagePtr->Get("width"); + unsigned int height = imagePtr->Get("height"); + + EXPECT_EQ(width, 320u); + EXPECT_EQ(height, 240u); + + // Setup ign-rendering with an empty scene + auto *engine = ignition::rendering::engine(_renderEngine); + if (!engine) + { + igndbg << "Engine '" << _renderEngine + << "' is not supported" << std::endl; + return; + } + + ignition::rendering::ScenePtr scene = engine->CreateScene("scene"); + BuildScene(scene); + + ignition::sensors::Manager mgr; + + sdf::Sensor sdfSensor; + sdfSensor.Load(sensorPtr); + + std::string type = sdfSensor.TypeStr(); + EXPECT_EQ(type, "boundingbox_camera"); + + ignition::sensors::BoundingBoxCameraSensor *sensor = + mgr.CreateSensor(sdfSensor); + + EXPECT_NE(sensor, nullptr); + sensor->SetScene(scene); + + EXPECT_EQ(width, sensor->ImageWidth()); + EXPECT_EQ(height, sensor->ImageHeight()); + + auto camera = sensor->BoundingBoxCamera(); + EXPECT_NE(camera, nullptr); + + camera->SetLocalPosition(0.0, 0.0, 0.0); + camera->SetLocalRotation(0.0, 0.0, 0.0); + camera->SetAspectRatio(1.333); + camera->SetHFOV(IGN_PI / 2); + camera->SetBoundingBoxType(BoundingBoxType::VisibleBox2D); + + EXPECT_EQ(camera->Type(), BoundingBoxType::VisibleBox2D); + EXPECT_EQ(camera->ImageWidth(), width); + EXPECT_EQ(camera->ImageHeight(), height); + + // Get the Msg + std::string topic = + "/test/integration/BoundingBoxCameraPlugin_boxesWithBuiltinSDF"; + WaitForMessageTestHelper< + ignition::msgs::AnnotatedAxisAligned2DBox_V> helper(topic); + + // Update once to create image + mgr.RunOnce(std::chrono::steady_clock::duration::zero()); + + EXPECT_TRUE(helper.WaitForMessage()) << helper; + + // subscribe to the BoundingBox camera topic + ignition::transport::Node node; + node.Subscribe(topic, &OnNewBoundingBoxes); + + // wait for a few BoundingBox camera boxes + mgr.RunOnce(std::chrono::steady_clock::duration::zero(), true); + // wait for a new boxes + WaitForNewFrame(); + + // accepted error with +/- in pixels in comparing the box coordinates + double margin_error = 1; + + // Visible box test + g_mutex.lock(); + g_counter = 0; + + // check if the invisible 3rd box is not exist + EXPECT_EQ(g_boxes.size(), size_t(2)); + + BoundingBox occludedBox = g_boxes[0]; + BoundingBox frontBox = g_boxes[1]; + + unsigned int occludedLabel = 1; + unsigned int frontLabel = 2; + + // hard-coded comparasion with acceptable error + EXPECT_NEAR(occludedBox.center.X(), 98, margin_error); + EXPECT_NEAR(occludedBox.center.Y(), 119, margin_error); + EXPECT_NEAR(occludedBox.size.X(), 15, margin_error); + EXPECT_NEAR(occludedBox.size.Y(), 45, margin_error); + EXPECT_EQ(occludedBox.label, occludedLabel); + + EXPECT_NEAR(frontBox.center.X(), 159, margin_error); + EXPECT_NEAR(frontBox.center.Y(), 119, margin_error); + EXPECT_NEAR(frontBox.size.X(), 105, margin_error); + EXPECT_NEAR(frontBox.size.Y(), 105, margin_error); + EXPECT_EQ(frontBox.label, frontLabel); + + g_mutex.unlock(); + + // Full Boxes Type Test + camera->SetBoundingBoxType(BoundingBoxType::FullBox2D); + + // wait for bounding boxes + mgr.RunOnce(std::chrono::steady_clock::duration::zero(), true); + WaitForNewFrame(); + + // test + g_mutex.lock(); + g_counter = 0; + + // check the hidden box + EXPECT_EQ(g_boxes.size(), size_t(2)); + BoundingBox occludedFullBox = g_boxes[0]; + BoundingBox frontFullBox = g_boxes[1]; + + // coordinates of partially occluded object is bigger + EXPECT_NEAR(occludedFullBox.center.X(), 116, margin_error); + EXPECT_NEAR(occludedFullBox.center.Y(), 119, margin_error); + EXPECT_NEAR(occludedFullBox.size.X(), 51, margin_error); + EXPECT_NEAR(occludedFullBox.size.Y(), 45, margin_error); + EXPECT_EQ(occludedFullBox.label, occludedLabel); + + EXPECT_NEAR(frontFullBox.center.X(), 159, margin_error); + EXPECT_NEAR(frontFullBox.center.Y(), 119, margin_error); + EXPECT_NEAR(frontFullBox.size.X(), 108, margin_error); + EXPECT_NEAR(frontFullBox.size.Y(), 108, margin_error); + EXPECT_EQ(frontFullBox.label, frontLabel); + + g_mutex.unlock(); + + // Clean up + engine->DestroyScene(scene); + ignition::rendering::unloadEngine(engine->Name()); +} + +///////////////////////////////////////////////// +void BoundingBoxCameraSensorTest::Boxes3DWithBuiltinSDF( + const std::string &_renderEngine) +{ + std::string path = ignition::common::joinPaths(PROJECT_SOURCE_PATH, "test", + "integration", "boundingbox_camera_sensor_builtin.sdf"); + sdf::SDFPtr doc(new sdf::SDF()); + sdf::init(doc); + ASSERT_TRUE(sdf::readFile(path, doc)); + ASSERT_NE(nullptr, doc->Root()); + ASSERT_TRUE(doc->Root()->HasElement("model")); + auto modelPtr = doc->Root()->GetElement("model"); + ASSERT_TRUE(modelPtr->HasElement("link")); + auto linkPtr = modelPtr->GetElement("link"); + ASSERT_TRUE(linkPtr->HasElement("sensor")); + auto sensorPtr = linkPtr->GetElement("sensor"); + ASSERT_TRUE(sensorPtr->HasElement("camera")); + auto cameraPtr = sensorPtr->GetElement("camera"); + ASSERT_TRUE(cameraPtr->HasElement("image")); + auto imagePtr = cameraPtr->GetElement("image"); + + unsigned int width = imagePtr->Get("width"); + unsigned int height = imagePtr->Get("height"); + + EXPECT_EQ(width, 320u); + EXPECT_EQ(height, 240u); + + // Setup ign-rendering with an empty scene + auto *engine = ignition::rendering::engine(_renderEngine); + if (!engine) + { + igndbg << "Engine '" << _renderEngine + << "' is not supported" << std::endl; + return; + } + + ignition::rendering::ScenePtr scene = engine->CreateScene("scene"); + BuildScene(scene); + + ignition::sensors::Manager mgr; + + sdf::Sensor sdfSensor; + sdfSensor.Load(sensorPtr); + + std::string type = sdfSensor.TypeStr(); + EXPECT_EQ(type, "boundingbox_camera"); + + ignition::sensors::BoundingBoxCameraSensor *sensor = + mgr.CreateSensor(sdfSensor); + + EXPECT_NE(sensor, nullptr); + sensor->SetScene(scene); + + EXPECT_EQ(width, sensor->ImageWidth()); + EXPECT_EQ(height, sensor->ImageHeight()); + + auto camera = sensor->BoundingBoxCamera(); + EXPECT_NE(camera, nullptr); + + camera->SetLocalPosition(0.0, 0.0, 0.0); + camera->SetLocalRotation(0.0, 0.0, 0.0); + camera->SetAspectRatio(1.333); + camera->SetHFOV(IGN_PI / 2); + camera->SetBoundingBoxType(BoundingBoxType::Box3D); + + // Get the Msg + std::string topic = + "/test/integration/BoundingBox3DCameraPlugin_boxesWithBuiltinSDF"; + WaitForMessageTestHelper< + ignition::msgs::AnnotatedOriented3DBox_V> helper(topic); + std::cout << 1 << std::endl; + // Update once to create image + mgr.RunOnce(std::chrono::steady_clock::duration::zero()); + std::cout << 1 << std::endl; + + EXPECT_TRUE(helper.WaitForMessage()) << helper; + std::cout << 1 << std::endl; + + // subscribe to the BoundingBox camera topic + ignition::transport::Node node; + node.Subscribe(topic, &OnNew3DBoundingBoxes); + std::cout << 1 << std::endl; + + // wait for a few BoundingBox camera boxes + mgr.RunOnce(std::chrono::steady_clock::duration::zero(), true); + // wait for a new boxes + WaitForNewFrame(); + std::cout << 1 << std::endl; + + g_mutex.lock(); + g_counter = 0; + + // check the hidden box + EXPECT_EQ(g_boxes.size(), size_t(2)); + BoundingBox box3D = g_boxes[0]; + + double margin_error = 0.01; + EXPECT_NEAR(box3D.center.X(), -1, margin_error); + EXPECT_NEAR(box3D.center.Y(), 0, margin_error); + EXPECT_NEAR(box3D.center.Z(), -4, margin_error); + + EXPECT_NEAR(box3D.size.X(), 1, margin_error); + EXPECT_NEAR(box3D.size.Y(), 1, margin_error); + EXPECT_NEAR(box3D.size.Z(), 1, margin_error); + + EXPECT_NEAR(box3D.oreintation.X(), 0.5, margin_error); + EXPECT_NEAR(box3D.oreintation.Y(), 0.5, margin_error); + EXPECT_NEAR(box3D.oreintation.Z(), -0.5, margin_error); + EXPECT_NEAR(box3D.oreintation.W(), -0.5, margin_error); + + std::cout << box3D.center << " " << box3D.size << " " << box3D.oreintation << std::endl; + g_mutex.unlock(); + + // Clean up + engine->DestroyScene(scene); + ignition::rendering::unloadEngine(engine->Name()); +} + +////////////////////////////////////////////////// +TEST_P(BoundingBoxCameraSensorTest, BoxesWithBuiltinSDF) +{ + BoxesWithBuiltinSDF(GetParam()); +} + +////////////////////////////////////////////////// +// TEST_P(BoundingBoxCameraSensorTest, Boxes3DWithBuiltinSDF) // { -// // Create a BoundingBox Camera sensor from a SDF and gets a image message -// public: void BoxesWithBuiltinSDF(const std::string &_renderEngine); -// }; - -// /// \brief mutex for thread safety -// std::mutex g_mutex; - -// /// \brief bounding boxes from the camera -// std::vector g_boxes; - -// /// \brief counter of received boundingboxes msg -// int g_counter = 0; - -// /// \brief callback to receive boxes from the camera -// void OnNewBoundingBoxes(const msgs::BoundingBoxes &boxes) -// { -// g_mutex.lock(); -// g_boxes.clear(); - -// int size = boxes.box_size(); -// for (int i = 0; i < size; i++) -// { -// auto boxMsg = boxes.box(i); -// BoundingBox box; -// box.minX = boxMsg.minx(); -// box.minY = boxMsg.miny(); -// box.maxX = boxMsg.maxx(); -// box.maxY = boxMsg.maxy(); -// box.label = boxMsg.label(); -// g_boxes.push_back(box); -// } -// g_counter++; -// g_mutex.unlock(); +// Boxes3DWithBuiltinSDF(GetParam()); // } -// /// \brief wait till you read the published frame -// void WaitForNewFrame() -// { -// auto waitTime = std::chrono::duration_cast< std::chrono::milliseconds >( -// std::chrono::duration< double >(0.001)); -// int counter = 0; -// // wait for the counter to increase -// for (int sleep = 0; sleep < 300 && counter == 0; ++sleep) -// { -// g_mutex.lock(); -// counter = g_counter; -// g_mutex.unlock(); -// std::this_thread::sleep_for(waitTime); -// } -// EXPECT_GT(counter, 0); -// } - -// /// \brief Build the scene with 3 boxes 2 overlapping boxes which 1 -// /// is behind the other, and the 3rd box is invisible behind them -// void BuildScene(rendering::ScenePtr scene) -// { -// math::Vector3d occludedPosition(4, 1, 0); -// math::Vector3d frontPosition(2, 0, 0); -// math::Vector3d invisiblePosition(5, 0, 0); - -// rendering::VisualPtr root = scene->RootVisual(); - -// // create front box visual (the smaller box) -// rendering::VisualPtr occludedBox = scene->CreateVisual(); -// occludedBox->AddGeometry(scene->CreateBox()); -// occludedBox->SetOrigin(0.0, 0.0, 0.0); -// occludedBox->SetLocalPosition(occludedPosition); -// occludedBox->SetLocalRotation(0, 0, 0); -// occludedBox->SetUserData("label", 1); -// root->AddChild(occludedBox); - -// // create occluded box visual (the bigger box) -// rendering::VisualPtr frontBox = scene->CreateVisual(); -// frontBox->AddGeometry(scene->CreateBox()); -// frontBox->SetOrigin(0.0, 0.0, 0.0); -// frontBox->SetLocalPosition(frontPosition); -// frontBox->SetLocalRotation(0, 0, 0); -// frontBox->SetUserData("label", 2); -// root->AddChild(frontBox); - -// rendering::VisualPtr invisibleBox = scene->CreateVisual(); -// invisibleBox->AddGeometry(scene->CreateBox()); -// invisibleBox->SetOrigin(0.0, 0.0, 0.0); -// invisibleBox->SetLocalPosition(invisiblePosition); -// invisibleBox->SetLocalRotation(0, 0, 0); -// invisibleBox->SetUserData("label", 2); -// root->AddChild(invisibleBox); -// } - -// ///////////////////////////////////////////////// -// void BoundingBoxCameraSensorTest::BoxesWithBuiltinSDF( -// const std::string &_renderEngine) -// { -// std::string path = ignition::common::joinPaths(PROJECT_SOURCE_PATH, "test", -// "integration", "boundingbox_camera_sensor_builtin.sdf"); -// sdf::SDFPtr doc(new sdf::SDF()); -// sdf::init(doc); -// ASSERT_TRUE(sdf::readFile(path, doc)); -// ASSERT_NE(nullptr, doc->Root()); -// ASSERT_TRUE(doc->Root()->HasElement("model")); -// auto modelPtr = doc->Root()->GetElement("model"); -// ASSERT_TRUE(modelPtr->HasElement("link")); -// auto linkPtr = modelPtr->GetElement("link"); -// ASSERT_TRUE(linkPtr->HasElement("sensor")); -// auto sensorPtr = linkPtr->GetElement("sensor"); -// ASSERT_TRUE(sensorPtr->HasElement("camera")); -// auto cameraPtr = sensorPtr->GetElement("camera"); -// ASSERT_TRUE(cameraPtr->HasElement("image")); -// auto imagePtr = cameraPtr->GetElement("image"); - -// unsigned int width = imagePtr->Get("width"); -// unsigned int height = imagePtr->Get("height"); - -// EXPECT_EQ(width, 320); -// EXPECT_EQ(height, 240); - -// // Setup ign-rendering with an empty scene -// auto *engine = ignition::rendering::engine(_renderEngine); -// if (!engine) -// { -// igndbg << "Engine '" << _renderEngine -// << "' is not supported" << std::endl; -// return; -// } - -// ignition::rendering::ScenePtr scene = engine->CreateScene("scene"); -// BuildScene(scene); - -// ignition::sensors::Manager mgr; - -// sdf::Sensor sdfSensor; -// sdfSensor.Load(sensorPtr); - -// std::string type = sdfSensor.TypeStr(); -// EXPECT_EQ(type, "boundingbox_camera"); - -// ignition::sensors::BoundingBoxCameraSensor *sensor = -// mgr.CreateSensor(sdfSensor); - -// EXPECT_NE(sensor, nullptr); -// sensor->SetScene(scene); - -// EXPECT_EQ(width, sensor->ImageWidth()); -// EXPECT_EQ(height, sensor->ImageHeight()); - -// auto camera = sensor->BoundingBoxCamera(); -// EXPECT_NE(camera, nullptr); - -// camera->SetLocalPosition(0.0, 0.0, 0.0); -// camera->SetLocalRotation(0.0, 0.0, 0.0); -// camera->SetAspectRatio(1.333); -// camera->SetHFOV(IGN_PI / 2); -// camera->SetBoundingBoxType(BoundingBoxType::VisibleBox); - -// EXPECT_EQ(camera->Type(), BoundingBoxType::VisibleBox); -// EXPECT_EQ(camera->ImageWidth(), width); -// EXPECT_EQ(camera->ImageHeight(), height); - -// // Get the Msg -// std::string topic = -// "/test/integration/BoundingBoxCameraPlugin_boxesWithBuiltinSDF"; -// WaitForMessageTestHelper helper(topic); - -// // Update once to create image -// mgr.RunOnce(std::chrono::steady_clock::duration::zero()); - -// EXPECT_TRUE(helper.WaitForMessage()) << helper; - -// // subscribe to the BoundingBox camera topic -// ignition::transport::Node node; -// node.Subscribe(topic, &OnNewBoundingBoxes); - -// // wait for a few BoundingBox camera boxes -// mgr.RunOnce(std::chrono::steady_clock::duration::zero(), true); -// // wait for a new boxes -// WaitForNewFrame(); - -// // accepted error with +/- in pixels in comparing the box coordinates -// int margin_error = 1; - -// // Visible box test -// g_mutex.lock(); -// g_counter = 0; - -// // check if the invisible 3rd box is not exist -// EXPECT_EQ(g_boxes.size(), size_t(2)); - -// BoundingBox occludedBox = g_boxes[0]; -// BoundingBox frontBox = g_boxes[1]; - -// unsigned int occludedLabel = 1; -// unsigned int frontLabel = 2; - -// // hard-coded comparasion with acceptable error -// EXPECT_NEAR(occludedBox.minX, 91, margin_error); -// EXPECT_NEAR(occludedBox.minY, 97, margin_error); -// EXPECT_NEAR(occludedBox.maxX, 106, margin_error); -// EXPECT_NEAR(occludedBox.maxY, 142, margin_error); -// EXPECT_EQ(occludedBox.label, occludedLabel); - -// EXPECT_NEAR(frontBox.minX, 107, margin_error); -// EXPECT_NEAR(frontBox.minY, 67, margin_error); -// EXPECT_NEAR(frontBox.maxX, 212, margin_error); -// EXPECT_NEAR(frontBox.maxY, 172, margin_error); -// EXPECT_EQ(frontBox.label, frontLabel); - -// g_mutex.unlock(); - -// // Full Boxes Type Test -// camera->SetBoundingBoxType(BoundingBoxType::FullBox); +INSTANTIATE_TEST_CASE_P(BoundingBoxCameraSensor, BoundingBoxCameraSensorTest, + RENDER_ENGINE_VALUES, ignition::rendering::PrintToStringParam()); -// // wait for bounding boxes -// mgr.RunOnce(std::chrono::steady_clock::duration::zero(), true); -// WaitForNewFrame(); - -// // test -// g_mutex.lock(); -// g_counter = 0; - -// // check the hidden box -// EXPECT_EQ(g_boxes.size(), size_t(2)); -// BoundingBox occludedFullBox = g_boxes[0]; -// BoundingBox frontFullBox = g_boxes[1]; - -// // coordinates of partially occluded object is bigger -// EXPECT_NEAR(occludedFullBox.minX, 91, margin_error); -// EXPECT_NEAR(occludedFullBox.minY, 97, margin_error); -// EXPECT_NEAR(occludedFullBox.maxX, 142, margin_error); -// EXPECT_NEAR(occludedFullBox.maxY, 142, margin_error); -// EXPECT_EQ(occludedFullBox.label, occludedLabel); - -// EXPECT_NEAR(frontFullBox.minX, 105, margin_error); -// EXPECT_NEAR(frontFullBox.minY, 65, margin_error); -// EXPECT_NEAR(frontFullBox.maxX, 214, margin_error); -// EXPECT_NEAR(frontFullBox.maxY, 174, margin_error); -// EXPECT_EQ(frontFullBox.label, frontLabel); -// g_mutex.unlock(); - -// // Clean up -// engine->DestroyScene(scene); -// ignition::rendering::unloadEngine(engine->Name()); -// } - -// ////////////////////////////////////////////////// -// TEST_P(BoundingBoxCameraSensorTest, BoxesWithBuiltinSDF) -// { -// BoxesWithBuiltinSDF(GetParam()); -// } - -// INSTANTIATE_TEST_CASE_P(BoundingBoxCameraSensor, BoundingBoxCameraSensorTest, -// RENDER_ENGINE_VALUES, ignition::rendering::PrintToStringParam()); - -// ////////////////////////////////////////////////// -// int main(int argc, char **argv) -// { -// ignition::common::Console::SetVerbosity(4); -// ::testing::InitGoogleTest(&argc, argv); -// return RUN_ALL_TESTS(); -// } +////////////////////////////////////////////////// +int main(int argc, char **argv) +{ + ignition::common::Console::SetVerbosity(4); + ::testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} From 026a8697b24c685daf3736013d7f759915f59982 Mon Sep 17 00:00:00 2001 From: AmrElsersy Date: Fri, 16 Jul 2021 06:42:12 +0200 Subject: [PATCH 06/31] Fix 3D testing Signed-off-by: AmrElsersy --- .../boundingbox_3d_camera_sensor_builtin.sdf | 4 +- test/integration/boundingbox_camera_plugin.cc | 59 +++++++++++-------- 2 files changed, 36 insertions(+), 27 deletions(-) diff --git a/test/integration/boundingbox_3d_camera_sensor_builtin.sdf b/test/integration/boundingbox_3d_camera_sensor_builtin.sdf index 7c1ae648..08b0b4eb 100644 --- a/test/integration/boundingbox_3d_camera_sensor_builtin.sdf +++ b/test/integration/boundingbox_3d_camera_sensor_builtin.sdf @@ -4,8 +4,8 @@ 10 - /test/integration/BoundingBoxCameraPlugin_boxesWithBuiltinSDF - 3d + /test/integration/BoundingBox3DCameraPlugin_boxesWithBuiltinSDF + 3d 320 diff --git a/test/integration/boundingbox_camera_plugin.cc b/test/integration/boundingbox_camera_plugin.cc index 49405fbf..89f44d1c 100644 --- a/test/integration/boundingbox_camera_plugin.cc +++ b/test/integration/boundingbox_camera_plugin.cc @@ -36,7 +36,7 @@ using namespace rendering; class BoundingBoxCameraSensorTest: public testing::Test, public testing::WithParamInterface { - // Create a BoundingBox Camera sensor from a SDF and gets a image message + // Create a BoundingBox Camera sensor from a SDF and gets a boxes message public: void BoxesWithBuiltinSDF(const std::string &_renderEngine); public: void Boxes3DWithBuiltinSDF(const std::string &renderEngine); @@ -91,7 +91,7 @@ void OnNew3DBoundingBoxes(const msgs::AnnotatedOriented3DBox_V &boxes) int size = boxes.annotated_box_size(); for (int i = 0; i < size; i++) { - BoundingBox box(BoundingBoxType::VisibleBox2D); + BoundingBox box(BoundingBoxType::Box3D); auto annotated_box = boxes.annotated_box(i); auto orientedBox = annotated_box.box(); // center @@ -99,8 +99,8 @@ void OnNew3DBoundingBoxes(const msgs::AnnotatedOriented3DBox_V &boxes) orientedBox.center().z()); // orientation auto orientation = orientedBox.orientation(); - box.oreintation.Set(orientation.x(), orientation.y(), - orientation.z(), orientation.w()); + box.oreintation.Set(orientation.w(), orientation.x(), + orientation.y(), orientation.z()); // size auto boxSize = orientedBox.boxsize(); box.size.Set(boxSize.x(), boxSize.y(), boxSize.z()); @@ -110,7 +110,6 @@ void OnNew3DBoundingBoxes(const msgs::AnnotatedOriented3DBox_V &boxes) } g_counter++; g_mutex.unlock(); - std::cout << "Received boxes " << g_counter << std::endl; } /// \brief wait till you read the published frame @@ -130,7 +129,7 @@ void WaitForNewFrame() EXPECT_GT(counter, 0); } -/// \brief Build the scene with 3 boxes 2 overlapping boxes which 1 +/// \brief Build a scene with 3 boxes 2 overlapping boxes which 1 /// is behind the other, and the 3rd box is invisible behind them void BuildScene(rendering::ScenePtr scene) { @@ -167,6 +166,21 @@ void BuildScene(rendering::ScenePtr scene) root->AddChild(invisibleBox); } +/// \brief Build a scene with 3d oreinted box +void BuildScene3D(rendering::ScenePtr scene) +{ + rendering::VisualPtr root = scene->RootVisual(); + + // create front box visual (the smaller box) + rendering::VisualPtr box = scene->CreateVisual(); + box->AddGeometry(scene->CreateBox()); + box->SetOrigin(0.0, 0.0, 0.0); + box->SetLocalPosition(math::Vector3d(2, 0, 0)); + box->SetLocalRotation(math::Quaternion(1.0, 0.5, 0.5, 0.2)); + box->SetUserData("label", 1); + root->AddChild(box); +} + ///////////////////////////////////////////////// void BoundingBoxCameraSensorTest::BoxesWithBuiltinSDF( const std::string &_renderEngine) @@ -328,7 +342,7 @@ void BoundingBoxCameraSensorTest::Boxes3DWithBuiltinSDF( const std::string &_renderEngine) { std::string path = ignition::common::joinPaths(PROJECT_SOURCE_PATH, "test", - "integration", "boundingbox_camera_sensor_builtin.sdf"); + "integration", "boundingbox_3d_camera_sensor_builtin.sdf"); sdf::SDFPtr doc(new sdf::SDF()); sdf::init(doc); ASSERT_TRUE(sdf::readFile(path, doc)); @@ -360,7 +374,7 @@ void BoundingBoxCameraSensorTest::Boxes3DWithBuiltinSDF( } ignition::rendering::ScenePtr scene = engine->CreateScene("scene"); - BuildScene(scene); + BuildScene3D(scene); ignition::sensors::Manager mgr; @@ -393,47 +407,42 @@ void BoundingBoxCameraSensorTest::Boxes3DWithBuiltinSDF( "/test/integration/BoundingBox3DCameraPlugin_boxesWithBuiltinSDF"; WaitForMessageTestHelper< ignition::msgs::AnnotatedOriented3DBox_V> helper(topic); - std::cout << 1 << std::endl; + // Update once to create image mgr.RunOnce(std::chrono::steady_clock::duration::zero()); - std::cout << 1 << std::endl; EXPECT_TRUE(helper.WaitForMessage()) << helper; - std::cout << 1 << std::endl; // subscribe to the BoundingBox camera topic ignition::transport::Node node; node.Subscribe(topic, &OnNew3DBoundingBoxes); - std::cout << 1 << std::endl; // wait for a few BoundingBox camera boxes mgr.RunOnce(std::chrono::steady_clock::duration::zero(), true); // wait for a new boxes WaitForNewFrame(); - std::cout << 1 << std::endl; g_mutex.lock(); g_counter = 0; // check the hidden box - EXPECT_EQ(g_boxes.size(), size_t(2)); + EXPECT_EQ(g_boxes.size(), size_t(1)); BoundingBox box3D = g_boxes[0]; double margin_error = 0.01; - EXPECT_NEAR(box3D.center.X(), -1, margin_error); + EXPECT_NEAR(box3D.center.X(), 0, margin_error); EXPECT_NEAR(box3D.center.Y(), 0, margin_error); - EXPECT_NEAR(box3D.center.Z(), -4, margin_error); + EXPECT_NEAR(box3D.center.Z(), -2, margin_error); EXPECT_NEAR(box3D.size.X(), 1, margin_error); EXPECT_NEAR(box3D.size.Y(), 1, margin_error); EXPECT_NEAR(box3D.size.Z(), 1, margin_error); - EXPECT_NEAR(box3D.oreintation.X(), 0.5, margin_error); - EXPECT_NEAR(box3D.oreintation.Y(), 0.5, margin_error); - EXPECT_NEAR(box3D.oreintation.Z(), -0.5, margin_error); - EXPECT_NEAR(box3D.oreintation.W(), -0.5, margin_error); + EXPECT_NEAR(box3D.oreintation.X(), 0.322329, margin_error); + EXPECT_NEAR(box3D.oreintation.Y(), -0.886405, margin_error); + EXPECT_NEAR(box3D.oreintation.Z(), -0.0805823, margin_error); + EXPECT_NEAR(box3D.oreintation.W(), -0.322329, margin_error); - std::cout << box3D.center << " " << box3D.size << " " << box3D.oreintation << std::endl; g_mutex.unlock(); // Clean up @@ -448,10 +457,10 @@ TEST_P(BoundingBoxCameraSensorTest, BoxesWithBuiltinSDF) } ////////////////////////////////////////////////// -// TEST_P(BoundingBoxCameraSensorTest, Boxes3DWithBuiltinSDF) -// { -// Boxes3DWithBuiltinSDF(GetParam()); -// } +TEST_P(BoundingBoxCameraSensorTest, Boxes3DWithBuiltinSDF) +{ + Boxes3DWithBuiltinSDF(GetParam()); +} INSTANTIATE_TEST_CASE_P(BoundingBoxCameraSensor, BoundingBoxCameraSensorTest, RENDER_ENGINE_VALUES, ignition::rendering::PrintToStringParam()); From f7cecfbbf14985354d2d98f3cc66d13a04cfcbdc Mon Sep 17 00:00:00 2001 From: AmrElsersy Date: Fri, 16 Jul 2021 10:39:31 +0200 Subject: [PATCH 07/31] Near/Far plane Signed-off-by: AmrElsersy --- src/BoundingBoxCameraSensor.cc | 2 -- 1 file changed, 2 deletions(-) diff --git a/src/BoundingBoxCameraSensor.cc b/src/BoundingBoxCameraSensor.cc index b4776009..3ae750fc 100644 --- a/src/BoundingBoxCameraSensor.cc +++ b/src/BoundingBoxCameraSensor.cc @@ -301,8 +301,6 @@ bool BoundingBoxCameraSensor::CreateCamera() this->dataPtr->boundingboxCamera->SetImageHeight(height); this->dataPtr->boundingboxCamera->SetNearClipPlane(sdfCamera->NearClip()); this->dataPtr->boundingboxCamera->SetFarClipPlane(sdfCamera->FarClip()); - this->dataPtr->boundingboxCamera->SetNearClipPlane(0.01); - this->dataPtr->boundingboxCamera->SetFarClipPlane(1000); this->dataPtr->boundingboxCamera->SetImageFormat(PixelFormat::PF_R8G8B8); this->dataPtr->boundingboxCamera->SetAspectRatio(aspectRatio); this->dataPtr->boundingboxCamera->SetHFOV(angle); From 79355fb104d832782c48d1f3c0421f25395a7f4c Mon Sep 17 00:00:00 2001 From: AmrElsersy Date: Thu, 22 Jul 2021 14:21:51 +0200 Subject: [PATCH 08/31] style Signed-off-by: AmrElsersy --- .../sensors/BoundingBoxCameraSensor.hh | 23 ++-- src/BoundingBoxCameraSensor.cc | 111 ++++++++---------- src/CMakeLists.txt | 2 +- 3 files changed, 66 insertions(+), 70 deletions(-) diff --git a/include/ignition/sensors/BoundingBoxCameraSensor.hh b/include/ignition/sensors/BoundingBoxCameraSensor.hh index 91a8c28e..9e0c4126 100644 --- a/include/ignition/sensors/BoundingBoxCameraSensor.hh +++ b/include/ignition/sensors/BoundingBoxCameraSensor.hh @@ -15,8 +15,8 @@ * */ -#ifndef IGNITION_SENSORS_BOUNDINGBOXCAMERASENSOR2_HH_ -#define IGNITION_SENSORS_BOUNDINGBOXCAMERASENSOR2_HH_ +#ifndef IGNITION_SENSORS_BOUNDINGBOXCAMERASENSOR_HH_ +#define IGNITION_SENSORS_BOUNDINGBOXCAMERASENSOR_HH_ #include #include @@ -26,16 +26,17 @@ #include #include #include +#include +#include +#include +#include -#include "ignition/msgs.hh" -#include "ignition/transport/Node.hh" -#include "ignition/transport/Publisher.hh" - -#include "ignition/rendering/BoundingBoxCamera.hh" #include "ignition/sensors/CameraSensor.hh" #include "ignition/sensors/Export.hh" #include "ignition/sensors/Sensor.hh" +#include "ignition/sensors/boundingbox_camera/Export.hh" + namespace ignition { namespace sensors @@ -52,7 +53,8 @@ namespace ignition /// It offers both an ignition-transport interface and a direct C++ API /// to access the image data. The API works by setting a callback to be /// called with image data. - class BoundingBoxCameraSensor : public CameraSensor + class IGNITION_SENSORS_BOUNDINGBOX_CAMERA_VISIBLE + BoundingBoxCameraSensor : public CameraSensor { /// \brief constructor public: BoundingBoxCameraSensor(); @@ -82,12 +84,13 @@ namespace ignition /// \brief Get the rendering BoundingBox camera /// \return BoundingBox camera pointer - public: virtual rendering::BoundingBoxCameraPtr BoundingBoxCamera(); + public: virtual rendering::BoundingBoxCameraPtr + BoundingBoxCamera() const; /// \brief Callback on new bounding boxes from bounding boxes camera /// \param[in] _boxes Detected bounding boxes from the camera public: void OnNewBoundingBoxes( - const std::vector &boxes); + const std::vector &_boxes); /// \brief Set the rendering scene. /// \param[in] _scene Pointer to the scene diff --git a/src/BoundingBoxCameraSensor.cc b/src/BoundingBoxCameraSensor.cc index 3ae750fc..387b204f 100644 --- a/src/BoundingBoxCameraSensor.cc +++ b/src/BoundingBoxCameraSensor.cc @@ -18,23 +18,20 @@ #include #include -#include "ignition/common/Console.hh" -#include "ignition/common/Profiler.hh" -#include "ignition/common/Image.hh" +#include +#include +#include #include "ignition/sensors/RenderingEvents.hh" #include "ignition/sensors/SensorFactory.hh" - #include "ignition/sensors/BoundingBoxCameraSensor.hh" -#include "ignition/rendering/BoundingBoxCamera.hh" - -#include "ignition/transport/Node.hh" -#include "ignition/transport/Publisher.hh" -#include "ignition/msgs.hh" +#include +#include +#include +#include using namespace ignition; using namespace sensors; -using namespace rendering; class ignition::sensors::BoundingBoxCameraSensorPrivate { @@ -49,7 +46,7 @@ class ignition::sensors::BoundingBoxCameraSensorPrivate /// \brief Rendering RGB Camera to draw boxes on it and publish /// its image (just for visualization) - public: rendering::CameraPtr camera {nullptr}; + public: rendering::CameraPtr rgbCamera {nullptr}; /// \brief Node to create publisher public: transport::Node node; @@ -69,18 +66,14 @@ class ignition::sensors::BoundingBoxCameraSensorPrivate /// \brief 3D oreinted bounding boxes msg public: msgs::AnnotatedOriented3DBox_V boxes3DMsg; - /// \brief Topic to publish the BoundingBox image + /// \brief Topic to publish the bounding box msg public: std::string topicBoundingBoxes = ""; - /// \brief Topic to publish the BoundingBox image + /// \brief Topic to publish the image with drawn boxes public: std::string topicImage = ""; - /// \brief True when the sensor published image with drawn boxes - /// False when the sensor publish only the boxes - public: bool IsPublishedImage {false}; - - /// \brief Buffer contains the BoundingBox map data - public: std::vector boundingBoxes; + /// \brief Vector to receive boxes from the rendering camera + public: std::vector boundingBoxes; /// \brief RGB Image to draw boxes on it public: rendering::Image image; @@ -98,7 +91,8 @@ class ignition::sensors::BoundingBoxCameraSensorPrivate public: std::mutex mutex; /// \brief BoundingBoxes type - public: BoundingBoxType type {BoundingBoxType::VisibleBox2D}; + public: rendering::BoundingBoxType type + {rendering::BoundingBoxType::VisibleBox2D}; }; ////////////////////////////////////////////////// @@ -145,12 +139,12 @@ bool BoundingBoxCameraSensor::Load(const sdf::Sensor &_sdf) }); if (type == "full_2d" || type == "full_box_2d") - this->dataPtr->type = BoundingBoxType::FullBox2D; + this->dataPtr->type = rendering::BoundingBoxType::FullBox2D; else if (type == "2d" || type == "visible_2d" || type == "visible_box_2d") - this->dataPtr->type = BoundingBoxType::VisibleBox2D; + this->dataPtr->type = rendering::BoundingBoxType::VisibleBox2D; else if (type == "3d") - this->dataPtr->type = BoundingBoxType::Box3D; + this->dataPtr->type = rendering::BoundingBoxType::Box3D; else { ignerr << "Unknown bounding box type " << type << std::endl; @@ -187,7 +181,7 @@ bool BoundingBoxCameraSensor::Load(const sdf::Sensor &_sdf) this->dataPtr->node.Advertise( this->dataPtr->topicImage); - if (this->dataPtr->type == BoundingBoxType::Box3D) + if (this->dataPtr->type == rendering::BoundingBoxType::Box3D) { this->dataPtr->boxesPublisher = this->dataPtr->node.Advertise< @@ -204,14 +198,14 @@ bool BoundingBoxCameraSensor::Load(const sdf::Sensor &_sdf) if (!this->dataPtr->imagePublisher) { - ignerr << "Unable to create publisher on topic[" + ignerr << "Unable to create publisher on topic [" << this->dataPtr->topicImage << "].\n"; return false; } if (!this->dataPtr->boxesPublisher) { - ignerr << "Unable to create publisher on topic[" + ignerr << "Unable to create publisher on topic [" << this->dataPtr->topicBoundingBoxes << "].\n"; return false; } @@ -244,7 +238,7 @@ void BoundingBoxCameraSensor::SetScene( if (this->Scene() != _scene) { this->dataPtr->boundingboxCamera = nullptr; - this->dataPtr->camera = nullptr; + this->dataPtr->rgbCamera = nullptr; RenderingSensor::SetScene(_scene); if (this->dataPtr->initialized) @@ -265,13 +259,13 @@ bool BoundingBoxCameraSensor::CreateCamera() // Camera Info Msg this->PopulateInfo(sdfCamera); - if (!this->dataPtr->camera) + if (!this->dataPtr->rgbCamera) { // Create rendering camera this->dataPtr->boundingboxCamera = this->Scene()->CreateBoundingBoxCamera(this->Name()); - this->dataPtr->camera = this->Scene()->CreateCamera( + this->dataPtr->rgbCamera = this->Scene()->CreateCamera( this->Name() + "_rgbCamera"); } @@ -279,14 +273,14 @@ bool BoundingBoxCameraSensor::CreateCamera() auto height = sdfCamera->ImageHeight(); // Set Camera Properties - this->dataPtr->camera->SetImageFormat(rendering::PF_R8G8B8); - this->dataPtr->camera->SetImageWidth(width); - this->dataPtr->camera->SetImageHeight(height); - this->dataPtr->camera->SetVisibilityMask(sdfCamera->VisibilityMask()); - this->dataPtr->camera->SetNearClipPlane(sdfCamera->NearClip()); - this->dataPtr->camera->SetFarClipPlane(sdfCamera->FarClip()); - this->dataPtr->camera->SetNearClipPlane(0.01); - this->dataPtr->camera->SetFarClipPlane(1000); + this->dataPtr->rgbCamera->SetImageFormat(rendering::PF_R8G8B8); + this->dataPtr->rgbCamera->SetImageWidth(width); + this->dataPtr->rgbCamera->SetImageHeight(height); + this->dataPtr->rgbCamera->SetVisibilityMask(sdfCamera->VisibilityMask()); + this->dataPtr->rgbCamera->SetNearClipPlane(sdfCamera->NearClip()); + this->dataPtr->rgbCamera->SetFarClipPlane(sdfCamera->FarClip()); + this->dataPtr->rgbCamera->SetNearClipPlane(0.01); + this->dataPtr->rgbCamera->SetFarClipPlane(1000); math::Angle angle = sdfCamera->HorizontalFov(); if (angle < 0.01 || angle > IGN_PI*2) { @@ -294,14 +288,15 @@ bool BoundingBoxCameraSensor::CreateCamera() return false; } double aspectRatio = static_cast(width)/height; - this->dataPtr->camera->SetAspectRatio(aspectRatio); - this->dataPtr->camera->SetHFOV(angle); + this->dataPtr->rgbCamera->SetAspectRatio(aspectRatio); + this->dataPtr->rgbCamera->SetHFOV(angle); this->dataPtr->boundingboxCamera->SetImageWidth(width); this->dataPtr->boundingboxCamera->SetImageHeight(height); this->dataPtr->boundingboxCamera->SetNearClipPlane(sdfCamera->NearClip()); this->dataPtr->boundingboxCamera->SetFarClipPlane(sdfCamera->FarClip()); - this->dataPtr->boundingboxCamera->SetImageFormat(PixelFormat::PF_R8G8B8); + this->dataPtr->boundingboxCamera->SetImageFormat( + rendering::PixelFormat::PF_R8G8B8); this->dataPtr->boundingboxCamera->SetAspectRatio(aspectRatio); this->dataPtr->boundingboxCamera->SetHFOV(angle); this->dataPtr->boundingboxCamera->SetVisibilityMask( @@ -309,7 +304,7 @@ bool BoundingBoxCameraSensor::CreateCamera() this->dataPtr->boundingboxCamera->SetBoundingBoxType(this->dataPtr->type); // Add the camera to the scene - this->Scene()->RootVisual()->AddChild(this->dataPtr->camera); + this->Scene()->RootVisual()->AddChild(this->dataPtr->rgbCamera); this->Scene()->RootVisual()->AddChild(this->dataPtr->boundingboxCamera); // Add the rendering sensor to handle its render, no need to add the @@ -322,13 +317,14 @@ bool BoundingBoxCameraSensor::CreateCamera() std::bind(&BoundingBoxCameraSensor::OnNewBoundingBoxes, this, std::placeholders::_1)); - this->dataPtr->image = this->dataPtr->camera->CreateImage(); + this->dataPtr->image = this->dataPtr->rgbCamera->CreateImage(); return true; } ///////////////////////////////////////////////// -rendering::BoundingBoxCameraPtr BoundingBoxCameraSensor::BoundingBoxCamera() +rendering::BoundingBoxCameraPtr + BoundingBoxCameraSensor::BoundingBoxCamera() const { return this->dataPtr->boundingboxCamera; } @@ -339,7 +335,7 @@ void BoundingBoxCameraSensor::OnNewBoundingBoxes( { std::lock_guard lock(this->dataPtr->mutex); this->dataPtr->boundingBoxes.clear(); - for (auto box : boxes) + for (const auto &box : boxes) this->dataPtr->boundingBoxes.push_back(box); } @@ -354,7 +350,7 @@ bool BoundingBoxCameraSensor::Update( return false; } - if (!this->dataPtr->boundingboxCamera || !this->dataPtr->camera) + if (!this->dataPtr->boundingboxCamera || !this->dataPtr->rgbCamera) { ignerr << "Camera doesn't exist.\n"; return false; @@ -371,18 +367,15 @@ bool BoundingBoxCameraSensor::Update( this->Render(); // Publish image only if there is subscribers for it - this->dataPtr->IsPublishedImage = - this->dataPtr->imagePublisher.HasConnections(); - - if (this->dataPtr->IsPublishedImage) + if (this->dataPtr->imagePublisher.HasConnections()) { // The sensor updates only the bounding box camera with its pose // as it has the same name, so make rgb camera with the same pose - this->dataPtr->camera->SetWorldPose( + this->dataPtr->rgbCamera->SetWorldPose( this->dataPtr->boundingboxCamera->WorldPose()); // Render the rgb camera - this->dataPtr->camera->Capture(this->dataPtr->image); + this->dataPtr->rgbCamera->Capture(this->dataPtr->image); // Draw bounding boxes for (auto box : this->dataPtr->boundingBoxes) @@ -393,8 +386,8 @@ bool BoundingBoxCameraSensor::Update( this->dataPtr->imageBuffer, box); } - auto width = this->dataPtr->camera->ImageWidth(); - auto height = this->dataPtr->camera->ImageHeight(); + auto width = this->dataPtr->rgbCamera->ImageWidth(); + auto height = this->dataPtr->rgbCamera->ImageHeight(); // Create Image message this->dataPtr->imageMsg.set_width(width); @@ -420,12 +413,12 @@ bool BoundingBoxCameraSensor::Update( this->dataPtr->imagePublisher.Publish(this->dataPtr->imageMsg); } - if (this->dataPtr->type == BoundingBoxType::Box3D) + if (this->dataPtr->type == rendering::BoundingBoxType::Box3D) { this->dataPtr->boxes3DMsg.Clear(); // Create 3D boxes message - for (auto box : this->dataPtr->boundingBoxes) + for (const auto &box : this->dataPtr->boundingBoxes) { // box data auto annotated_box = this->dataPtr->boxes3DMsg.add_annotated_box(); @@ -468,7 +461,7 @@ bool BoundingBoxCameraSensor::Update( this->dataPtr->boxes2DMsg.Clear(); // Create 2D boxes message - for (auto box : this->dataPtr->boundingBoxes) + for (const auto &box : this->dataPtr->boundingBoxes) { // box data auto annotated_box = this->dataPtr->boxes2DMsg.add_annotated_box(); @@ -501,7 +494,7 @@ bool BoundingBoxCameraSensor::Update( // Publish this->PublishInfo(_now); - if (this->dataPtr->type == BoundingBoxType::Box3D) + if (this->dataPtr->type == rendering::BoundingBoxType::Box3D) { this->AddSequence( this->dataPtr->boxes3DMsg.mutable_header(), "boundingboxes"); @@ -520,13 +513,13 @@ bool BoundingBoxCameraSensor::Update( ///////////////////////////////////////////////// unsigned int BoundingBoxCameraSensor::ImageHeight() const { - return this->dataPtr->camera->ImageHeight(); + return this->dataPtr->rgbCamera->ImageHeight(); } ///////////////////////////////////////////////// unsigned int BoundingBoxCameraSensor::ImageWidth() const { - return this->dataPtr->camera->ImageWidth(); + return this->dataPtr->rgbCamera->ImageWidth(); } IGN_SENSORS_REGISTER_SENSOR(BoundingBoxCameraSensor) diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index 18f67e05..f07d7c62 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -144,7 +144,7 @@ target_link_libraries(${boundingbox_camera_target} ${camera_target} ignition-msgs${IGN_MSGS_VER}::ignition-msgs${IGN_MSGS_VER} ignition-transport${IGN_TRANSPORT_VER}::ignition-transport${IGN_TRANSPORT_VER} - ) +) # Build the unit tests. ign_build_tests(TYPE UNIT SOURCES ${gtest_sources} LIB_DEPS ${rendering_target}) From ff5e94a3cfe7120d9ac618c2ef38e5ec7f5f6e05 Mon Sep 17 00:00:00 2001 From: AmrElsersy Date: Thu, 5 Aug 2021 12:35:04 +0200 Subject: [PATCH 09/31] style Signed-off-by: AmrElsersy --- .../sensors/BoundingBoxCameraSensor.hh | 5 +++-- src/BoundingBoxCameraSensor.cc | 19 +++++++++---------- 2 files changed, 12 insertions(+), 12 deletions(-) diff --git a/include/ignition/sensors/BoundingBoxCameraSensor.hh b/include/ignition/sensors/BoundingBoxCameraSensor.hh index 9e0c4126..77b3de32 100644 --- a/include/ignition/sensors/BoundingBoxCameraSensor.hh +++ b/include/ignition/sensors/BoundingBoxCameraSensor.hh @@ -21,15 +21,16 @@ #include #include #include + #include #include #include #include #include #include +#include #include #include -#include #include "ignition/sensors/CameraSensor.hh" #include "ignition/sensors/Export.hh" @@ -48,7 +49,7 @@ namespace ignition /// \brief BoundingBox camera sensor class. /// - /// This class creates BoundingBox image from an ignition rendering scene. + /// This class creates a BoundingBox image from an ignition rendering scene. /// The scene must be created in advance and given to Manager::Init(). /// It offers both an ignition-transport interface and a direct C++ API /// to access the image data. The API works by setting a callback to be diff --git a/src/BoundingBoxCameraSensor.cc b/src/BoundingBoxCameraSensor.cc index 387b204f..cdb291db 100644 --- a/src/BoundingBoxCameraSensor.cc +++ b/src/BoundingBoxCameraSensor.cc @@ -15,20 +15,21 @@ * */ -#include +#include "ignition/sensors/BoundingBoxCameraSensor.hh" + #include +#include #include -#include #include -#include "ignition/sensors/RenderingEvents.hh" -#include "ignition/sensors/SensorFactory.hh" -#include "ignition/sensors/BoundingBoxCameraSensor.hh" +#include +#include #include #include #include -#include +#include "ignition/sensors/RenderingEvents.hh" +#include "ignition/sensors/SensorFactory.hh" using namespace ignition; using namespace sensors; @@ -279,8 +280,6 @@ bool BoundingBoxCameraSensor::CreateCamera() this->dataPtr->rgbCamera->SetVisibilityMask(sdfCamera->VisibilityMask()); this->dataPtr->rgbCamera->SetNearClipPlane(sdfCamera->NearClip()); this->dataPtr->rgbCamera->SetFarClipPlane(sdfCamera->FarClip()); - this->dataPtr->rgbCamera->SetNearClipPlane(0.01); - this->dataPtr->rgbCamera->SetFarClipPlane(1000); math::Angle angle = sdfCamera->HorizontalFov(); if (angle < 0.01 || angle > IGN_PI*2) { @@ -356,7 +355,7 @@ bool BoundingBoxCameraSensor::Update( return false; } - // don't render if there is no subscribers + // don't render if there are no subscribers if (!this->dataPtr->imagePublisher.HasConnections() && !this->dataPtr->boxesPublisher.HasConnections()) { @@ -378,7 +377,7 @@ bool BoundingBoxCameraSensor::Update( this->dataPtr->rgbCamera->Capture(this->dataPtr->image); // Draw bounding boxes - for (auto box : this->dataPtr->boundingBoxes) + for (const auto &box : this->dataPtr->boundingBoxes) { this->dataPtr->imageBuffer = this->dataPtr->image.Data(); From cfdece730bc6a09d98b658cb27fca0ba69c8831f Mon Sep 17 00:00:00 2001 From: AmrElsersy Date: Fri, 6 Aug 2021 13:35:15 +0200 Subject: [PATCH 10/31] Add Tutorial Signed-off-by: AmrElsersy --- tutorials/04_boundingbox_camera.md | 328 ++++++++++++++++++ .../boundingbox_camera_full.png | Bin 0 -> 93533 bytes .../boundingbox_camera_visible.png | Bin 0 -> 95656 bytes 3 files changed, 328 insertions(+) create mode 100644 tutorials/04_boundingbox_camera.md create mode 100644 tutorials/files/boundingbox_camera/boundingbox_camera_full.png create mode 100644 tutorials/files/boundingbox_camera/boundingbox_camera_visible.png diff --git a/tutorials/04_boundingbox_camera.md b/tutorials/04_boundingbox_camera.md new file mode 100644 index 00000000..eacc689d --- /dev/null +++ b/tutorials/04_boundingbox_camera.md @@ -0,0 +1,328 @@ +# Bounding Box Camera in Ignition Gazebo +In this tutorial, we will discuss how to use a bounding box camera sensor in Ignition Gazebo. + +## Requirements + +Since this tutorial will show how to use a bounding box camera sensor in Ignition Gazebo, you'll need to have Ignition Gazebo installed. We recommend installing all Ignition libraries, using version Fortress or newer (the bounding box camera is not available in Ignition versions prior to Fortress). +If you need to install Ignition, pick the version you'd like to use and then follow the installation instructions. + +## Setting up the bounding box camera +Here's an example of how to attach a bounding box camera sensor to a model in a SDF file: + +```xml + + 4 0 1.0 0 0.0 3.14 + + 0.05 0.05 0.05 0 0 0 + + 0.1 + + 0.000166667 + 0.000166667 + 0.000166667 + + + + + + 0.1 0.1 0.1 + + + + + + + 0.1 0.1 0.1 + + + + + + 2d + boxes + + 1.047 + + 800 + 600 + + + 0.1 + 10 + + + 1 + 30 + true + + + +``` +Let’s take a closer look at the portion of the code above that focuses on the bounding box camera sensor: + +```xml + + 2d + boxes + + 1.047 + + 800 + 600 + + + 0.1 + 10 + + + 1 + 30 + true + +``` + +As we can see, we define a sensor with the following SDF elements: +* ``: The camera, which has the following child elements: + * ``: The horizontal field of view, in radians. + * ``: The image size, in pixels. + * ``: The near and far clip planes. Objects are only rendered if they're within these planes. +* ``: Whether the sensor will always be updated (indicated by 1) or not (indicated by 0). This is currently unused by Ignition Gazebo. +* ``: The sensor's update rate, in Hz. +* ``: Whether the sensor should be visualized in the GUI (indicated by true) or not (indicated by false). This is currently unused by Ignition Gazebo. +* ``: The name of the topic where sensor data is published. + +
+There's also an optional plugin used here that allows for further configuration of the bounding box camera. Here's a description of the elements in this plugin (if the plugin isn't used, the default values mentioned below are used): + +``: The type of bounding boxes, boxes can be 2d or 3d, and 2d boxes can be visible 2d boxes or full 2d boxes + +Visible 2d box: 2D axis aligned box that shows the visible part of the occluded object +Full 2d box: 2D axis aligned box that shows the full box of occluded objects +3D box: Oriented 3D box defined by the center position / orientation / size +Default value for the `` is “Visible 2d box” +The `` values can be +- For visible 2d boxes: [“2d”, “visible_2d”, “visible_box_2d” ] +- For full 2d boxes: [“full_2d”, “visible_box_2d”] +- For 3d boxes: [“3d”] + + +## Assigning a label to a model +Only models with labels (annotated classes) will be visible by the bounding box camera sensor, and unlabeled models will be considered as a background + +To assign a label to a model we use the label plugin in the SDF file + +```xml + + 0 -1 0.5 0 0 0 + + + + 1 + 0 + 0 + 1 + 0 + 1 + + 1.0 + + + + + 1 1 1 + + + + + + + + 1 1 1 + + + + 0 0 0.5 1 + 0 0 1 1 + 0 0 0.3 1 + + + + + + + +``` + +Lets zoom in the label plugin + +```xml + + + +``` + +We assign the label of the model by adding that plugin to the `` tag of the model or the `` tag(will be shown below), And we add the `