From ee486121ff599a0c7077a1b195ef9731744a5917 Mon Sep 17 00:00:00 2001 From: AmrElsersy <35613645+AmrElsersy@users.noreply.github.com> Date: Fri, 17 Jun 2022 18:00:16 +0200 Subject: [PATCH] Add BoundingBox Sensor (#136) Signed-off-by: AmrElsersy Signed-off-by: Ashton Larkin Signed-off-by: Louise Poubel Co-authored-by: Ashton Larkin <42042756+adlarkin@users.noreply.github.com> Co-authored-by: Ashton Larkin Co-authored-by: Louise Poubel --- CMakeLists.txt | 2 +- .../sensors/BoundingBoxCameraSensor.hh | 117 +++ src/BoundingBoxCameraSensor.cc | 676 ++++++++++++++++++ src/CMakeLists.txt | 9 + test/integration/CMakeLists.txt | 2 + test/integration/boundingbox_camera.cc | 494 +++++++++++++ .../boundingbox_3d_camera_sensor_builtin.sdf | 22 + .../sdf/boundingbox_camera_sensor_builtin.sdf | 21 + tutorials/04_boundingbox_camera.md | 577 +++++++++++++++ .../boundingbox_camera_full.png | Bin 0 -> 93533 bytes .../boundingbox_camera_visible.png | Bin 0 -> 95656 bytes .../object_detection_dataset.gif | Bin 0 -> 816718 bytes .../files/boundingbox_camera/visualize_2d.png | Bin 0 -> 15970 bytes .../files/boundingbox_camera/visualize_3d.png | Bin 0 -> 31149 bytes 14 files changed, 1919 insertions(+), 1 deletion(-) create mode 100644 include/ignition/sensors/BoundingBoxCameraSensor.hh create mode 100644 src/BoundingBoxCameraSensor.cc create mode 100644 test/integration/boundingbox_camera.cc create mode 100644 test/sdf/boundingbox_3d_camera_sensor_builtin.sdf create mode 100644 test/sdf/boundingbox_camera_sensor_builtin.sdf 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 create mode 100644 tutorials/files/boundingbox_camera/object_detection_dataset.gif create mode 100644 tutorials/files/boundingbox_camera/visualize_2d.png create mode 100644 tutorials/files/boundingbox_camera/visualize_3d.png diff --git a/CMakeLists.txt b/CMakeLists.txt index 955c8299..f8caeb41 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -66,7 +66,7 @@ if ((NOT ${CMAKE_VERSION} VERSION_LESS 3.11) AND (NOT OpenGL_GL_PREFERENCE)) set(OpenGL_GL_PREFERENCE "GLVND") endif() -ign_find_package(ignition-rendering6 REQUIRED VERSION 6.2 OPTIONAL_COMPONENTS ogre ogre2) +ign_find_package(ignition-rendering6 REQUIRED VERSION 6.5 OPTIONAL_COMPONENTS ogre ogre2) set(IGN_RENDERING_VER ${ignition-rendering6_VERSION_MAJOR}) if (TARGET ignition-rendering${IGN_RENDERING_VER}::ogre) diff --git a/include/ignition/sensors/BoundingBoxCameraSensor.hh b/include/ignition/sensors/BoundingBoxCameraSensor.hh new file mode 100644 index 00000000..9dc77e18 --- /dev/null +++ b/include/ignition/sensors/BoundingBoxCameraSensor.hh @@ -0,0 +1,117 @@ +/* + * 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_BOUNDINGBOXCAMERASENSOR_HH_ +#define IGNITION_SENSORS_BOUNDINGBOXCAMERASENSOR_HH_ + +#include +#include + +#include +#include + +#include "ignition/sensors/CameraSensor.hh" +#include "ignition/sensors/boundingbox_camera/Export.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 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 + /// called with image data. + class IGNITION_SENSORS_BOUNDINGBOX_CAMERA_VISIBLE + 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 successful + 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() 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); + + /// \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 Check if there are any subscribers + /// \return True if there are subscribers, false otherwise + /// \todo(iche033) Make this function virtual on Garden + public: bool HasConnections() const; + + /// \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..adafd9da --- /dev/null +++ b/src/BoundingBoxCameraSensor.cc @@ -0,0 +1,676 @@ +/* + * 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 +#include + +#include "ignition/sensors/BoundingBoxCameraSensor.hh" +#include "ignition/sensors/RenderingEvents.hh" +#include "ignition/sensors/SensorFactory.hh" + +using namespace ignition; +using namespace sensors; + +class ignition::sensors::BoundingBoxCameraSensorPrivate +{ + /// \brief Save an image of rgb camera + public: void SaveImage(); + + /// \brief Save the bounding boxes + public: void SaveBoxes(); + + /// \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 rgbCamera{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 Vector to receive boxes from the rendering camera + public: std::vector boundingBoxes; + + /// \brief RGB Image to draw boxes on it + public: rendering::Image image; + + /// \brief Buffer contains the image data to be saved + public: unsigned char *saveImageBuffer{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: rendering::BoundingBoxType type + {rendering::BoundingBoxType::BBT_VISIBLEBOX2D}; + + /// \brief True to save images & boxes + public: bool saveSample{false}; + + /// \brief path directory to where images & boxes are saved + public: std::string savePath{"./"}; + + /// \brief Folder to save the image + public: std::string saveImageFolder{"/images"}; + + /// \brief Folder to save the bounding boxes + public: std::string saveBoxesFolder{"/boxes"}; + + /// \brief counter used to set the sample filename + public: std::uint64_t saveCounter{0}; +}; + +////////////////////////////////////////////////// +BoundingBoxCameraSensor::BoundingBoxCameraSensor() + : CameraSensor(), dataPtr(std::make_unique()) +{ +} + +///////////////////////////////////////////////// +BoundingBoxCameraSensor::~BoundingBoxCameraSensor() +{ +} + +///////////////////////////////////////////////// +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); + + auto sdfCamera = _sdf.CameraSensor(); + if (!sdfCamera) + return false; + + // BoundingBox Type + if (sdfCamera->HasBoundingBoxType()) + { + std::string type = sdfCamera->BoundingBoxType(); + + if (type == "full_2d" || type == "full_box_2d") + this->dataPtr->type = rendering::BoundingBoxType::BBT_FULLBOX2D; + else if (type == "2d" || type == "visible_2d" + || type == "visible_box_2d") + this->dataPtr->type = rendering::BoundingBoxType::BBT_VISIBLEBOX2D; + else if (type == "3d") + this->dataPtr->type = rendering::BoundingBoxType::BBT_BOX3D; + else + { + ignerr << "Unknown bounding box type " << type << std::endl; + return false; + } + } + + 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; + + auto topicBoundingBoxes = this->Topic(); + auto topicImage = this->Topic() + "_image"; + + this->dataPtr->imagePublisher = + this->dataPtr->node.Advertise(topicImage); + + if (!this->dataPtr->imagePublisher) + { + ignerr << "Unable to create publisher on topic [" + << topicImage << "].\n"; + return false; + } + + igndbg << "Camera images for [" << this->Name() << "] advertised on [" + << topicImage << "]" << std::endl; + + if (this->dataPtr->type == rendering::BoundingBoxType::BBT_BOX3D) + { + this->dataPtr->boxesPublisher = this->dataPtr->node.Advertise< + msgs::AnnotatedOriented3DBox_V>(topicBoundingBoxes); + } + else + { + this->dataPtr->boxesPublisher = this->dataPtr->node.Advertise< + msgs::AnnotatedAxisAligned2DBox_V>(topicBoundingBoxes); + } + + if (!this->dataPtr->boxesPublisher) + { + ignerr << "Unable to create publisher on topic [" + << topicBoundingBoxes << "].\n"; + return false; + } + + igndbg << "Bounding boxes for [" << this->Name() << "] advertised on [" + << topicBoundingBoxes << std::endl; + + 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( + 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->rgbCamera = 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->rgbCamera) + { + // Create rendering camera + this->dataPtr->boundingboxCamera = + this->Scene()->CreateBoundingBoxCamera(this->Name()); + + this->dataPtr->rgbCamera = this->Scene()->CreateCamera( + this->Name() + "_rgbCamera"); + } + + auto width = sdfCamera->ImageWidth(); + auto height = sdfCamera->ImageHeight(); + + // Set Camera Properties + 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()); + 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->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( + rendering::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->rgbCamera); + this->Scene()->RootVisual()->AddChild(this->dataPtr->boundingboxCamera); + + // Add the rendering sensors to handle its render + this->AddSensor(this->dataPtr->boundingboxCamera); + this->AddSensor(this->dataPtr->rgbCamera); + + // Create the directory to store frames + if (sdfCamera->SaveFrames()) + { + this->dataPtr->savePath = sdfCamera->SaveFramesPath(); + this->dataPtr->saveImageFolder = + this->dataPtr->savePath + this->dataPtr->saveImageFolder; + this->dataPtr->saveBoxesFolder = + this->dataPtr->savePath + this->dataPtr->saveBoxesFolder; + this->dataPtr->saveSample = true; + + // Set the save counter to be equal number of images in the folder + 1 + // to continue adding to the images in the folder (multi scene datasets) + if (common::isDirectory(this->dataPtr->saveImageFolder)) + { + common::DirIter endIter; + for (common::DirIter dirIter(this->dataPtr->saveImageFolder); + dirIter != endIter; ++dirIter) + { + this->dataPtr->saveCounter++; + } + } + } + + // 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->rgbCamera->CreateImage(); + + return true; +} + +///////////////////////////////////////////////// +rendering::BoundingBoxCameraPtr + BoundingBoxCameraSensor::BoundingBoxCamera() const +{ + return this->dataPtr->boundingboxCamera; +} + +///////////////////////////////////////////////// +void BoundingBoxCameraSensor::OnNewBoundingBoxes( + const std::vector &_boxes) +{ + std::lock_guard lock(this->dataPtr->mutex); + this->dataPtr->boundingBoxes.clear(); + for (const 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->rgbCamera) + { + ignerr << "Camera doesn't exist.\n"; + return false; + } + + // don't render if there are no subscribers nor saving + if (!this->dataPtr->imagePublisher.HasConnections() && + !this->dataPtr->boxesPublisher.HasConnections() && + !this->dataPtr->saveSample) + { + return false; + } + + // 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->rgbCamera->SetWorldPose( + this->dataPtr->boundingboxCamera->WorldPose()); + + // Render the bounding box camera + this->Render(); + + // Render the rgb camera + this->dataPtr->rgbCamera->Copy(this->dataPtr->image); + + auto imageBuffer = this->dataPtr->image.Data(); + + if (this->dataPtr->saveSample) + { + auto bufferSize = this->dataPtr->image.MemorySize(); + if (!this->dataPtr->saveImageBuffer) + this->dataPtr->saveImageBuffer = new uint8_t[bufferSize]; + + memcpy(this->dataPtr->saveImageBuffer, imageBuffer, + bufferSize); + } + + // Draw bounding boxes + for (const auto &box : this->dataPtr->boundingBoxes) + { + this->dataPtr->boundingboxCamera->DrawBoundingBox( + imageBuffer, math::Color::Green, box); + } + + auto width = this->dataPtr->rgbCamera->ImageWidth(); + auto height = this->dataPtr->rgbCamera->ImageHeight(); + + // Create Image message + msgs::Image imageMsg; + imageMsg.set_width(width); + imageMsg.set_height(height); + // Format + imageMsg.set_step( + width * rendering::PixelUtil::BytesPerPixel(rendering::PF_R8G8B8)); + imageMsg.set_pixel_format_type( + msgs::PixelFormatType::RGB_INT8); + // Time stamp + auto stamp = imageMsg.mutable_header()->mutable_stamp(); + *stamp = msgs::Convert(_now); + auto frame = imageMsg.mutable_header()->add_data(); + frame->set_key("frame_id"); + frame->add_value(this->Name()); + // Image data + imageMsg.set_data(imageBuffer, + rendering::PixelUtil::MemorySize(rendering::PF_R8G8B8, + width, height)); + + // Publish + this->AddSequence(imageMsg.mutable_header(), "rgbImage"); + this->dataPtr->imagePublisher.Publish(imageMsg); + + msgs::AnnotatedAxisAligned2DBox_V boxes2DMsg; + msgs::AnnotatedOriented3DBox_V boxes3DMsg; + + if (this->dataPtr->type == rendering::BoundingBoxType::BBT_BOX3D) + { + // Create 3D boxes message + for (const auto &box : this->dataPtr->boundingBoxes) + { + // box data + auto annotatedBox = boxes3DMsg.add_annotated_box(); + annotatedBox->set_label(box.Label()); + + auto oriented3DBox = annotatedBox->mutable_box(); + msgs::Set(oriented3DBox->mutable_center(), box.Center()); + msgs::Set(oriented3DBox->mutable_boxsize(), box.Size()); + msgs::Set(oriented3DBox->mutable_orientation(), box.Orientation()); + } + // time stamp + auto stampBoxes = + boxes3DMsg.mutable_header()->mutable_stamp(); + *stampBoxes = msgs::Convert(_now); + auto frameBoxes = boxes3DMsg.mutable_header()->add_data(); + frameBoxes->set_key("frame_id"); + frameBoxes->add_value(this->Name()); + } + else + { + // Create 2D boxes message + for (const auto &box : this->dataPtr->boundingBoxes) + { + // box data + auto annotatedBox = boxes2DMsg.add_annotated_box(); + annotatedBox->set_label(box.Label()); + + auto minCorner = box.Center() - box.Size() * 0.5; + auto maxCorner = box.Center() + box.Size() * 0.5; + + auto axisAlignedBox = annotatedBox->mutable_box(); + msgs::Set(axisAlignedBox->mutable_min_corner(), + {minCorner.X(), minCorner.Y()}); + msgs::Set(axisAlignedBox->mutable_max_corner(), + {maxCorner.X(), maxCorner.Y()}); + } + // time stamp + auto stampBoxes = boxes2DMsg.mutable_header()->mutable_stamp(); + *stampBoxes = msgs::Convert(_now); + auto frameBoxes = 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); + if (this->dataPtr->type == rendering::BoundingBoxType::BBT_BOX3D) + { + this->AddSequence(boxes3DMsg.mutable_header(), "boundingboxes"); + this->dataPtr->boxesPublisher.Publish(boxes3DMsg); + } + else + { + this->AddSequence(boxes2DMsg.mutable_header(), "boundingboxes"); + this->dataPtr->boxesPublisher.Publish(boxes2DMsg); + } + + // Save a sample (image & its bounding boxes) + if (this->dataPtr->saveSample) + { + this->dataPtr->SaveImage(); + this->dataPtr->SaveBoxes(); + ++this->dataPtr->saveCounter; + } + + return true; +} + +///////////////////////////////////////////////// +unsigned int BoundingBoxCameraSensor::ImageHeight() const +{ + return this->dataPtr->rgbCamera->ImageHeight(); +} + +///////////////////////////////////////////////// +unsigned int BoundingBoxCameraSensor::ImageWidth() const +{ + return this->dataPtr->rgbCamera->ImageWidth(); +} + +////////////////////////////////////////////////// +void BoundingBoxCameraSensorPrivate::SaveImage() +{ + // Attempt to create the save directory if it doesn't exist + if (!common::isDirectory(this->savePath)) + { + if (!common::createDirectories(this->savePath)) + { + ignerr << "Failed to create directory [" << this->savePath << "]" + << std::endl; + return; + } + } + // Attempt to create the image directory if it doesn't exist + if (!common::isDirectory(this->saveImageFolder)) + { + if (!common::createDirectories(this->saveImageFolder)) + { + ignerr << "Failed to create directory [" << this->saveImageFolder << "]" + << std::endl; + return; + } + } + + auto width = this->rgbCamera->ImageWidth(); + auto height = this->rgbCamera->ImageHeight(); + if (width == 0 || height == 0) + return; + + common::Image localImage; + + // Save the images in format of 0000001, 0000002 .. etc + // Useful in sorting them in python + std::stringstream ss; + ss << std::setw(7) << std::setfill('0') << this->saveCounter; + std::string saveCounterString = ss.str(); + + std::string filename = "image_" + saveCounterString + ".png"; + + localImage.SetFromData(this->saveImageBuffer, width, height, + common::Image::RGB_INT8); + localImage.SavePNG( + common::joinPaths(this->saveImageFolder, filename)); +} + +////////////////////////////////////////////////// +void BoundingBoxCameraSensorPrivate::SaveBoxes() +{ + // Attempt to create the save directory if it doesn't exist + if (!common::isDirectory(this->savePath)) + { + if (!common::createDirectories(this->savePath)) + { + ignerr << "Failed to create directory [" << this->savePath << "]" + << std::endl; + return; + } + } + // Attempt to create the boxes directory if it doesn't exist + if (!common::isDirectory(this->saveBoxesFolder)) + { + if (!common::createDirectories(this->saveBoxesFolder)) + { + ignerr << "Failed to create directory [" << this->saveBoxesFolder << "]" + << std::endl; + return; + } + } + + // Save the images in format of 0000001, 0000002 .. etc + // Useful in sorting them in python + std::stringstream ss; + ss << std::setw(7) << std::setfill('0') << this->saveCounter; + std::string saveCounterString = ss.str(); + + std::string filename = this->saveBoxesFolder + "/boxes_" + + saveCounterString + ".csv"; + std::ofstream file(filename); + + if (this->type == rendering::BoundingBoxType::BBT_BOX3D) + { + file << "label,x,y,z,w,h,l,roll,pitch,yaw\n"; + for (const auto &box : this->boundingBoxes) + { + auto label = std::to_string(box.Label()); + + auto x = std::to_string(box.Center().X()); + auto y = std::to_string(box.Center().Y()); + auto z = std::to_string(box.Center().Z()); + + auto w = std::to_string(box.Size().X()); + auto h = std::to_string(box.Size().Y()); + auto l = std::to_string(box.Size().Z()); + + auto roll = std::to_string(box.Orientation().Roll()); + auto pitch = std::to_string(box.Orientation().Pitch()); + auto yaw = std::to_string(box.Orientation().Yaw()); + + // label x y z w h l roll pitch yaw + std::string sep = ","; + std::string boxString = label + sep + x + sep + y + sep + z + sep + + w + sep + h + sep + l + sep + roll + sep + pitch + sep + yaw; + + file << boxString + '\n'; + } + } + else + { + file << "label,x_center,y_center,width,height\n"; + for (const auto &box : this->boundingBoxes) + { + auto label = std::to_string(box.Label()); + + auto x = std::to_string(box.Center().X()); + auto y = std::to_string(box.Center().Y()); + auto width = std::to_string(box.Size().X()); + auto height = std::to_string(box.Size().Y()); + + // label x y width height + std::string sep = ","; + std::string boxString = label + sep + + x + sep + y + sep + width + sep + height; + + file << boxString + '\n'; + } + } + file.close(); +} + +////////////////////////////////////////////////// +bool BoundingBoxCameraSensor::HasConnections() const +{ + return (this->dataPtr->imagePublisher && + this->dataPtr->imagePublisher.HasConnections()) || + (this->dataPtr->boxesPublisher && + this->dataPtr->boxesPublisher.HasConnections()); +} diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index 76a20b9e..bac9daa6 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -147,6 +147,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} +) + set(segmentation_camera_sources SegmentationCameraSensor.cc) ign_add_component(segmentation_camera SOURCES ${segmentation_camera_sources} GET_TARGET_NAME segmentation_camera_target) target_compile_definitions(${segmentation_camera_target} PUBLIC SegmentationCameraSensor_EXPORTS) diff --git a/test/integration/CMakeLists.txt b/test/integration/CMakeLists.txt index 1c833c56..398c3b34 100644 --- a/test/integration/CMakeLists.txt +++ b/test/integration/CMakeLists.txt @@ -1,6 +1,7 @@ set(TEST_TYPE "INTEGRATION") set(dri_tests + boundingbox_camera.cc camera.cc depth_camera.cc distortion_camera.cc @@ -38,6 +39,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 diff --git a/test/integration/boundingbox_camera.cc b/test/integration/boundingbox_camera.cc new file mode 100644 index 00000000..badb7610 --- /dev/null +++ b/test/integration/boundingbox_camera.cc @@ -0,0 +1,494 @@ +/* + * 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 + +#include "test_config.h" // NOLINT(build/include) +#include "TransportTestTools.hh" + +using namespace ignition; + +class BoundingBoxCameraSensorTest: public testing::Test, + public testing::WithParamInterface +{ + // Create a BoundingBox Camera sensor from a SDF and gets a boxes message + public: void BoxesWithBuiltinSDF(const std::string &_renderEngine); + + // Create a BoundingBox Camera 3D sensor from a SDF and gets a boxes message + 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) + { + rendering::BoundingBox box; + 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.SetSize({max_corner.x() - min_corner.x(), + max_corner.y() - min_corner.y(), + 0}); + + box.SetCenter({min_corner.x() + box.Size().X() / 2, + min_corner.y() + box.Size().Y() / 2, + 0}); + + box.SetOrientation({0, 0, 0}); + box.SetLabel(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) + { + rendering::BoundingBox box; + auto annotated_box = _boxes.annotated_box(i); + auto orientedBox = annotated_box.box(); + // center + box.SetCenter({orientedBox.center().x(), orientedBox.center().y(), + orientedBox.center().z()}); + // orientation + auto orientation = orientedBox.orientation(); + box.SetOrientation({orientation.w(), orientation.x(), + orientation.y(), orientation.z()}); + // size + auto boxSize = orientedBox.boxsize(); + box.SetSize({boxSize.x(), boxSize.y(), boxSize.z()}); + + box.SetLabel(annotated_box.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 a scene with 3 overlapping boxes - one box will be fully +/// visible, another box will be partially visible, and the last box won't be +/// visible (it's occluded by the other 2 boxes) +void BuildScene2d(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); +} + +/// \brief Build a scene with 3d oriented 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) +{ + std::string path = common::joinPaths(PROJECT_SOURCE_PATH, "test", + "sdf", "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); + + // Skip unsupported engines + if (_renderEngine != "ogre2") + { + igndbg << "Engine '" << _renderEngine + << "' doesn't support bounding box cameras" << std::endl; + return; + } + + // Setup ign-rendering with an empty scene + auto *engine = rendering::engine(_renderEngine); + if (!engine) + { + igndbg << "Engine '" << _renderEngine + << "' is not supported" << std::endl; + return; + } + + rendering::ScenePtr scene = engine->CreateScene("scene"); + ASSERT_NE(nullptr, scene); + BuildScene2d(scene); + + sensors::Manager mgr; + + sdf::Sensor sdfSensor; + sdfSensor.Load(sensorPtr); + + std::string type = sdfSensor.TypeStr(); + EXPECT_EQ(type, "boundingbox_camera"); + + auto *sensor = + mgr.CreateSensor(sdfSensor); + + ASSERT_NE(sensor, nullptr); + sensor->SetScene(scene); + + EXPECT_EQ(width, sensor->ImageWidth()); + EXPECT_EQ(height, sensor->ImageHeight()); + + auto camera = sensor->BoundingBoxCamera(); + ASSERT_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(rendering::BoundingBoxType::BBT_VISIBLEBOX2D); + + EXPECT_EQ(camera->Type(), rendering::BoundingBoxType::BBT_VISIBLEBOX2D); + EXPECT_EQ(camera->ImageWidth(), width); + EXPECT_EQ(camera->ImageHeight(), height); + + // Get the Msg + std::string topic = + "/test/integration/BoundingBoxCameraPlugin_boxesWithBuiltinSDF"; + WaitForMessageTestHelper< + 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 + 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 box + WaitForNewFrame(); + + // accepted error with +/- in pixels in comparing the box coordinates + double marginError = 2.0; + + // Visible box test + g_mutex.lock(); + g_counter = 0; + + // check that the invisible 3rd box does not exist + EXPECT_EQ(g_boxes.size(), size_t(2)); + + rendering::BoundingBox occludedBox = g_boxes[0]; + rendering::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, marginError); + EXPECT_NEAR(occludedBox.Center().Y(), 119, marginError); + EXPECT_NEAR(occludedBox.Size().X(), 15, marginError); + EXPECT_NEAR(occludedBox.Size().Y(), 45, marginError); + EXPECT_EQ(occludedBox.Label(), occludedLabel); + + EXPECT_NEAR(frontBox.Center().X(), 159, marginError); + EXPECT_NEAR(frontBox.Center().Y(), 119, marginError); + EXPECT_NEAR(frontBox.Size().X(), 105, marginError); + EXPECT_NEAR(frontBox.Size().Y(), 105, marginError); + EXPECT_EQ(frontBox.Label(), frontLabel); + + g_mutex.unlock(); + + // Full Boxes Type Test + camera->SetBoundingBoxType(rendering::BoundingBoxType::BBT_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)); + rendering::BoundingBox occludedFullBox = g_boxes[0]; + rendering::BoundingBox frontFullBox = g_boxes[1]; + + // coordinates of partially occluded object is bigger + EXPECT_NEAR(occludedFullBox.Center().X(), 116, marginError); + EXPECT_NEAR(occludedFullBox.Center().Y(), 119, marginError); + EXPECT_NEAR(occludedFullBox.Size().X(), 51, marginError); + EXPECT_NEAR(occludedFullBox.Size().Y(), 45, marginError); + EXPECT_EQ(occludedFullBox.Label(), occludedLabel); + + EXPECT_NEAR(frontFullBox.Center().X(), 159, marginError); + EXPECT_NEAR(frontFullBox.Center().Y(), 119, marginError); + EXPECT_NEAR(frontFullBox.Size().X(), 108, marginError); + EXPECT_NEAR(frontFullBox.Size().Y(), 108, marginError); + EXPECT_EQ(frontFullBox.Label(), frontLabel); + + g_mutex.unlock(); + + // Clean up + engine->DestroyScene(scene); + rendering::unloadEngine(engine->Name()); +} + +///////////////////////////////////////////////// +void BoundingBoxCameraSensorTest::Boxes3DWithBuiltinSDF( + const std::string &_renderEngine) +{ + std::string path = common::joinPaths(PROJECT_SOURCE_PATH, "test", + "sdf", "boundingbox_3d_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); + + // Skip unsupported engines + if (_renderEngine != "ogre2") + { + igndbg << "Engine '" << _renderEngine + << "' doesn't support bounding box cameras" << std::endl; + return; + } + + // Setup ign-rendering with an empty scene + auto *engine = rendering::engine(_renderEngine); + if (!engine) + { + igndbg << "Engine '" << _renderEngine + << "' is not supported" << std::endl; + return; + } + + rendering::ScenePtr scene = engine->CreateScene("scene"); + ASSERT_NE(nullptr, scene); + BuildScene3D(scene); + + sensors::Manager mgr; + + sdf::Sensor sdfSensor; + sdfSensor.Load(sensorPtr); + + std::string type = sdfSensor.TypeStr(); + EXPECT_EQ(type, "boundingbox_camera"); + + auto *sensor = + mgr.CreateSensor(sdfSensor); + + ASSERT_NE(sensor, nullptr); + sensor->SetScene(scene); + + EXPECT_EQ(width, sensor->ImageWidth()); + EXPECT_EQ(height, sensor->ImageHeight()); + + auto camera = sensor->BoundingBoxCamera(); + ASSERT_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(rendering::BoundingBoxType::BBT_BOX3D); + + // Get the Msg + std::string topic = + "/test/integration/BoundingBox3DCameraPlugin_boxesWithBuiltinSDF"; + WaitForMessageTestHelper< + msgs::AnnotatedOriented3DBox_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 + transport::Node node; + node.Subscribe(topic, &OnNew3DBoundingBoxes); + + // wait for a few BoundingBox camera boxes + mgr.RunOnce(std::chrono::steady_clock::duration::zero(), true); + // wait for a new boxes + WaitForNewFrame(); + + g_mutex.lock(); + g_counter = 0; + + EXPECT_EQ(g_boxes.size(), size_t(1)); + rendering::BoundingBox box3D = g_boxes[0]; + + double marginError = 0.01; + EXPECT_NEAR(box3D.Center().X(), 0, marginError); + EXPECT_NEAR(box3D.Center().Y(), 0, marginError); + EXPECT_NEAR(box3D.Center().Z(), -2, marginError); + + EXPECT_NEAR(box3D.Size().X(), 1, marginError); + EXPECT_NEAR(box3D.Size().Y(), 1, marginError); + EXPECT_NEAR(box3D.Size().Z(), 1, marginError); + + EXPECT_NEAR(box3D.Orientation().X(), 0.322329, marginError); + EXPECT_NEAR(box3D.Orientation().Y(), -0.886405, marginError); + EXPECT_NEAR(box3D.Orientation().Z(), -0.0805823, marginError); + EXPECT_NEAR(box3D.Orientation().W(), -0.322329, marginError); + + g_mutex.unlock(); + + // Clean up + engine->DestroyScene(scene); + rendering::unloadEngine(engine->Name()); +} + +////////////////////////////////////////////////// +TEST_P(BoundingBoxCameraSensorTest, BoxesWithBuiltinSDF) +{ + BoxesWithBuiltinSDF(GetParam()); +} + +////////////////////////////////////////////////// +TEST_P(BoundingBoxCameraSensorTest, Boxes3DWithBuiltinSDF) +{ + Boxes3DWithBuiltinSDF(GetParam()); +} + +INSTANTIATE_TEST_CASE_P(BoundingBoxCameraSensor, BoundingBoxCameraSensorTest, + RENDER_ENGINE_VALUES, rendering::PrintToStringParam()); + +////////////////////////////////////////////////// +int main(int argc, char **argv) +{ + common::Console::SetVerbosity(4); + ::testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/test/sdf/boundingbox_3d_camera_sensor_builtin.sdf b/test/sdf/boundingbox_3d_camera_sensor_builtin.sdf new file mode 100644 index 00000000..06cf8206 --- /dev/null +++ b/test/sdf/boundingbox_3d_camera_sensor_builtin.sdf @@ -0,0 +1,22 @@ + + + + + + 10 + /test/integration/BoundingBox3DCameraPlugin_boxesWithBuiltinSDF + + 3d + + 320 + 240 + + + 0.1 + 1000.0 + + + + + + \ No newline at end of file diff --git a/test/sdf/boundingbox_camera_sensor_builtin.sdf b/test/sdf/boundingbox_camera_sensor_builtin.sdf new file mode 100644 index 00000000..b133767d --- /dev/null +++ b/test/sdf/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 diff --git a/tutorials/04_boundingbox_camera.md b/tutorials/04_boundingbox_camera.md new file mode 100644 index 00000000..8730414a --- /dev/null +++ b/tutorials/04_boundingbox_camera.md @@ -0,0 +1,577 @@ +# Bounding Box Camera in Gazebo Sim +In this tutorial, we will discuss how to use a bounding box camera sensor in Gazebo Sim. + +## Requirements + +Since this tutorial will show how to use a bounding box camera sensor in Gazebo Sim, you'll need to have Gazebo Sim installed. We recommend installing all Gazebo libraries, using version Fortress or newer (the bounding box camera is not available in Gazebo versions prior to Fortress). +If you need to install Gazebo, 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 + + + + + + boxes + + 2d + 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 + + boxes + + 2d + 1.047 + + 800 + 600 + + + 0.1 + 10 + + + 1 + 30 + true + +``` + +As we can see, we define a sensor with the following SDF elements: +* ``: 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 + The default value 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` +* ``: 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 Gazebo Sim. +* ``: 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 Gazebo Sim. +* ``: The name of the topic where sensor data is published. + +
+ + +## 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 `