From 1c5eec6796e8015eaf7de298742d5501b5e39011 Mon Sep 17 00:00:00 2001 From: AmrElsersy Date: Sat, 12 Jun 2021 15:06:09 +0200 Subject: [PATCH 01/22] Segmentation Camera Sensor Signed-off-by: AmrElsersy --- .../sensors/SegmentationCameraSensor.hh | 129 +++++++ src/CMakeLists.txt | 9 + src/SegmentationCameraSensor.cc | 362 ++++++++++++++++++ 3 files changed, 500 insertions(+) create mode 100644 include/ignition/sensors/SegmentationCameraSensor.hh create mode 100644 src/SegmentationCameraSensor.cc diff --git a/include/ignition/sensors/SegmentationCameraSensor.hh b/include/ignition/sensors/SegmentationCameraSensor.hh new file mode 100644 index 00000000..0a8dc948 --- /dev/null +++ b/include/ignition/sensors/SegmentationCameraSensor.hh @@ -0,0 +1,129 @@ +/* + * 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_SEGMENTATIONCAMERASENSOR_HH_ +#define IGNITION_SENSORS_SEGMENTATIONCAMERASENSOR_HH_ + +#include +#include +#include + +#include +#include +#include +#include + +#include "ignition/msgs.hh" +#include "ignition/transport/Node.hh" +#include "ignition/transport/Publisher.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 SegmentationCameraSensorPrivate; + + /// \brief Segmentation camera sensor class. + /// + /// This class creates Segmentation 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 SegmentationCameraSensor : public CameraSensor + { + /// \brief constructor + public: SegmentationCameraSensor(); + + /// \brief destructor + public: virtual ~SegmentationCameraSensor(); + + /// \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 IGN_DEPRECATED(4) Update( + const ignition::common::Time &_now) 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 Force the sensor to generate data + /// \param[in] _now The current time + /// \return true if the update was successfull + public: virtual rendering::SegmentationCameraPtr SegmentationCamera(); + + /// \brief Segmentation data callback used to get the data from the sensor + /// \param[in] _scan pointer to the data from the sensor + /// \param[in] _width width of the Segmentation image + /// \param[in] _height height of the Segmentation image + /// \param[in] _channel bytes used for the Segmentation data + /// \param[in] _format string with the format + public: void OnNewSegmentationFrame(const uint8_t *, unsigned int, + unsigned int, unsigned int, const std::string &); + + /// \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/CMakeLists.txt b/src/CMakeLists.txt index 1afdb7c0..2dd9919b 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(segmentation_camera_sources SegmentationCameraSensor.cc) +ign_add_component(segmentation_camera SOURCES ${segmentation_camera_sources} GET_TARGET_NAME segmentation_camera_target) +target_link_libraries(${segmentation_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/src/SegmentationCameraSensor.cc b/src/SegmentationCameraSensor.cc new file mode 100644 index 00000000..dc11d5ee --- /dev/null +++ b/src/SegmentationCameraSensor.cc @@ -0,0 +1,362 @@ +/* + * 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/sensors/RenderingEvents.hh" +#include "ignition/sensors/SensorFactory.hh" + +#include "ignition/sensors/SegmentationCameraSensor.hh" +#include "ignition/rendering/SegmentationCamera.hh" + +#include "ignition/transport/Node.hh" +#include "ignition/transport/Publisher.hh" +#include "ignition/msgs.hh" + + +using namespace ignition; +using namespace sensors; + +class ignition::sensors::SegmentationCameraSensorPrivate +{ + /// \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 Segmentation Camera + public: rendering::SegmentationCameraPtr camera {nullptr}; + + /// \brief node to create publisher + public: transport::Node node; + + /// \brief publisher to publish segmentation image + public: transport::Node::Publisher publisher; + + /// \brief Segmentation Image Msg + public: msgs::Image segmentationMsg; + + /// \brief Topic to publish the segmentation image + public: std::string topicSegmentation = ""; + + /// \brief Buffer contains the segmentation map data + public: uint8_t *segmentationBuffer {nullptr}; + + /// \brief Segmentation type (Semantic / Instance) + public: SegmentationType type {SegmentationType::Semantic}; + + /// \brief True if camera generates a colored map image + /// False if camera generates labels ids image + public: bool isColoredMap {true}; + + /// \brief Connection to the new segmentation frames data + public: common::ConnectionPtr newSegmentationConnection; + + /// \brief Connection to the Manager's scene change event. + public: common::ConnectionPtr sceneChangeConnection; + + /// \brief Just a mutex for thread safety + public: std::mutex mutex; +}; + +////////////////////////////////////////////////// +SegmentationCameraSensor::SegmentationCameraSensor() + : CameraSensor(), dataPtr(new SegmentationCameraSensorPrivate) +{ +} + +///////////////////////////////////////////////// +SegmentationCameraSensor::~SegmentationCameraSensor() +{ + if (this->dataPtr->segmentationBuffer) + delete this->dataPtr->segmentationBuffer; +} + +///////////////////////////////////////////////// +bool SegmentationCameraSensor::Init() +{ + return CameraSensor::Init(); +} + +///////////////////////////////////////////////// +bool SegmentationCameraSensor::Load(sdf::ElementPtr _sdf) +{ + sdf::Sensor sdfSensor; + sdfSensor.Load(_sdf); + return this->Load(sdfSensor); +} + +///////////////////////////////////////////////// +bool SegmentationCameraSensor::Load(const sdf::Sensor &_sdf) +{ + std::lock_guard lock(this->dataPtr->mutex); + + // Segmentation Type & Colored Properties + sdf::ElementPtr sdfElement = _sdf.Element(); + if (sdfElement->HasElement("segmentation_type")) + { + std::string type = sdfElement->Get("segmentation_type"); + + // convert type to lowercase + std::for_each(type.begin(), type.end(), [](char & c){ + c = std::tolower(c); + }); + + if (type == "semantic") + this->dataPtr->type = SegmentationType::Semantic; + else if (type == "instance" || type == "panoptic") + this->dataPtr->type = SegmentationType::Panoptic; + } + + if (sdfElement->HasElement("colored")) + { + this->dataPtr->isColoredMap = sdfElement->Get("colored"); + } + + if (!Sensor::Load(_sdf)) + { + return false; + } + + // Check if this is the right type + if (_sdf.Type() != sdf::SensorType::SEGMENTATION_CAMERA) + { + ignerr << "Attempting to a load a Segmentation Camera sensor, but received " + << "a " << _sdf.TypeStr() << std::endl; + return false; + } + + if (_sdf.CameraSensor() == nullptr) + { + ignerr << "Attempting to a load a Segmentation Camera sensor, but received " + << "a null sensor." << std::endl; + return false; + } + + this->dataPtr->sdfSensor = _sdf; + + // Create the thermal image publisher + this->dataPtr->publisher = + this->dataPtr->node.Advertise( + this->Topic()); + + if (!this->dataPtr->publisher) + { + ignerr << "Unable to create publisher on topic[" + << this->Topic() << "].\n"; + return false; + } + + if (!this->AdvertiseInfo()) + return false; + + if (this->Scene()) + { + this->CreateCamera(); + } + + this->dataPtr->sceneChangeConnection = + RenderingEvents::ConnectSceneChangeCallback( + std::bind(&SegmentationCameraSensor::SetScene, this, + std::placeholders::_1)); + + this->dataPtr->initialized = true; + + return true; +} + +///////////////////////////////////////////////// +void SegmentationCameraSensor::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) + { + // TODO(anyone) Remove camera from scene + this->dataPtr->camera = nullptr; + RenderingSensor::SetScene(_scene); + + if (this->dataPtr->initialized) + this->CreateCamera(); + } +} + +///////////////////////////////////////////////// +bool SegmentationCameraSensor::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->camera = this->Scene()->CreateSegmentationCamera( + this->Name()); + } + + // Segmentation properties + this->dataPtr->camera->SetSegmentationType(this->dataPtr->type); + this->dataPtr->camera->EnableColoredMap(this->dataPtr->isColoredMap); + + auto width = sdfCamera->ImageWidth(); + auto height = sdfCamera->ImageHeight(); + + // Set Camera Properties + 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->SetHFOV(sdfCamera->HorizontalFov()); + math::Angle angle = sdfCamera->HorizontalFov(); + if (angle < 0.01 || angle > IGN_PI*2) + { + ignerr << "Invalid horizontal field of view [" << angle << "]\n"; + return false; + } + this->dataPtr->camera->SetAspectRatio(static_cast(width)/height); + this->dataPtr->camera->SetHFOV(angle); + + // Add the camera to the scene + // this->dataPtr->camera->SetLocalPose(sdfCamera->RawPose()); + this->Scene()->RootVisual()->AddChild(this->dataPtr->camera); + + // Add the rendering sensor to handle its render + this->AddSensor(this->dataPtr->camera); + + // Connection to receive the segmentation buffer + this->dataPtr->newSegmentationConnection = + this->dataPtr->camera->ConnectNewSegmentationFrame( + std::bind(&SegmentationCameraSensor::OnNewSegmentationFrame, this, + std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, + std::placeholders::_4, std::placeholders::_5)); + + return true; +} + +///////////////////////////////////////////////// +rendering::SegmentationCameraPtr SegmentationCameraSensor::SegmentationCamera() +{ + return this->dataPtr->camera; +} + +///////////////////////////////////////////////// +void SegmentationCameraSensor::OnNewSegmentationFrame(const uint8_t * _data, + unsigned int _width, unsigned int _height, unsigned int _channles, + const std::string &/*_format*/) +{ + std::lock_guard lock(this->dataPtr->mutex); + + unsigned int bufferSize = _width * _height * _channles; + + if (!this->dataPtr->segmentationBuffer) + this->dataPtr->segmentationBuffer = new uint8_t[bufferSize]; + + memcpy(this->dataPtr->segmentationBuffer, _data, bufferSize); +} + +////////////////////////////////////////////////// +bool SegmentationCameraSensor::Update(const ignition::common::Time &_now) +{ + return this->Update(math::secNsecToDuration(_now.sec, _now.nsec)); +} + +////////////////////////////////////////////////// +bool SegmentationCameraSensor::Update( + const std::chrono::steady_clock::duration &_now) +{ + IGN_PROFILE("SegmentationCameraSensor::Update"); + if (!this->dataPtr->initialized) + { + ignerr << "Not initialized, update ignored.\n"; + return false; + } + + if (!this->dataPtr->camera) + { + ignerr << "Camera doesn't exist.\n"; + return false; + } + + // don't render if there is no subscribers + if (!this->dataPtr->publisher.HasConnections()) + return false; + + // Actual render + this->Render(); + + if (!this->dataPtr->segmentationBuffer) + return false; + + + auto width = this->dataPtr->camera->ImageWidth(); + auto height = this->dataPtr->camera->ImageHeight(); + + // create message + this->dataPtr->segmentationMsg.set_width(width); + this->dataPtr->segmentationMsg.set_height(height); + // format + this->dataPtr->segmentationMsg.set_step( + width * rendering::PixelUtil::BytesPerPixel(rendering::PF_R8G8B8)); + this->dataPtr->segmentationMsg.set_pixel_format_type( + msgs::PixelFormatType::RGB_INT8); + // time stamp + auto stamp = this->dataPtr->segmentationMsg.mutable_header()->mutable_stamp(); + *stamp = msgs::Convert(_now); + auto frame = this->dataPtr->segmentationMsg.mutable_header()->add_data(); + frame->set_key("frame_id"); + frame->add_value(this->Name()); + // segmentation data + this->dataPtr->segmentationMsg.set_data(this->dataPtr->segmentationBuffer, + rendering::PixelUtil::MemorySize(rendering::PF_R8G8B8, + width, height)); + + std::lock_guard lock(this->dataPtr->mutex); + + // publish + this->PublishInfo(_now); + this->dataPtr->publisher.Publish(this->dataPtr->segmentationMsg); + + return true; +} + +///////////////////////////////////////////////// +unsigned int SegmentationCameraSensor::ImageHeight() const +{ + return this->dataPtr->camera->ImageHeight(); +} + +///////////////////////////////////////////////// +unsigned int SegmentationCameraSensor::ImageWidth() const +{ + return this->dataPtr->camera->ImageWidth(); +} + +IGN_SENSORS_REGISTER_SENSOR(SegmentationCameraSensor) From 6bde206516437772c2208089161ff0671503cea8 Mon Sep 17 00:00:00 2001 From: AmrElsersy Date: Sat, 12 Jun 2021 16:54:28 +0200 Subject: [PATCH 02/22] Image Events Signed-off-by: AmrElsersy --- .../sensors/SegmentationCameraSensor.hh | 12 +++ src/SegmentationCameraSensor.cc | 95 ++++++++++++++++++- 2 files changed, 105 insertions(+), 2 deletions(-) diff --git a/include/ignition/sensors/SegmentationCameraSensor.hh b/include/ignition/sensors/SegmentationCameraSensor.hh index 0a8dc948..216049aa 100644 --- a/include/ignition/sensors/SegmentationCameraSensor.hh +++ b/include/ignition/sensors/SegmentationCameraSensor.hh @@ -99,6 +99,18 @@ namespace ignition public: void OnNewSegmentationFrame(const uint8_t *, unsigned int, unsigned int, unsigned int, const std::string &); + /// \brief Set a callback to be called when image frame data is + /// generated. + /// \param[in] _callback This callback will be called every time the + /// camera produces image data. The Update function will be blocked + /// while the callbacks are executed. + /// \remark Do not block inside of the callback. + /// \return A connection pointer that must remain in scope. When the + /// connection pointer falls out of scope, the connection is broken. + public: ignition::common::ConnectionPtr ConnectImageCallback( + std::function< + void(const ignition::msgs::Image &)> _callback); + /// \brief Set the rendering scene. /// \param[in] _scene Pointer to the scene public: virtual void SetScene( diff --git a/src/SegmentationCameraSensor.cc b/src/SegmentationCameraSensor.cc index dc11d5ee..8ffd33d2 100644 --- a/src/SegmentationCameraSensor.cc +++ b/src/SegmentationCameraSensor.cc @@ -20,6 +20,7 @@ #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" @@ -36,6 +37,17 @@ using namespace sensors; class ignition::sensors::SegmentationCameraSensorPrivate { + /// \brief Save an image + /// \param[in] _data the image data to be saved + /// \param[in] _width width of image in pixels + /// \param[in] _height height of image in pixels + /// \return True if the image was saved successfully. False can mean + /// that the path provided to the constructor does exist and creation + /// of the path was not possible. + /// \sa ImageSaver + public: bool SaveImage(const uint8_t *_data, unsigned int _width, + unsigned int _height); + /// \brief SDF Sensor DOM Object public: sdf::Sensor sdfSensor; @@ -75,6 +87,23 @@ class ignition::sensors::SegmentationCameraSensorPrivate /// \brief Just a mutex for thread safety public: std::mutex mutex; + + /// \brief True to save images + public: bool saveImage = false; + + /// \brief path directory to where images are saved + public: std::string saveImagePath = "./"; + + /// \brief Prefix of an image name + public: std::string saveImagePrefix = "./"; + + /// \brief counter used to set the image filename + public: std::uint64_t saveImageCounter = 0; + + /// \brief Event that is used to trigger callbacks when a new image + /// is generated + public: ignition::common::EventT< + void(const ignition::msgs::Image &)> imageEvent; }; ////////////////////////////////////////////////// @@ -258,6 +287,14 @@ bool SegmentationCameraSensor::CreateCamera() std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, std::placeholders::_4, std::placeholders::_5)); + // Create the directory to store frames + if (sdfCamera->SaveFrames()) + { + this->dataPtr->saveImagePath = sdfCamera->SaveFramesPath(); + this->dataPtr->saveImagePrefix = this->Name() + "_"; + this->dataPtr->saveImage = true; + } + return true; } @@ -306,8 +343,12 @@ bool SegmentationCameraSensor::Update( } // don't render if there is no subscribers - if (!this->dataPtr->publisher.HasConnections()) + if (!this->dataPtr->publisher.HasConnections() && + this->dataPtr->imageEvent.ConnectionCount() <= 0 && + !this->dataPtr->saveImage) + { return false; + } // Actual render this->Render(); @@ -340,10 +381,29 @@ bool SegmentationCameraSensor::Update( std::lock_guard lock(this->dataPtr->mutex); - // publish + // Publish this->PublishInfo(_now); this->dataPtr->publisher.Publish(this->dataPtr->segmentationMsg); + // Trigger callbacks. + if (this->dataPtr->imageEvent.ConnectionCount() > 0) + { + try + { + this->dataPtr->imageEvent(this->dataPtr->segmentationMsg); + } + catch(...) + { + ignerr << "Exception thrown in an image callback.\n"; + } + } + + // Save image + if (this->dataPtr->saveImage) + { + this->dataPtr->SaveImage(this->dataPtr->segmentationBuffer, width, height); + } + return true; } @@ -359,4 +419,35 @@ unsigned int SegmentationCameraSensor::ImageWidth() const return this->dataPtr->camera->ImageWidth(); } +///////////////////////////////////////////////// +common::ConnectionPtr SegmentationCameraSensor::ConnectImageCallback( + std::function _callback) +{ + return this->dataPtr->imageEvent.Connect(_callback); +} + +////////////////////////////////////////////////// +bool SegmentationCameraSensorPrivate::SaveImage( + const uint8_t *_data, unsigned int _width, unsigned int _height) +{ + // Attempt to create the directory if it doesn't exist + if (!ignition::common::isDirectory(this->saveImagePath)) + { + if (!ignition::common::createDirectories(this->saveImagePath)) + return false; + } + + std::string filename = this->saveImagePrefix + + std::to_string(this->saveImageCounter) + ".png"; + ++this->saveImageCounter; + + ignition::common::Image localImage; + localImage.SetFromData(_data, _width, _height, + ignition::common::Image::RGB_INT8); + + localImage.SavePNG( + ignition::common::joinPaths(this->saveImagePath, filename)); + return true; +} + IGN_SENSORS_REGISTER_SENSOR(SegmentationCameraSensor) From 4ea873a49e96ca3be0c8ad8fb21936e9cc132b84 Mon Sep 17 00:00:00 2001 From: AmrElsersy Date: Sun, 13 Jun 2021 05:22:43 +0200 Subject: [PATCH 03/22] Add Testing Signed-off-by: AmrElsersy --- src/SegmentationCameraSensor.cc | 3 +- test/integration/CMakeLists.txt | 2 + .../integration/segmentation_camera_plugin.cc | 305 ++++++++++++++++++ .../segmentation_camera_sensor_builtin.sdf | 22 ++ 4 files changed, 330 insertions(+), 2 deletions(-) create mode 100644 test/integration/segmentation_camera_plugin.cc create mode 100644 test/integration/segmentation_camera_sensor_builtin.sdf diff --git a/src/SegmentationCameraSensor.cc b/src/SegmentationCameraSensor.cc index 8ffd33d2..4820af43 100644 --- a/src/SegmentationCameraSensor.cc +++ b/src/SegmentationCameraSensor.cc @@ -77,7 +77,7 @@ class ignition::sensors::SegmentationCameraSensorPrivate /// \brief True if camera generates a colored map image /// False if camera generates labels ids image - public: bool isColoredMap {true}; + public: bool isColoredMap {false}; /// \brief Connection to the new segmentation frames data public: common::ConnectionPtr newSegmentationConnection; @@ -263,7 +263,6 @@ bool SegmentationCameraSensor::CreateCamera() this->dataPtr->camera->SetVisibilityMask(sdfCamera->VisibilityMask()); this->dataPtr->camera->SetNearClipPlane(sdfCamera->NearClip()); this->dataPtr->camera->SetFarClipPlane(sdfCamera->FarClip()); - this->dataPtr->camera->SetHFOV(sdfCamera->HorizontalFov()); math::Angle angle = sdfCamera->HorizontalFov(); if (angle < 0.01 || angle > IGN_PI*2) { diff --git a/test/integration/CMakeLists.txt b/test/integration/CMakeLists.txt index 2c5de9fe..0099f5d0 100644 --- a/test/integration/CMakeLists.txt +++ b/test/integration/CMakeLists.txt @@ -6,6 +6,7 @@ set(dri_tests gpu_lidar_sensor_plugin.cc rgbd_camera_plugin.cc thermal_camera_plugin.cc + segmentation_camera_plugin.cc ) set(tests @@ -39,6 +40,7 @@ if (DRI_TESTS) ${PROJECT_LIBRARY_TARGET_NAME}-gpu_lidar ${PROJECT_LIBRARY_TARGET_NAME}-rgbd_camera ${PROJECT_LIBRARY_TARGET_NAME}-thermal_camera + ${PROJECT_LIBRARY_TARGET_NAME}-segmentation_camera ) endif() diff --git a/test/integration/segmentation_camera_plugin.cc b/test/integration/segmentation_camera_plugin.cc new file mode 100644 index 00000000..099d78fd --- /dev/null +++ b/test/integration/segmentation_camera_plugin.cc @@ -0,0 +1,305 @@ +/* + * 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 + +#ifdef _WIN32 +#pragma warning(push) +#pragma warning(disable: 4251) +#endif +#include +#include +#include +#include +#ifdef _WIN32 +#pragma warning(pop) +#endif + +#ifdef _WIN32 +#pragma warning(push) +#pragma warning(disable: 4005) +#pragma warning(disable: 4251) +#endif +#include +#ifdef _WIN32 +#pragma warning(pop) +#endif + +#include "test_config.h" // NOLINT(build/include) +#include "TransportTestTools.hh" + +using namespace ignition; + +class SegmentationCameraSensorTest: public testing::Test, + public testing::WithParamInterface +{ + // Create a Segmentation Camera sensor from a SDF and gets a image message + public: void ImagesWithBuiltinSDF(const std::string &_renderEngine); +}; + +/// \brief mutex for thread safety +std::mutex g_mutex; + +/// \brief Segmentation buffer +unsigned char *g_buffer = nullptr; + +/// \brief counter of received segmentation msgs +int g_counter = 0; + +/// \brief callback to get the segmentation buffer +void OnNewSegmentationFrame(const msgs::Image &_msg) +{ + g_mutex.lock(); + unsigned int size = _msg.width() * _msg.height() * 3; + + if (!g_buffer) + g_buffer = new unsigned char[size]; + + memcpy(g_buffer, _msg.data().c_str(), size); + 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 besides each other +/// the 2 aside boxes have the same label & the middle is different +void BuildScene(rendering::ScenePtr scene) +{ + math::Vector3d leftPosition(3, -1.5, 0); + math::Vector3d rightPosition(3, 1.5, 0); + math::Vector3d middlePosition(3, 0, 0); + + rendering::VisualPtr root = scene->RootVisual(); + + // create box visual + rendering::VisualPtr box = scene->CreateVisual(); + box->AddGeometry(scene->CreateBox()); + box->SetOrigin(0.0, 0.0, 0.0); + box->SetLocalPosition(leftPosition); + box->SetLocalRotation(0, 0, 0); + box->SetUserData("label", 1); + root->AddChild(box); + + // create box visual of same label + rendering::VisualPtr box1 = scene->CreateVisual(); + box1->AddGeometry(scene->CreateBox()); + box1->SetOrigin(0.0, 0.0, 0.0); + box1->SetLocalPosition(rightPosition); + box1->SetLocalRotation(0, 0, 0); + box1->SetUserData("label", 1); + root->AddChild(box1); + + // create box visual of different label + ignition::rendering::VisualPtr box2 = scene->CreateVisual(); + box2->AddGeometry(scene->CreateBox()); + box2->SetOrigin(0.0, 0.0, 0.0); + box2->SetLocalPosition(middlePosition); + box2->SetLocalRotation(0, 0, 0); + box2->SetUserData("label", 2); + root->AddChild(box2); +} + + +void SegmentationCameraSensorTest::ImagesWithBuiltinSDF( + const std::string &_renderEngine) +{ + std::string path = ignition::common::joinPaths(PROJECT_SOURCE_PATH, "test", + "integration", "segmentation_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"); + + int width = imagePtr->Get("width"); + int height = imagePtr->Get("height"); + + // 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, "segmentation_camera"); + + ignition::sensors::SegmentationCameraSensor *sensor = + mgr.CreateSensor(sdfSensor); + + EXPECT_NE(sensor, nullptr); + sensor->SetScene(scene); + + EXPECT_EQ(width, sensor->ImageWidth()); + EXPECT_EQ(height, sensor->ImageHeight()); + + auto camera = sensor->SegmentationCamera(); + EXPECT_NE(camera, nullptr); + + uint32_t backgroundLabel = 23; + camera->SetSegmentationType(SegmentationType::Semantic); + camera->EnableColoredMap(false); + camera->SetBackgroundLabel(backgroundLabel); + camera->SetLocalPosition(0.0, 0.0, 0.0); + camera->SetLocalRotation(0.0, 0.0, 0.0); + + // Get the Msg + std::string topic = + "/test/integration/SegmentationCameraPlugin_imagesWithBuiltinSDF"; + WaitForMessageTestHelper helper(topic); + + // Update once to create image + mgr.RunOnce(std::chrono::steady_clock::duration::zero()); + + EXPECT_TRUE(helper.WaitForMessage()) << helper; + + // subscribe to the segmentation camera topic + ignition::transport::Node node; + node.Subscribe(topic, &OnNewSegmentationFrame); + + // wait for a few segmentation camera frames + mgr.RunOnce(std::chrono::steady_clock::duration::zero(), true); + + // wait for a new frame + WaitForNewFrame(); + + // get the center of each box, the percentages locates the center + math::Vector2d leftProj(width * 0.25, height * 0.5); + math::Vector2d rightProj(width * 0.75, height * 0.5); + math::Vector2d middleProj(width * 0.5, height * 0.5); + + // get their index in the buffer + uint32_t leftIndex = (leftProj.Y() * width + leftProj.X()) * 3; + uint32_t rightIndex = (rightProj.Y() * width + rightProj.X()) * 3; + uint32_t middleIndex = (middleProj.Y() * width + middleProj.X()) * 3; + + // test + g_mutex.lock(); + g_counter = 0; + + // cast the unsigned char to unsigned int to read it + unsigned int leftLabel = g_buffer[leftIndex]; + unsigned int rightLabel = g_buffer[rightIndex]; + unsigned int middleLabel = g_buffer[middleIndex]; + + // check the label + EXPECT_EQ(leftLabel , 1); + EXPECT_EQ(middleLabel, 2); + EXPECT_EQ(rightLabel , 1); + + // check if the first pixel(background) = the background label + unsigned int background = g_buffer[0]; + EXPECT_EQ(background, backgroundLabel); + + g_mutex.unlock(); + + + // Instance/Panoptic Segmentation Test + camera->SetSegmentationType(SegmentationType::Panoptic); + + // wait for a few segmentation camera frames + mgr.RunOnce(std::chrono::steady_clock::duration::zero(), true); + // wait for a new frame + WaitForNewFrame(); + + // the label in the last channel + leftLabel = g_buffer[leftIndex + 2]; + rightLabel = g_buffer[rightIndex + 2]; + middleLabel = g_buffer[middleIndex + 2]; + + // the instances count in the first channel + unsigned int leftCount = g_buffer[leftIndex]; + unsigned int rightCount = g_buffer[rightIndex]; + unsigned int middleCount = g_buffer[middleIndex]; + + // test + g_mutex.lock(); + g_counter = 0; + + // check the label + EXPECT_EQ(leftLabel , 1); + EXPECT_EQ(middleLabel, 2); + EXPECT_EQ(rightLabel , 1); + + // instance count + EXPECT_EQ(middleCount, 1); + EXPECT_EQ(rightCount, 1); + EXPECT_EQ(leftCount, 2); + + g_mutex.unlock(); + + // Clean up + // engine->DestroyScene(scene); + // ignition::rendering::unloadEngine(engine->Name()); +} + + +////////////////////////////////////////////////// +TEST_P(SegmentationCameraSensorTest, ImagesWithBuiltinSDF) +{ + ImagesWithBuiltinSDF(GetParam()); +} + +INSTANTIATE_TEST_CASE_P(SegmentationCameraSensor, SegmentationCameraSensorTest, + 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/segmentation_camera_sensor_builtin.sdf b/test/integration/segmentation_camera_sensor_builtin.sdf new file mode 100644 index 00000000..b40d1505 --- /dev/null +++ b/test/integration/segmentation_camera_sensor_builtin.sdf @@ -0,0 +1,22 @@ + + + + + + 10 + /test/integration/SegmentationCameraPlugin_imagesWithBuiltinSDF + + 1.05 + + 320 + 240 + + + 0.1 + 1000.0 + + + + + + From d2457899bbcd353c25cc0f56f342e2da9d05631e Mon Sep 17 00:00:00 2001 From: AmrElsersy Date: Tue, 15 Jun 2021 02:27:56 +0200 Subject: [PATCH 04/22] style Signed-off-by: AmrElsersy --- .../sensors/SegmentationCameraSensor.hh | 11 ++-------- src/SegmentationCameraSensor.cc | 20 +++++++------------ .../integration/segmentation_camera_plugin.cc | 8 ++++---- 3 files changed, 13 insertions(+), 26 deletions(-) diff --git a/include/ignition/sensors/SegmentationCameraSensor.hh b/include/ignition/sensors/SegmentationCameraSensor.hh index 216049aa..a34788f2 100644 --- a/include/ignition/sensors/SegmentationCameraSensor.hh +++ b/include/ignition/sensors/SegmentationCameraSensor.hh @@ -73,21 +73,14 @@ namespace ignition /// \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 IGN_DEPRECATED(4) Update( - const ignition::common::Time &_now) 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 Force the sensor to generate data - /// \param[in] _now The current time - /// \return true if the update was successfull + /// \brief Get the rendering segmentation camera + /// \return Segmentation camera pointer public: virtual rendering::SegmentationCameraPtr SegmentationCamera(); /// \brief Segmentation data callback used to get the data from the sensor diff --git a/src/SegmentationCameraSensor.cc b/src/SegmentationCameraSensor.cc index 4820af43..2e881494 100644 --- a/src/SegmentationCameraSensor.cc +++ b/src/SegmentationCameraSensor.cc @@ -34,6 +34,7 @@ using namespace ignition; using namespace sensors; +using namespace rendering; class ignition::sensors::SegmentationCameraSensorPrivate { @@ -51,16 +52,16 @@ class ignition::sensors::SegmentationCameraSensorPrivate /// \brief SDF Sensor DOM Object public: sdf::Sensor sdfSensor; - /// \brief true if Load() has been called and was successful + /// \brief True if Load() has been called and was successful public: bool initialized = false; - /// \brief rendering Segmentation Camera + /// \brief Rendering Segmentation Camera public: rendering::SegmentationCameraPtr camera {nullptr}; - /// \brief node to create publisher + /// \brief Node to create publisher public: transport::Node node; - /// \brief publisher to publish segmentation image + /// \brief Publisher to publish segmentation image public: transport::Node::Publisher publisher; /// \brief Segmentation Image Msg @@ -91,13 +92,13 @@ class ignition::sensors::SegmentationCameraSensorPrivate /// \brief True to save images public: bool saveImage = false; - /// \brief path directory to where images are saved + /// \brief Path directory to where images are saved public: std::string saveImagePath = "./"; /// \brief Prefix of an image name public: std::string saveImagePrefix = "./"; - /// \brief counter used to set the image filename + /// \brief Counter used to set the image filename public: std::uint64_t saveImageCounter = 0; /// \brief Event that is used to trigger callbacks when a new image @@ -273,7 +274,6 @@ bool SegmentationCameraSensor::CreateCamera() this->dataPtr->camera->SetHFOV(angle); // Add the camera to the scene - // this->dataPtr->camera->SetLocalPose(sdfCamera->RawPose()); this->Scene()->RootVisual()->AddChild(this->dataPtr->camera); // Add the rendering sensor to handle its render @@ -318,12 +318,6 @@ void SegmentationCameraSensor::OnNewSegmentationFrame(const uint8_t * _data, memcpy(this->dataPtr->segmentationBuffer, _data, bufferSize); } -////////////////////////////////////////////////// -bool SegmentationCameraSensor::Update(const ignition::common::Time &_now) -{ - return this->Update(math::secNsecToDuration(_now.sec, _now.nsec)); -} - ////////////////////////////////////////////////// bool SegmentationCameraSensor::Update( const std::chrono::steady_clock::duration &_now) diff --git a/test/integration/segmentation_camera_plugin.cc b/test/integration/segmentation_camera_plugin.cc index 099d78fd..1b566094 100644 --- a/test/integration/segmentation_camera_plugin.cc +++ b/test/integration/segmentation_camera_plugin.cc @@ -47,6 +47,7 @@ #include "TransportTestTools.hh" using namespace ignition; +using namespace rendering; class SegmentationCameraSensorTest: public testing::Test, public testing::WithParamInterface @@ -133,7 +134,7 @@ void BuildScene(rendering::ScenePtr scene) root->AddChild(box2); } - +///////////////////////////////////////////////// void SegmentationCameraSensorTest::ImagesWithBuiltinSDF( const std::string &_renderEngine) { @@ -246,7 +247,6 @@ void SegmentationCameraSensorTest::ImagesWithBuiltinSDF( g_mutex.unlock(); - // Instance/Panoptic Segmentation Test camera->SetSegmentationType(SegmentationType::Panoptic); @@ -282,8 +282,8 @@ void SegmentationCameraSensorTest::ImagesWithBuiltinSDF( g_mutex.unlock(); // Clean up - // engine->DestroyScene(scene); - // ignition::rendering::unloadEngine(engine->Name()); + engine->DestroyScene(scene); + ignition::rendering::unloadEngine(engine->Name()); } From 08486833fe5e226c0c005d655ae621e214bc844b Mon Sep 17 00:00:00 2001 From: AmrElsersy Date: Wed, 30 Jun 2021 06:10:45 +0200 Subject: [PATCH 05/22] Publish both colored map & labels map Signed-off-by: AmrElsersy --- .../sensors/SegmentationCameraSensor.hh | 7 +- src/CMakeLists.txt | 3 +- src/SegmentationCameraSensor.cc | 125 +++++++++++------- .../integration/segmentation_camera_plugin.cc | 7 +- 4 files changed, 88 insertions(+), 54 deletions(-) diff --git a/include/ignition/sensors/SegmentationCameraSensor.hh b/include/ignition/sensors/SegmentationCameraSensor.hh index a34788f2..3ac587a0 100644 --- a/include/ignition/sensors/SegmentationCameraSensor.hh +++ b/include/ignition/sensors/SegmentationCameraSensor.hh @@ -75,7 +75,7 @@ namespace ignition /// \brief Force the sensor to generate data /// \param[in] _now The current time - /// \return true if the update was successfull + /// \return true if the update was successful public: virtual bool Update( const std::chrono::steady_clock::duration &_now) override; @@ -89,8 +89,9 @@ namespace ignition /// \param[in] _height height of the Segmentation image /// \param[in] _channel bytes used for the Segmentation data /// \param[in] _format string with the format - public: void OnNewSegmentationFrame(const uint8_t *, unsigned int, - unsigned int, unsigned int, const std::string &); + public: void OnNewSegmentationFrame(const uint8_t * _scan, + unsigned int _width, unsigned int _height, unsigned int _channel, + const std::string &_format); /// \brief Set a callback to be called when image frame data is /// generated. diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index 2dd9919b..f7a20326 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -139,12 +139,13 @@ target_link_libraries(${thermal_camera_target} 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) target_link_libraries(${segmentation_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/src/SegmentationCameraSensor.cc b/src/SegmentationCameraSensor.cc index 2e881494..017f5046 100644 --- a/src/SegmentationCameraSensor.cc +++ b/src/SegmentationCameraSensor.cc @@ -34,7 +34,6 @@ using namespace ignition; using namespace sensors; -using namespace rendering; class ignition::sensors::SegmentationCameraSensorPrivate { @@ -61,30 +60,39 @@ class ignition::sensors::SegmentationCameraSensorPrivate /// \brief Node to create publisher public: transport::Node node; - /// \brief Publisher to publish segmentation image - public: transport::Node::Publisher publisher; + /// \brief Publisher to publish segmentation colored image + public: transport::Node::Publisher coloredMapPublisher; + + /// \brief Publisher to publish segmentation labels image + public: transport::Node::Publisher labelsMapPublisher; /// \brief Segmentation Image Msg - public: msgs::Image segmentationMsg; + public: msgs::Image coloredMapMsg; - /// \brief Topic to publish the segmentation image - public: std::string topicSegmentation = ""; + /// \brief Segmentation Image Msg + public: msgs::Image labelsMapMsg; - /// \brief Buffer contains the segmentation map data - public: uint8_t *segmentationBuffer {nullptr}; + /// \brief Topic to publish the segmentation colored map + public: std::string topicColoredMap = "/colored_map"; - /// \brief Segmentation type (Semantic / Instance) - public: SegmentationType type {SegmentationType::Semantic}; + /// \brief Topic to publish the segmentation labels map + public: std::string topicLabelsMap = "/labels_map"; - /// \brief True if camera generates a colored map image - /// False if camera generates labels ids image - public: bool isColoredMap {false}; + /// \brief Buffer contains the segmentation colored map data + public: uint8_t *segmentationColoredBuffer {nullptr}; + + /// \brief Buffer contains the segmentation labels map data + public: uint8_t *segmentationLabelsBuffer {nullptr}; + + /// \brief Segmentation type (Semantic / Instance) + public: rendering::SegmentationType type + {rendering::SegmentationType::Semantic}; /// \brief Connection to the new segmentation frames data - public: common::ConnectionPtr newSegmentationConnection; + public: common::ConnectionPtr newSegmentationConnection {nullptr}; /// \brief Connection to the Manager's scene change event. - public: common::ConnectionPtr sceneChangeConnection; + public: common::ConnectionPtr sceneChangeConnection {nullptr}; /// \brief Just a mutex for thread safety public: std::mutex mutex; @@ -116,8 +124,11 @@ SegmentationCameraSensor::SegmentationCameraSensor() ///////////////////////////////////////////////// SegmentationCameraSensor::~SegmentationCameraSensor() { - if (this->dataPtr->segmentationBuffer) - delete this->dataPtr->segmentationBuffer; + if (this->dataPtr->segmentationColoredBuffer) + delete this->dataPtr->segmentationColoredBuffer; + + if (this->dataPtr->segmentationLabelsBuffer) + delete this->dataPtr->segmentationLabelsBuffer; } ///////////////////////////////////////////////// @@ -151,14 +162,9 @@ bool SegmentationCameraSensor::Load(const sdf::Sensor &_sdf) }); if (type == "semantic") - this->dataPtr->type = SegmentationType::Semantic; + this->dataPtr->type = rendering::SegmentationType::Semantic; else if (type == "instance" || type == "panoptic") - this->dataPtr->type = SegmentationType::Panoptic; - } - - if (sdfElement->HasElement("colored")) - { - this->dataPtr->isColoredMap = sdfElement->Get("colored"); + this->dataPtr->type = rendering::SegmentationType::Panoptic; } if (!Sensor::Load(_sdf)) @@ -183,12 +189,17 @@ bool SegmentationCameraSensor::Load(const sdf::Sensor &_sdf) this->dataPtr->sdfSensor = _sdf; - // Create the thermal image publisher - this->dataPtr->publisher = + // Create the segmentation colored map image publisher + this->dataPtr->coloredMapPublisher = + this->dataPtr->node.Advertise( + this->Topic() + this->dataPtr->topicColoredMap); + + // Create the segmentation labels map image publisher + this->dataPtr->labelsMapPublisher = this->dataPtr->node.Advertise( - this->Topic()); + this->Topic() + this->dataPtr->topicLabelsMap); - if (!this->dataPtr->publisher) + if (!this->dataPtr->labelsMapPublisher || !this->dataPtr->coloredMapPublisher) { ignerr << "Unable to create publisher on topic[" << this->Topic() << "].\n"; @@ -253,7 +264,8 @@ bool SegmentationCameraSensor::CreateCamera() // Segmentation properties this->dataPtr->camera->SetSegmentationType(this->dataPtr->type); - this->dataPtr->camera->EnableColoredMap(this->dataPtr->isColoredMap); + // Must be true to generate the colored map first then convert it + this->dataPtr->camera->EnableColoredMap(true); auto width = sdfCamera->ImageWidth(); auto height = sdfCamera->ImageHeight(); @@ -312,10 +324,17 @@ void SegmentationCameraSensor::OnNewSegmentationFrame(const uint8_t * _data, unsigned int bufferSize = _width * _height * _channles; - if (!this->dataPtr->segmentationBuffer) - this->dataPtr->segmentationBuffer = new uint8_t[bufferSize]; + if (!this->dataPtr->segmentationColoredBuffer) + this->dataPtr->segmentationColoredBuffer = new uint8_t[bufferSize]; + + if (!this->dataPtr->segmentationLabelsBuffer) + this->dataPtr->segmentationLabelsBuffer = new uint8_t[bufferSize]; + + memcpy(this->dataPtr->segmentationColoredBuffer, _data, bufferSize); - memcpy(this->dataPtr->segmentationBuffer, _data, bufferSize); + // Convert the colored map to labels map + this->dataPtr->camera->LabelMapFromColoredBuffer( + this->dataPtr->segmentationLabelsBuffer); } ////////////////////////////////////////////////// @@ -336,7 +355,8 @@ bool SegmentationCameraSensor::Update( } // don't render if there is no subscribers - if (!this->dataPtr->publisher.HasConnections() && + if (!this->dataPtr->coloredMapPublisher.HasConnections() && + !this->dataPtr->labelsMapPublisher.HasConnections() && this->dataPtr->imageEvent.ConnectionCount() <= 0 && !this->dataPtr->saveImage) { @@ -346,29 +366,38 @@ bool SegmentationCameraSensor::Update( // Actual render this->Render(); - if (!this->dataPtr->segmentationBuffer) + if (!this->dataPtr->segmentationColoredBuffer || + !this->dataPtr->segmentationLabelsBuffer) return false; - auto width = this->dataPtr->camera->ImageWidth(); auto height = this->dataPtr->camera->ImageHeight(); - // create message - this->dataPtr->segmentationMsg.set_width(width); - this->dataPtr->segmentationMsg.set_height(height); + // create colored map message + this->dataPtr->coloredMapMsg.set_width(width); + this->dataPtr->coloredMapMsg.set_height(height); // format - this->dataPtr->segmentationMsg.set_step( + this->dataPtr->coloredMapMsg.set_step( width * rendering::PixelUtil::BytesPerPixel(rendering::PF_R8G8B8)); - this->dataPtr->segmentationMsg.set_pixel_format_type( + this->dataPtr->coloredMapMsg.set_pixel_format_type( msgs::PixelFormatType::RGB_INT8); // time stamp - auto stamp = this->dataPtr->segmentationMsg.mutable_header()->mutable_stamp(); + auto stamp = this->dataPtr->coloredMapMsg.mutable_header()->mutable_stamp(); *stamp = msgs::Convert(_now); - auto frame = this->dataPtr->segmentationMsg.mutable_header()->add_data(); + auto frame = this->dataPtr->coloredMapMsg.mutable_header()->add_data(); frame->set_key("frame_id"); frame->add_value(this->Name()); - // segmentation data - this->dataPtr->segmentationMsg.set_data(this->dataPtr->segmentationBuffer, + + this->dataPtr->labelsMapMsg.CopyFrom(this->dataPtr->coloredMapMsg); + + // segmentation colored map data + this->dataPtr->coloredMapMsg.set_data( + this->dataPtr->segmentationColoredBuffer, + rendering::PixelUtil::MemorySize(rendering::PF_R8G8B8, + width, height)); + + // segmentation colored map data + this->dataPtr->labelsMapMsg.set_data(this->dataPtr->segmentationLabelsBuffer, rendering::PixelUtil::MemorySize(rendering::PF_R8G8B8, width, height)); @@ -376,14 +405,15 @@ bool SegmentationCameraSensor::Update( // Publish this->PublishInfo(_now); - this->dataPtr->publisher.Publish(this->dataPtr->segmentationMsg); + this->dataPtr->coloredMapPublisher.Publish(this->dataPtr->coloredMapMsg); + this->dataPtr->labelsMapPublisher.Publish(this->dataPtr->labelsMapMsg); // Trigger callbacks. if (this->dataPtr->imageEvent.ConnectionCount() > 0) { try { - this->dataPtr->imageEvent(this->dataPtr->segmentationMsg); + this->dataPtr->imageEvent(this->dataPtr->coloredMapMsg); } catch(...) { @@ -394,7 +424,8 @@ bool SegmentationCameraSensor::Update( // Save image if (this->dataPtr->saveImage) { - this->dataPtr->SaveImage(this->dataPtr->segmentationBuffer, width, height); + this->dataPtr->SaveImage( + this->dataPtr->segmentationColoredBuffer, width, height); } return true; diff --git a/test/integration/segmentation_camera_plugin.cc b/test/integration/segmentation_camera_plugin.cc index 1b566094..d016f6f4 100644 --- a/test/integration/segmentation_camera_plugin.cc +++ b/test/integration/segmentation_camera_plugin.cc @@ -192,14 +192,16 @@ void SegmentationCameraSensorTest::ImagesWithBuiltinSDF( uint32_t backgroundLabel = 23; camera->SetSegmentationType(SegmentationType::Semantic); - camera->EnableColoredMap(false); + camera->EnableColoredMap(true); camera->SetBackgroundLabel(backgroundLabel); camera->SetLocalPosition(0.0, 0.0, 0.0); camera->SetLocalRotation(0.0, 0.0, 0.0); - // Get the Msg + // Topic std::string topic = "/test/integration/SegmentationCameraPlugin_imagesWithBuiltinSDF"; + // Get the topic of the labels map + topic += "/labels_map"; WaitForMessageTestHelper helper(topic); // Update once to create image @@ -286,7 +288,6 @@ void SegmentationCameraSensorTest::ImagesWithBuiltinSDF( ignition::rendering::unloadEngine(engine->Name()); } - ////////////////////////////////////////////////// TEST_P(SegmentationCameraSensorTest, ImagesWithBuiltinSDF) { From c851f1e24ac039b7b40c962bff4b6ead1d34c40a Mon Sep 17 00:00:00 2001 From: AmrElsersy Date: Wed, 30 Jun 2021 07:26:11 +0200 Subject: [PATCH 06/22] Style + Add complete overlapping in integration test Signed-off-by: AmrElsersy --- .../sensors/SegmentationCameraSensor.hh | 22 ++- src/SegmentationCameraSensor.cc | 66 +++++---- .../integration/segmentation_camera_plugin.cc | 132 +++++++++++------- 3 files changed, 131 insertions(+), 89 deletions(-) diff --git a/include/ignition/sensors/SegmentationCameraSensor.hh b/include/ignition/sensors/SegmentationCameraSensor.hh index 3ac587a0..10230f99 100644 --- a/include/ignition/sensors/SegmentationCameraSensor.hh +++ b/include/ignition/sensors/SegmentationCameraSensor.hh @@ -20,20 +20,18 @@ #include #include -#include #include #include #include #include - -#include "ignition/msgs.hh" -#include "ignition/transport/Node.hh" -#include "ignition/transport/Publisher.hh" - #include "ignition/sensors/CameraSensor.hh" #include "ignition/sensors/Export.hh" +#include "ignition/transport/Node.hh" +#include "ignition/transport/Publisher.hh" #include "ignition/sensors/Sensor.hh" +#include "ignition/msgs.hh" +#include namespace ignition { @@ -84,13 +82,13 @@ namespace ignition public: virtual rendering::SegmentationCameraPtr SegmentationCamera(); /// \brief Segmentation data callback used to get the data from the sensor - /// \param[in] _scan pointer to the data from the sensor - /// \param[in] _width width of the Segmentation image - /// \param[in] _height height of the Segmentation image - /// \param[in] _channel bytes used for the Segmentation data + /// \param[in] _data pointer to the data from the sensor + /// \param[in] _width width of the segmentation image + /// \param[in] _height height of the segmentation image + /// \param[in] _channels num of channels /// \param[in] _format string with the format - public: void OnNewSegmentationFrame(const uint8_t * _scan, - unsigned int _width, unsigned int _height, unsigned int _channel, + public: void OnNewSegmentationFrame(const uint8_t * _data, + unsigned int _width, unsigned int _height, unsigned int _channels, const std::string &_format); /// \brief Set a callback to be called when image frame data is diff --git a/src/SegmentationCameraSensor.cc b/src/SegmentationCameraSensor.cc index 017f5046..a445b007 100644 --- a/src/SegmentationCameraSensor.cc +++ b/src/SegmentationCameraSensor.cc @@ -21,16 +21,13 @@ #include "ignition/common/Console.hh" #include "ignition/common/Profiler.hh" #include "ignition/common/Image.hh" +#include "ignition/msgs.hh" +#include "ignition/rendering/SegmentationCamera.hh" #include "ignition/sensors/RenderingEvents.hh" #include "ignition/sensors/SensorFactory.hh" - #include "ignition/sensors/SegmentationCameraSensor.hh" -#include "ignition/rendering/SegmentationCamera.hh" - #include "ignition/transport/Node.hh" #include "ignition/transport/Publisher.hh" -#include "ignition/msgs.hh" - using namespace ignition; using namespace sensors; @@ -150,23 +147,6 @@ bool SegmentationCameraSensor::Load(const sdf::Sensor &_sdf) { std::lock_guard lock(this->dataPtr->mutex); - // Segmentation Type & Colored Properties - sdf::ElementPtr sdfElement = _sdf.Element(); - if (sdfElement->HasElement("segmentation_type")) - { - std::string type = sdfElement->Get("segmentation_type"); - - // convert type to lowercase - std::for_each(type.begin(), type.end(), [](char & c){ - c = std::tolower(c); - }); - - if (type == "semantic") - this->dataPtr->type = rendering::SegmentationType::Semantic; - else if (type == "instance" || type == "panoptic") - this->dataPtr->type = rendering::SegmentationType::Panoptic; - } - if (!Sensor::Load(_sdf)) { return false; @@ -189,6 +169,23 @@ bool SegmentationCameraSensor::Load(const sdf::Sensor &_sdf) this->dataPtr->sdfSensor = _sdf; + // Segmentation type + sdf::ElementPtr sdfElement = _sdf.Element(); + if (sdfElement->HasElement("segmentation_type")) + { + auto type = sdfElement->Get("segmentation_type"); + + // convert type to lowercase + std::for_each(type.begin(), type.end(), [](char & c){ + c = std::tolower(c); + }); + + if (type == "semantic") + this->dataPtr->type = rendering::SegmentationType::Semantic; + else if (type == "instance" || type == "panoptic") + this->dataPtr->type = rendering::SegmentationType::Panoptic; + } + // Create the segmentation colored map image publisher this->dataPtr->coloredMapPublisher = this->dataPtr->node.Advertise( @@ -206,12 +203,16 @@ bool SegmentationCameraSensor::Load(const sdf::Sensor &_sdf) return false; } - if (!this->AdvertiseInfo()) + if (!this->AdvertiseInfo(this->Topic() + "/camera_info")) return false; if (this->Scene()) { - this->CreateCamera(); + if (!this->CreateCamera()) + { + ignerr << "Unable to create segmentation camera sensor\n"; + return false; + } } this->dataPtr->sceneChangeConnection = @@ -245,7 +246,7 @@ void SegmentationCameraSensor::SetScene( ///////////////////////////////////////////////// bool SegmentationCameraSensor::CreateCamera() { - auto sdfCamera = this->dataPtr->sdfSensor.CameraSensor(); + const auto sdfCamera = this->dataPtr->sdfSensor.CameraSensor(); if (!sdfCamera) { ignerr << "Unable to access camera SDF element\n"; @@ -357,7 +358,7 @@ bool SegmentationCameraSensor::Update( // don't render if there is no subscribers if (!this->dataPtr->coloredMapPublisher.HasConnections() && !this->dataPtr->labelsMapPublisher.HasConnections() && - this->dataPtr->imageEvent.ConnectionCount() <= 0 && + this->dataPtr->imageEvent.ConnectionCount() <= 0u && !this->dataPtr->saveImage) { return false; @@ -390,26 +391,27 @@ bool SegmentationCameraSensor::Update( this->dataPtr->labelsMapMsg.CopyFrom(this->dataPtr->coloredMapMsg); + // Protect the data being modified by the segmentation buffers + std::lock_guard lock(this->dataPtr->mutex); + // segmentation colored map data this->dataPtr->coloredMapMsg.set_data( this->dataPtr->segmentationColoredBuffer, rendering::PixelUtil::MemorySize(rendering::PF_R8G8B8, width, height)); - // segmentation colored map data + // segmentation labels map data this->dataPtr->labelsMapMsg.set_data(this->dataPtr->segmentationLabelsBuffer, rendering::PixelUtil::MemorySize(rendering::PF_R8G8B8, width, height)); - std::lock_guard lock(this->dataPtr->mutex); - // Publish this->PublishInfo(_now); this->dataPtr->coloredMapPublisher.Publish(this->dataPtr->coloredMapMsg); this->dataPtr->labelsMapPublisher.Publish(this->dataPtr->labelsMapMsg); // Trigger callbacks. - if (this->dataPtr->imageEvent.ConnectionCount() > 0) + if (this->dataPtr->imageEvent.ConnectionCount() > 0u) { try { @@ -434,12 +436,16 @@ bool SegmentationCameraSensor::Update( ///////////////////////////////////////////////// unsigned int SegmentationCameraSensor::ImageHeight() const { + if (!this->dataPtr->camera) + return 0u; return this->dataPtr->camera->ImageHeight(); } ///////////////////////////////////////////////// unsigned int SegmentationCameraSensor::ImageWidth() const { + if (!this->dataPtr->camera) + return 0u; return this->dataPtr->camera->ImageWidth(); } diff --git a/test/integration/segmentation_camera_plugin.cc b/test/integration/segmentation_camera_plugin.cc index d016f6f4..8897fef9 100644 --- a/test/integration/segmentation_camera_plugin.cc +++ b/test/integration/segmentation_camera_plugin.cc @@ -47,32 +47,29 @@ #include "TransportTestTools.hh" using namespace ignition; -using namespace rendering; - -class SegmentationCameraSensorTest: public testing::Test, - public testing::WithParamInterface -{ - // Create a Segmentation Camera sensor from a SDF and gets a image message - public: void ImagesWithBuiltinSDF(const std::string &_renderEngine); -}; /// \brief mutex for thread safety std::mutex g_mutex; /// \brief Segmentation buffer -unsigned char *g_buffer = nullptr; +uint8_t *g_buffer = nullptr; /// \brief counter of received segmentation msgs int g_counter = 0; +/// \brief Label of the hidden box +uint8_t hiddenLabel = 4; + /// \brief callback to get the segmentation buffer void OnNewSegmentationFrame(const msgs::Image &_msg) { g_mutex.lock(); + + // the image has 3 channels unsigned int size = _msg.width() * _msg.height() * 3; if (!g_buffer) - g_buffer = new unsigned char[size]; + g_buffer = new uint8_t[size]; memcpy(g_buffer, _msg.data().c_str(), size); g_counter++; @@ -80,10 +77,16 @@ void OnNewSegmentationFrame(const msgs::Image &_msg) } /// \brief wait till you read the published frame -void WaitForNewFrame() +void WaitForNewFrame(ignition::sensors::Manager &mgr) { + // wait for a few segmentation camera frames + mgr.RunOnce(std::chrono::steady_clock::duration::zero(), true); + + g_counter = 0; + 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) @@ -97,15 +100,18 @@ void WaitForNewFrame() } /// \brief Build the scene with 3 boxes besides each other -/// the 2 aside boxes have the same label & the middle is different +/// the 2 outer boxes have the same label & the middle is different void BuildScene(rendering::ScenePtr scene) { math::Vector3d leftPosition(3, -1.5, 0); math::Vector3d rightPosition(3, 1.5, 0); math::Vector3d middlePosition(3, 0, 0); + math::Vector3d hiddenPosition(8, 0, 0); rendering::VisualPtr root = scene->RootVisual(); + double unitBoxSize = 1.0; + // create box visual rendering::VisualPtr box = scene->CreateVisual(); box->AddGeometry(scene->CreateBox()); @@ -113,6 +119,7 @@ void BuildScene(rendering::ScenePtr scene) box->SetLocalPosition(leftPosition); box->SetLocalRotation(0, 0, 0); box->SetUserData("label", 1); + box->SetLocalScale(unitBoxSize, unitBoxSize, unitBoxSize); root->AddChild(box); // create box visual of same label @@ -122,6 +129,7 @@ void BuildScene(rendering::ScenePtr scene) box1->SetLocalPosition(rightPosition); box1->SetLocalRotation(0, 0, 0); box1->SetUserData("label", 1); + box1->SetLocalScale(unitBoxSize, unitBoxSize, unitBoxSize); root->AddChild(box1); // create box visual of different label @@ -131,9 +139,28 @@ void BuildScene(rendering::ScenePtr scene) box2->SetLocalPosition(middlePosition); box2->SetLocalRotation(0, 0, 0); box2->SetUserData("label", 2); + box2->SetLocalScale(unitBoxSize, unitBoxSize, unitBoxSize); root->AddChild(box2); + + // create box visual of the hidden box + ignition::rendering::VisualPtr hiddenBox = scene->CreateVisual(); + hiddenBox->AddGeometry(scene->CreateBox()); + hiddenBox->SetOrigin(0.0, 0.0, 0.0); + hiddenBox->SetLocalPosition(hiddenPosition); + hiddenBox->SetLocalRotation(0, 0, 0); + hiddenBox->SetUserData("label", hiddenLabel); + hiddenBox->SetLocalScale(unitBoxSize, unitBoxSize, unitBoxSize); + root->AddChild(hiddenBox); } +///////////////////////////////////////////////// +class SegmentationCameraSensorTest: public testing::Test, + public testing::WithParamInterface +{ + // Create a Segmentation Camera sensor from a SDF and gets a image message + public: void ImagesWithBuiltinSDF(const std::string &_renderEngine); +}; + ///////////////////////////////////////////////// void SegmentationCameraSensorTest::ImagesWithBuiltinSDF( const std::string &_renderEngine) @@ -158,6 +185,13 @@ void SegmentationCameraSensorTest::ImagesWithBuiltinSDF( int width = imagePtr->Get("width"); int height = imagePtr->Get("height"); + // If ogre2 is not the engine, don't run the test + if (_renderEngine.compare("ogre2") != 0) + { + igndbg << "Engine '" << _renderEngine + << "' doesn't support segmentation cameras" << std::endl; + return; + } // Setup ign-rendering with an empty scene auto *engine = ignition::rendering::engine(_renderEngine); if (!engine) @@ -181,17 +215,17 @@ void SegmentationCameraSensorTest::ImagesWithBuiltinSDF( ignition::sensors::SegmentationCameraSensor *sensor = mgr.CreateSensor(sdfSensor); - EXPECT_NE(sensor, nullptr); + ASSERT_NE(sensor, nullptr); sensor->SetScene(scene); - EXPECT_EQ(width, sensor->ImageWidth()); - EXPECT_EQ(height, sensor->ImageHeight()); + EXPECT_EQ(width, (int)sensor->ImageWidth()); + EXPECT_EQ(height, (int)sensor->ImageHeight()); auto camera = sensor->SegmentationCamera(); - EXPECT_NE(camera, nullptr); + ASSERT_NE(camera, nullptr); uint32_t backgroundLabel = 23; - camera->SetSegmentationType(SegmentationType::Semantic); + camera->SetSegmentationType(rendering::SegmentationType::Semantic); camera->EnableColoredMap(true); camera->SetBackgroundLabel(backgroundLabel); camera->SetLocalPosition(0.0, 0.0, 0.0); @@ -200,24 +234,25 @@ void SegmentationCameraSensorTest::ImagesWithBuiltinSDF( // Topic std::string topic = "/test/integration/SegmentationCameraPlugin_imagesWithBuiltinSDF"; + std::string infoTopic = topic + "/camera_info"; // Get the topic of the labels map topic += "/labels_map"; + WaitForMessageTestHelper helper(topic); + WaitForMessageTestHelper infoHelper(infoTopic); // Update once to create image mgr.RunOnce(std::chrono::steady_clock::duration::zero()); EXPECT_TRUE(helper.WaitForMessage()) << helper; + EXPECT_TRUE(infoHelper.WaitForMessage()) << infoHelper; // subscribe to the segmentation camera topic ignition::transport::Node node; node.Subscribe(topic, &OnNewSegmentationFrame); - // wait for a few segmentation camera frames - mgr.RunOnce(std::chrono::steady_clock::duration::zero(), true); - // wait for a new frame - WaitForNewFrame(); + WaitForNewFrame(mgr); // get the center of each box, the percentages locates the center math::Vector2d leftProj(width * 0.25, height * 0.5); @@ -225,64 +260,67 @@ void SegmentationCameraSensorTest::ImagesWithBuiltinSDF( math::Vector2d middleProj(width * 0.5, height * 0.5); // get their index in the buffer - uint32_t leftIndex = (leftProj.Y() * width + leftProj.X()) * 3; - uint32_t rightIndex = (rightProj.Y() * width + rightProj.X()) * 3; - uint32_t middleIndex = (middleProj.Y() * width + middleProj.X()) * 3; + const uint32_t channels = 3; + uint32_t leftIndex = (leftProj.Y() * width + leftProj.X()) * channels; + uint32_t rightIndex = (rightProj.Y() * width + rightProj.X()) * channels; + uint32_t middleIndex = (middleProj.Y() * width + middleProj.X()) * channels; // test - g_mutex.lock(); g_counter = 0; - // cast the unsigned char to unsigned int to read it - unsigned int leftLabel = g_buffer[leftIndex]; - unsigned int rightLabel = g_buffer[rightIndex]; - unsigned int middleLabel = g_buffer[middleIndex]; + uint8_t leftLabel = g_buffer[leftIndex]; + uint8_t rightLabel = g_buffer[rightIndex]; + uint8_t middleLabel = g_buffer[middleIndex]; // check the label - EXPECT_EQ(leftLabel , 1); + EXPECT_EQ(leftLabel, 1); EXPECT_EQ(middleLabel, 2); - EXPECT_EQ(rightLabel , 1); + EXPECT_EQ(rightLabel, 1); // check if the first pixel(background) = the background label - unsigned int background = g_buffer[0]; + uint8_t background = g_buffer[0]; EXPECT_EQ(background, backgroundLabel); - g_mutex.unlock(); + // search for the hidden box label in all pixels + for (int i = 0; i < height; i++) + { + for (int j = 0; j < width; j++) + { + uint32_t index = (i * width + j) * channels; + uint8_t label = g_buffer[index]; + EXPECT_NE(label, hiddenLabel); + } + } // Instance/Panoptic Segmentation Test - camera->SetSegmentationType(SegmentationType::Panoptic); + camera->SetSegmentationType(rendering::SegmentationType::Panoptic); - // wait for a few segmentation camera frames - mgr.RunOnce(std::chrono::steady_clock::duration::zero(), true); // wait for a new frame - WaitForNewFrame(); + WaitForNewFrame(mgr); - // the label in the last channel + // the label is in the last channel leftLabel = g_buffer[leftIndex + 2]; rightLabel = g_buffer[rightIndex + 2]; middleLabel = g_buffer[middleIndex + 2]; - // the instances count in the first channel - unsigned int leftCount = g_buffer[leftIndex]; - unsigned int rightCount = g_buffer[rightIndex]; - unsigned int middleCount = g_buffer[middleIndex]; + // the instance count is in the first channel + uint8_t leftCount = g_buffer[leftIndex]; + uint8_t rightCount = g_buffer[rightIndex]; + uint8_t middleCount = g_buffer[middleIndex]; // test - g_mutex.lock(); g_counter = 0; // check the label - EXPECT_EQ(leftLabel , 1); + EXPECT_EQ(leftLabel, 1); EXPECT_EQ(middleLabel, 2); - EXPECT_EQ(rightLabel , 1); + EXPECT_EQ(rightLabel, 1); // instance count EXPECT_EQ(middleCount, 1); EXPECT_EQ(rightCount, 1); EXPECT_EQ(leftCount, 2); - g_mutex.unlock(); - // Clean up engine->DestroyScene(scene); ignition::rendering::unloadEngine(engine->Name()); From 6589a829dda295a765dbd75829fd61443783efc9 Mon Sep 17 00:00:00 2001 From: AmrElsersy Date: Sat, 17 Jul 2021 05:35:10 +0200 Subject: [PATCH 07/22] Style + save both images + add background testing Signed-off-by: AmrElsersy --- .../sensors/SegmentationCameraSensor.hh | 13 +-- src/SegmentationCameraSensor.cc | 90 ++++++++++++------- .../integration/segmentation_camera_plugin.cc | 75 +++++++++------- 3 files changed, 108 insertions(+), 70 deletions(-) diff --git a/include/ignition/sensors/SegmentationCameraSensor.hh b/include/ignition/sensors/SegmentationCameraSensor.hh index 10230f99..3d30bd7c 100644 --- a/include/ignition/sensors/SegmentationCameraSensor.hh +++ b/include/ignition/sensors/SegmentationCameraSensor.hh @@ -25,13 +25,15 @@ #include #include #include +#include +#include +#include +#include #include "ignition/sensors/CameraSensor.hh" #include "ignition/sensors/Export.hh" -#include "ignition/transport/Node.hh" -#include "ignition/transport/Publisher.hh" #include "ignition/sensors/Sensor.hh" -#include "ignition/msgs.hh" -#include + +#include "ignition/sensors/segmentation_camera/Export.hh" namespace ignition { @@ -49,7 +51,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 SegmentationCameraSensor : public CameraSensor + class IGNITION_SENSORS_SEGMENTATION_CAMERA_VISIBLE + SegmentationCameraSensor : public CameraSensor { /// \brief constructor public: SegmentationCameraSensor(); diff --git a/src/SegmentationCameraSensor.cc b/src/SegmentationCameraSensor.cc index a445b007..24b94a5f 100644 --- a/src/SegmentationCameraSensor.cc +++ b/src/SegmentationCameraSensor.cc @@ -18,16 +18,17 @@ #include #include -#include "ignition/common/Console.hh" -#include "ignition/common/Profiler.hh" -#include "ignition/common/Image.hh" -#include "ignition/msgs.hh" -#include "ignition/rendering/SegmentationCamera.hh" +#include +#include +#include +#include +#include +#include +#include + #include "ignition/sensors/RenderingEvents.hh" -#include "ignition/sensors/SensorFactory.hh" #include "ignition/sensors/SegmentationCameraSensor.hh" -#include "ignition/transport/Node.hh" -#include "ignition/transport/Publisher.hh" +#include "ignition/sensors/SensorFactory.hh" using namespace ignition; using namespace sensors; @@ -35,15 +36,16 @@ using namespace sensors; class ignition::sensors::SegmentationCameraSensorPrivate { /// \brief Save an image - /// \param[in] _data the image data to be saved + /// \param[in] _coloredBuffer buffer of colored map + /// \param[in] _labelsBuffer buffer of labels map /// \param[in] _width width of image in pixels /// \param[in] _height height of image in pixels /// \return True if the image was saved successfully. False can mean /// that the path provided to the constructor does exist and creation /// of the path was not possible. /// \sa ImageSaver - public: bool SaveImage(const uint8_t *_data, unsigned int _width, - unsigned int _height); + public: bool SaveImage(const uint8_t *_coloredBuffer, + const uint8_t *_labelsBuffer, unsigned int _width, unsigned int _height); /// \brief SDF Sensor DOM Object public: sdf::Sensor sdfSensor; @@ -63,17 +65,17 @@ class ignition::sensors::SegmentationCameraSensorPrivate /// \brief Publisher to publish segmentation labels image public: transport::Node::Publisher labelsMapPublisher; - /// \brief Segmentation Image Msg + /// \brief Segmentation colored image message public: msgs::Image coloredMapMsg; - /// \brief Segmentation Image Msg + /// \brief Segmentation labels image message public: msgs::Image labelsMapMsg; - /// \brief Topic to publish the segmentation colored map - public: std::string topicColoredMap = "/colored_map"; + /// \brief Topic suffix to publish the segmentation colored map + public: const std::string topicColoredMapSuffix = "/colored_map"; - /// \brief Topic to publish the segmentation labels map - public: std::string topicLabelsMap = "/labels_map"; + /// \brief Topic suffix to publish the segmentation labels map + public: std::string topicLabelsMapSuffix = "/labels_map"; /// \brief Buffer contains the segmentation colored map data public: uint8_t *segmentationColoredBuffer {nullptr}; @@ -184,25 +186,38 @@ bool SegmentationCameraSensor::Load(const sdf::Sensor &_sdf) this->dataPtr->type = rendering::SegmentationType::Semantic; else if (type == "instance" || type == "panoptic") this->dataPtr->type = rendering::SegmentationType::Panoptic; + else + { + igndbg << "Wrong type {" << type << + "}, type should be semantic or instance or panoptic" << std::endl; + return false; + } } // Create the segmentation colored map image publisher this->dataPtr->coloredMapPublisher = this->dataPtr->node.Advertise( - this->Topic() + this->dataPtr->topicColoredMap); + this->Topic() + this->dataPtr->topicColoredMapSuffix); // Create the segmentation labels map image publisher this->dataPtr->labelsMapPublisher = this->dataPtr->node.Advertise( - this->Topic() + this->dataPtr->topicLabelsMap); + this->Topic() + this->dataPtr->topicLabelsMapSuffix); - if (!this->dataPtr->labelsMapPublisher || !this->dataPtr->coloredMapPublisher) + if (!this->dataPtr->labelsMapPublisher) + { + ignerr << "Unable to create publisher on topic[" + << this->Topic() + this->dataPtr->topicLabelsMapSuffix << "].\n"; + return false; + } + if (!this->dataPtr->coloredMapPublisher) { ignerr << "Unable to create publisher on topic[" - << this->Topic() << "].\n"; + << this->Topic() + this->dataPtr->topicColoredMapSuffix << "].\n"; return false; } + std::cout << this->InfoTopic() << std::endl; if (!this->AdvertiseInfo(this->Topic() + "/camera_info")) return false; @@ -355,11 +370,10 @@ bool SegmentationCameraSensor::Update( return false; } - // don't render if there is no subscribers + // don't render if there are no subscribers if (!this->dataPtr->coloredMapPublisher.HasConnections() && !this->dataPtr->labelsMapPublisher.HasConnections() && - this->dataPtr->imageEvent.ConnectionCount() <= 0u && - !this->dataPtr->saveImage) + this->dataPtr->imageEvent.ConnectionCount() <= 0u) { return false; } @@ -426,7 +440,7 @@ bool SegmentationCameraSensor::Update( // Save image if (this->dataPtr->saveImage) { - this->dataPtr->SaveImage( + this->dataPtr->SaveImage(this->dataPtr->segmentationLabelsBuffer, this->dataPtr->segmentationColoredBuffer, width, height); } @@ -458,7 +472,8 @@ common::ConnectionPtr SegmentationCameraSensor::ConnectImageCallback( ////////////////////////////////////////////////// bool SegmentationCameraSensorPrivate::SaveImage( - const uint8_t *_data, unsigned int _width, unsigned int _height) + const uint8_t *_coloredBuffer, const uint8_t *_labelsBuffer, + unsigned int _width, unsigned int _height) { // Attempt to create the directory if it doesn't exist if (!ignition::common::isDirectory(this->saveImagePath)) @@ -467,16 +482,29 @@ bool SegmentationCameraSensorPrivate::SaveImage( return false; } - std::string filename = this->saveImagePrefix + + std::string coloredName = this->saveImagePrefix + "labels_" + std::to_string(this->saveImageCounter) + ".png"; + std::string labelsName = this->saveImagePrefix + "colored_" + + std::to_string(this->saveImageCounter) + ".png"; + ++this->saveImageCounter; - ignition::common::Image localImage; - localImage.SetFromData(_data, _width, _height, + // save colored map + ignition::common::Image localColoredImage; + localColoredImage.SetFromData(_coloredBuffer, _width, _height, ignition::common::Image::RGB_INT8); - localImage.SavePNG( - ignition::common::joinPaths(this->saveImagePath, filename)); + localColoredImage.SavePNG( + ignition::common::joinPaths(this->saveImagePath, coloredName)); + + // save labels map + ignition::common::Image localLabelsImage; + localLabelsImage.SetFromData(_labelsBuffer, _width, _height, + ignition::common::Image::RGB_INT8); + + localLabelsImage.SavePNG( + ignition::common::joinPaths(this->saveImagePath, labelsName)); + return true; } diff --git a/test/integration/segmentation_camera_plugin.cc b/test/integration/segmentation_camera_plugin.cc index 8897fef9..f79f323a 100644 --- a/test/integration/segmentation_camera_plugin.cc +++ b/test/integration/segmentation_camera_plugin.cc @@ -55,10 +55,13 @@ std::mutex g_mutex; uint8_t *g_buffer = nullptr; /// \brief counter of received segmentation msgs -int g_counter = 0; +unsigned int g_counter = 0; /// \brief Label of the hidden box -uint8_t hiddenLabel = 4; +const uint8_t hiddenLabel = 4; +const uint8_t leftBoxLabel = 1; +const uint8_t rightBoxLabel = 1; +const uint8_t middleBoxLabel = 2; /// \brief callback to get the segmentation buffer void OnNewSegmentationFrame(const msgs::Image &_msg) @@ -79,15 +82,15 @@ void OnNewSegmentationFrame(const msgs::Image &_msg) /// \brief wait till you read the published frame void WaitForNewFrame(ignition::sensors::Manager &mgr) { + g_counter = 0; + // wait for a few segmentation camera frames mgr.RunOnce(std::chrono::steady_clock::duration::zero(), true); - g_counter = 0; - auto waitTime = std::chrono::duration_cast< std::chrono::milliseconds >( - std::chrono::duration< double >(0.001)); + std::chrono::duration< double >(0.001)); - int counter = 0; + unsigned int counter = 0; // wait for the counter to increase for (int sleep = 0; sleep < 300 && counter == 0; ++sleep) { @@ -96,21 +99,23 @@ void WaitForNewFrame(ignition::sensors::Manager &mgr) g_mutex.unlock(); std::this_thread::sleep_for(waitTime); } - EXPECT_GT(counter, 0); + EXPECT_GT(counter, 0u); } /// \brief Build the scene with 3 boxes besides each other -/// the 2 outer boxes have the same label & the middle is different +/// the 2 outer boxes have the same label & the middle is different. +/// There is also another box with a unique label that is hidden +/// behind the middle box void BuildScene(rendering::ScenePtr scene) { - math::Vector3d leftPosition(3, -1.5, 0); - math::Vector3d rightPosition(3, 1.5, 0); - math::Vector3d middlePosition(3, 0, 0); - math::Vector3d hiddenPosition(8, 0, 0); + const math::Vector3d leftPosition(3, -1.5, 0); + const math::Vector3d rightPosition(3, 1.5, 0); + const math::Vector3d middlePosition(3, 0, 0); + const math::Vector3d hiddenPosition(8, 0, 0); rendering::VisualPtr root = scene->RootVisual(); - double unitBoxSize = 1.0; + const double unitBoxSize = 1.0; // create box visual rendering::VisualPtr box = scene->CreateVisual(); @@ -118,7 +123,7 @@ void BuildScene(rendering::ScenePtr scene) box->SetOrigin(0.0, 0.0, 0.0); box->SetLocalPosition(leftPosition); box->SetLocalRotation(0, 0, 0); - box->SetUserData("label", 1); + box->SetUserData("label", leftBoxLabel); box->SetLocalScale(unitBoxSize, unitBoxSize, unitBoxSize); root->AddChild(box); @@ -128,7 +133,7 @@ void BuildScene(rendering::ScenePtr scene) box1->SetOrigin(0.0, 0.0, 0.0); box1->SetLocalPosition(rightPosition); box1->SetLocalRotation(0, 0, 0); - box1->SetUserData("label", 1); + box1->SetUserData("label", rightBoxLabel); box1->SetLocalScale(unitBoxSize, unitBoxSize, unitBoxSize); root->AddChild(box1); @@ -138,7 +143,7 @@ void BuildScene(rendering::ScenePtr scene) box2->SetOrigin(0.0, 0.0, 0.0); box2->SetLocalPosition(middlePosition); box2->SetLocalRotation(0, 0, 0); - box2->SetUserData("label", 2); + box2->SetUserData("label", middleBoxLabel); box2->SetLocalScale(unitBoxSize, unitBoxSize, unitBoxSize); root->AddChild(box2); @@ -224,6 +229,7 @@ void SegmentationCameraSensorTest::ImagesWithBuiltinSDF( auto camera = sensor->SegmentationCamera(); ASSERT_NE(camera, nullptr); + // 23 is a random number that is not used for boxes uint32_t backgroundLabel = 23; camera->SetSegmentationType(rendering::SegmentationType::Semantic); camera->EnableColoredMap(true); @@ -245,11 +251,10 @@ void SegmentationCameraSensorTest::ImagesWithBuiltinSDF( mgr.RunOnce(std::chrono::steady_clock::duration::zero()); EXPECT_TRUE(helper.WaitForMessage()) << helper; - EXPECT_TRUE(infoHelper.WaitForMessage()) << infoHelper; // subscribe to the segmentation camera topic ignition::transport::Node node; - node.Subscribe(topic, &OnNewSegmentationFrame); + EXPECT_TRUE(node.Subscribe(topic, &OnNewSegmentationFrame)); // wait for a new frame WaitForNewFrame(mgr); @@ -265,21 +270,26 @@ void SegmentationCameraSensorTest::ImagesWithBuiltinSDF( uint32_t rightIndex = (rightProj.Y() * width + rightProj.X()) * channels; uint32_t middleIndex = (middleProj.Y() * width + middleProj.X()) * channels; - // test - g_counter = 0; - uint8_t leftLabel = g_buffer[leftIndex]; uint8_t rightLabel = g_buffer[rightIndex]; uint8_t middleLabel = g_buffer[middleIndex]; // check the label - EXPECT_EQ(leftLabel, 1); - EXPECT_EQ(middleLabel, 2); - EXPECT_EQ(rightLabel, 1); + EXPECT_EQ(leftLabel, leftBoxLabel); + EXPECT_EQ(middleLabel, middleBoxLabel); + EXPECT_EQ(rightLabel, rightBoxLabel); + // check a pixel between 2 boxes & a pixel below a box + uint32_t betweenBoxesIndex = (120 * width + 230) * channels; + uint32_t belowBoxesIndex = (200 * width + 280) * channels; // check if the first pixel(background) = the background label uint8_t background = g_buffer[0]; + uint8_t betweenBoxes = g_buffer[betweenBoxesIndex]; + uint8_t belowBoxes = g_buffer[belowBoxesIndex]; + EXPECT_EQ(background, backgroundLabel); + EXPECT_EQ(betweenBoxes, backgroundLabel); + EXPECT_EQ(belowBoxes, backgroundLabel); // search for the hidden box label in all pixels for (int i = 0; i < height; i++) @@ -298,23 +308,20 @@ void SegmentationCameraSensorTest::ImagesWithBuiltinSDF( // wait for a new frame WaitForNewFrame(mgr); - // the label is in the last channel - leftLabel = g_buffer[leftIndex + 2]; - rightLabel = g_buffer[rightIndex + 2]; - middleLabel = g_buffer[middleIndex + 2]; + const int labelOffset = 2; + leftLabel = g_buffer[leftIndex + labelOffset]; + rightLabel = g_buffer[rightIndex + labelOffset]; + middleLabel = g_buffer[middleIndex + labelOffset]; // the instance count is in the first channel uint8_t leftCount = g_buffer[leftIndex]; uint8_t rightCount = g_buffer[rightIndex]; uint8_t middleCount = g_buffer[middleIndex]; - // test - g_counter = 0; - // check the label - EXPECT_EQ(leftLabel, 1); - EXPECT_EQ(middleLabel, 2); - EXPECT_EQ(rightLabel, 1); + EXPECT_EQ(leftLabel, leftBoxLabel); + EXPECT_EQ(middleLabel, middleBoxLabel); + EXPECT_EQ(rightLabel, rightBoxLabel); // instance count EXPECT_EQ(middleCount, 1); From 8a771167d2a042aa11326cfc4c78949936315781 Mon Sep 17 00:00:00 2001 From: AmrElsersy Date: Tue, 20 Jul 2021 13:55:13 +0200 Subject: [PATCH 08/22] style Signed-off-by: AmrElsersy --- .../sensors/SegmentationCameraSensor.hh | 3 ++- src/SegmentationCameraSensor.cc | 24 +++++++++---------- .../integration/segmentation_camera_plugin.cc | 8 +++---- 3 files changed, 18 insertions(+), 17 deletions(-) diff --git a/include/ignition/sensors/SegmentationCameraSensor.hh b/include/ignition/sensors/SegmentationCameraSensor.hh index 3d30bd7c..a294b150 100644 --- a/include/ignition/sensors/SegmentationCameraSensor.hh +++ b/include/ignition/sensors/SegmentationCameraSensor.hh @@ -25,10 +25,11 @@ #include #include #include +#include #include #include -#include #include + #include "ignition/sensors/CameraSensor.hh" #include "ignition/sensors/Export.hh" #include "ignition/sensors/Sensor.hh" diff --git a/src/SegmentationCameraSensor.cc b/src/SegmentationCameraSensor.cc index 24b94a5f..167deb68 100644 --- a/src/SegmentationCameraSensor.cc +++ b/src/SegmentationCameraSensor.cc @@ -19,8 +19,8 @@ #include #include -#include #include +#include #include #include #include @@ -75,7 +75,7 @@ class ignition::sensors::SegmentationCameraSensorPrivate public: const std::string topicColoredMapSuffix = "/colored_map"; /// \brief Topic suffix to publish the segmentation labels map - public: std::string topicLabelsMapSuffix = "/labels_map"; + public: const std::string topicLabelsMapSuffix = "/labels_map"; /// \brief Buffer contains the segmentation colored map data public: uint8_t *segmentationColoredBuffer {nullptr}; @@ -85,7 +85,7 @@ class ignition::sensors::SegmentationCameraSensorPrivate /// \brief Segmentation type (Semantic / Instance) public: rendering::SegmentationType type - {rendering::SegmentationType::Semantic}; + {rendering::SegmentationType::SEMANTIC}; /// \brief Connection to the new segmentation frames data public: common::ConnectionPtr newSegmentationConnection {nullptr}; @@ -183,9 +183,9 @@ bool SegmentationCameraSensor::Load(const sdf::Sensor &_sdf) }); if (type == "semantic") - this->dataPtr->type = rendering::SegmentationType::Semantic; + this->dataPtr->type = rendering::SegmentationType::SEMANTIC; else if (type == "instance" || type == "panoptic") - this->dataPtr->type = rendering::SegmentationType::Panoptic; + this->dataPtr->type = rendering::SegmentationType::PANOPTIC; else { igndbg << "Wrong type {" << type << @@ -206,18 +206,18 @@ bool SegmentationCameraSensor::Load(const sdf::Sensor &_sdf) if (!this->dataPtr->labelsMapPublisher) { - ignerr << "Unable to create publisher on topic[" + ignerr << "Unable to create publisher on topic [" << this->Topic() + this->dataPtr->topicLabelsMapSuffix << "].\n"; return false; } if (!this->dataPtr->coloredMapPublisher) { - ignerr << "Unable to create publisher on topic[" + ignerr << "Unable to create publisher on topic [" << this->Topic() + this->dataPtr->topicColoredMapSuffix << "].\n"; return false; } - std::cout << this->InfoTopic() << std::endl; + // TODO(anyone) Access the info topic from the parent class if (!this->AdvertiseInfo(this->Topic() + "/camera_info")) return false; @@ -440,8 +440,8 @@ bool SegmentationCameraSensor::Update( // Save image if (this->dataPtr->saveImage) { - this->dataPtr->SaveImage(this->dataPtr->segmentationLabelsBuffer, - this->dataPtr->segmentationColoredBuffer, width, height); + this->dataPtr->SaveImage(this->dataPtr->segmentationColoredBuffer, + this->dataPtr->segmentationLabelsBuffer, width, height); } return true; @@ -482,9 +482,9 @@ bool SegmentationCameraSensorPrivate::SaveImage( return false; } - std::string coloredName = this->saveImagePrefix + "labels_" + + std::string coloredName = this->saveImagePrefix + "colored_" + std::to_string(this->saveImageCounter) + ".png"; - std::string labelsName = this->saveImagePrefix + "colored_" + + std::string labelsName = this->saveImagePrefix + "labels_" + std::to_string(this->saveImageCounter) + ".png"; ++this->saveImageCounter; diff --git a/test/integration/segmentation_camera_plugin.cc b/test/integration/segmentation_camera_plugin.cc index f79f323a..97b3a080 100644 --- a/test/integration/segmentation_camera_plugin.cc +++ b/test/integration/segmentation_camera_plugin.cc @@ -231,7 +231,7 @@ void SegmentationCameraSensorTest::ImagesWithBuiltinSDF( // 23 is a random number that is not used for boxes uint32_t backgroundLabel = 23; - camera->SetSegmentationType(rendering::SegmentationType::Semantic); + camera->SetSegmentationType(rendering::SegmentationType::SEMANTIC); camera->EnableColoredMap(true); camera->SetBackgroundLabel(backgroundLabel); camera->SetLocalPosition(0.0, 0.0, 0.0); @@ -282,10 +282,10 @@ void SegmentationCameraSensorTest::ImagesWithBuiltinSDF( // check a pixel between 2 boxes & a pixel below a box uint32_t betweenBoxesIndex = (120 * width + 230) * channels; uint32_t belowBoxesIndex = (200 * width + 280) * channels; - // check if the first pixel(background) = the background label - uint8_t background = g_buffer[0]; uint8_t betweenBoxes = g_buffer[betweenBoxesIndex]; uint8_t belowBoxes = g_buffer[belowBoxesIndex]; + // check if the first pixel(background) = the background label + uint8_t background = g_buffer[0]; EXPECT_EQ(background, backgroundLabel); EXPECT_EQ(betweenBoxes, backgroundLabel); @@ -303,7 +303,7 @@ void SegmentationCameraSensorTest::ImagesWithBuiltinSDF( } // Instance/Panoptic Segmentation Test - camera->SetSegmentationType(rendering::SegmentationType::Panoptic); + camera->SetSegmentationType(rendering::SegmentationType::PANOPTIC); // wait for a new frame WaitForNewFrame(mgr); From 031179069829353b1a02b33de0853ea26809169b Mon Sep 17 00:00:00 2001 From: AmrElsersy Date: Fri, 6 Aug 2021 10:38:09 +0200 Subject: [PATCH 09/22] style Signed-off-by: AmrElsersy --- include/ignition/sensors/SegmentationCameraSensor.hh | 4 ++-- src/SegmentationCameraSensor.cc | 10 +++++----- test/integration/segmentation_camera_plugin.cc | 6 +++--- 3 files changed, 10 insertions(+), 10 deletions(-) diff --git a/include/ignition/sensors/SegmentationCameraSensor.hh b/include/ignition/sensors/SegmentationCameraSensor.hh index a294b150..de3eb83b 100644 --- a/include/ignition/sensors/SegmentationCameraSensor.hh +++ b/include/ignition/sensors/SegmentationCameraSensor.hh @@ -18,8 +18,8 @@ #ifndef IGNITION_SENSORS_SEGMENTATIONCAMERASENSOR_HH_ #define IGNITION_SENSORS_SEGMENTATIONCAMERASENSOR_HH_ -#include #include +#include #include #include @@ -83,7 +83,7 @@ namespace ignition /// \brief Get the rendering segmentation camera /// \return Segmentation camera pointer - public: virtual rendering::SegmentationCameraPtr SegmentationCamera(); + public: virtual rendering::SegmentationCameraPtr SegmentationCamera() const; /// \brief Segmentation data callback used to get the data from the sensor /// \param[in] _data pointer to the data from the sensor diff --git a/src/SegmentationCameraSensor.cc b/src/SegmentationCameraSensor.cc index 167deb68..99b89799 100644 --- a/src/SegmentationCameraSensor.cc +++ b/src/SegmentationCameraSensor.cc @@ -15,8 +15,8 @@ * */ -#include #include +#include #include #include @@ -41,9 +41,8 @@ class ignition::sensors::SegmentationCameraSensorPrivate /// \param[in] _width width of image in pixels /// \param[in] _height height of image in pixels /// \return True if the image was saved successfully. False can mean - /// that the path provided to the constructor does exist and creation + /// that the image save path does not exist and creation /// of the path was not possible. - /// \sa ImageSaver public: bool SaveImage(const uint8_t *_coloredBuffer, const uint8_t *_labelsBuffer, unsigned int _width, unsigned int _height); @@ -188,8 +187,8 @@ bool SegmentationCameraSensor::Load(const sdf::Sensor &_sdf) this->dataPtr->type = rendering::SegmentationType::PANOPTIC; else { - igndbg << "Wrong type {" << type << - "}, type should be semantic or instance or panoptic" << std::endl; + igndbg << "Wrong type [" << type << + "], type should be semantic or instance or panoptic" << std::endl; return false; } } @@ -327,6 +326,7 @@ bool SegmentationCameraSensor::CreateCamera() ///////////////////////////////////////////////// rendering::SegmentationCameraPtr SegmentationCameraSensor::SegmentationCamera() + const { return this->dataPtr->camera; } diff --git a/test/integration/segmentation_camera_plugin.cc b/test/integration/segmentation_camera_plugin.cc index 97b3a080..582feee2 100644 --- a/test/integration/segmentation_camera_plugin.cc +++ b/test/integration/segmentation_camera_plugin.cc @@ -57,7 +57,7 @@ uint8_t *g_buffer = nullptr; /// \brief counter of received segmentation msgs unsigned int g_counter = 0; -/// \brief Label of the hidden box +/// \brief Label of the boxes in the scene const uint8_t hiddenLabel = 4; const uint8_t leftBoxLabel = 1; const uint8_t rightBoxLabel = 1; @@ -223,8 +223,8 @@ void SegmentationCameraSensorTest::ImagesWithBuiltinSDF( ASSERT_NE(sensor, nullptr); sensor->SetScene(scene); - EXPECT_EQ(width, (int)sensor->ImageWidth()); - EXPECT_EQ(height, (int)sensor->ImageHeight()); + EXPECT_EQ(width, static_cast(sensor->ImageWidth())); + EXPECT_EQ(height, static_cast(sensor->ImageHeight())); auto camera = sensor->SegmentationCamera(); ASSERT_NE(camera, nullptr); From e2c5b52ed569d1cc2fafec4eba651e1befde7380 Mon Sep 17 00:00:00 2001 From: AmrElsersy Date: Fri, 6 Aug 2021 13:33:05 +0200 Subject: [PATCH 10/22] Add Tutorial Signed-off-by: AmrElsersy --- tutorials/05_segmentation_camera.md | 331 ++++++++++++++++++ .../segmentation_camera_panoptic.png | Bin 0 -> 94539 bytes .../segmentation_camera_semantic.png | Bin 0 -> 96368 bytes 3 files changed, 331 insertions(+) create mode 100644 tutorials/05_segmentation_camera.md create mode 100644 tutorials/files/segmentation_camera/segmentation_camera_panoptic.png create mode 100644 tutorials/files/segmentation_camera/segmentation_camera_semantic.png diff --git a/tutorials/05_segmentation_camera.md b/tutorials/05_segmentation_camera.md new file mode 100644 index 00000000..0209f49d --- /dev/null +++ b/tutorials/05_segmentation_camera.md @@ -0,0 +1,331 @@ +# Segmentation Camera in Ignition Gazebo +In this tutorial, we will discuss how to use a segmentation camera sensor in Ignition Gazebo. + +## Requirements + +Since this tutorial will show how to use a segmentation 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 segmentation 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 segmentation camera +Here's an example of how to attach a segmentation 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 + + + + + + instance + segmentation + + 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 segmentation camera sensor: + +```xml + + instance + segmentation + + 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 which will be used to publish the sensor data. + +
+There's also an optional plugin used here that allows for further configuration of the segmentation 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 segmentation, it could be semantic segmentation (where each pixel contains the label of the object) or panoptic segmentation (which is another format of instance segmentation where each pixel contains 3 values: one for the label of the object and two for the instance count of that label) + +`` available values: `semantic` for semantic segmentation and `panoptic` or `instance` for panoptic segmentation. + +Default value for the `` is `semantic` or semantic segmentation + + +#### Label map & Colored map +The segmentation sensor makes 2 types of maps: + +- The `label map` which is described above where each pixel contains the label value in semantic segmentation and (label & num of instances) in case of panoptic segmentation +- The `colored map` which is the colored version of the label map, in semantic segmentation each pixel contains the coresponding color to the label of the object, so all items of the same label will have the same color. And in panoptic segmentation, each pixel contains a unique color for each instance in the scene. + +## Assigning a label to a model +Only models with labels (annotated classes) will be visible by the segmentation 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 `