diff --git a/include/ignition/sensors/SegmentationCameraSensor.hh b/include/ignition/sensors/SegmentationCameraSensor.hh new file mode 100644 index 00000000..9e04bc7d --- /dev/null +++ b/include/ignition/sensors/SegmentationCameraSensor.hh @@ -0,0 +1,136 @@ +/* + * 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 +#include +#include + +#include "ignition/sensors/CameraSensor.hh" +#include "ignition/sensors/Export.hh" +#include "ignition/sensors/Sensor.hh" + +#include "ignition/sensors/segmentation_camera/Export.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 images from an ignition rendering scene. + /// The scene must be created in advance and given to Manager::Init(). + /// It offers both an ignition-transport interface and a direct C++ API + /// to access the image data. The API works by setting a callback to be + /// called with image data. + class IGNITION_SENSORS_SEGMENTATION_CAMERA_VISIBLE + 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 successful + public: virtual bool Update( + const std::chrono::steady_clock::duration &_now) override; + + /// \brief Get the rendering segmentation camera + /// \return Segmentation camera pointer + public: 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 + /// \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 * _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 + /// 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 _callback); + + /// \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 1eb3bb4a..263a1589 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -136,6 +136,16 @@ 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_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 new file mode 100644 index 00000000..2af0aee0 --- /dev/null +++ b/src/SegmentationCameraSensor.cc @@ -0,0 +1,620 @@ +/* + * Copyright (C) 2021 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * +*/ + +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +#include "ignition/sensors/RenderingEvents.hh" +#include "ignition/sensors/SegmentationCameraSensor.hh" +#include "ignition/sensors/SensorFactory.hh" + +using namespace ignition; +using namespace sensors; + +class ignition::sensors::SegmentationCameraSensorPrivate +{ + /// \brief Save a sample for the dataset (image & colored map & labels map) + /// \return True if the image was saved successfully. False can mean + /// that the image save path does not exist and creation + /// of the path was not possible. + public: bool SaveSample(); + + /// \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 Rendering RGB Camera to save rgb images for dataset generation + public: rendering::CameraPtr rgbCamera {nullptr}; + + /// \brief RGB Image to load the rgb camera data + public: rendering::Image image; + + /// \brief Node to create publisher + public: transport::Node node; + + /// \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 colored image message + public: msgs::Image coloredMapMsg; + + /// \brief Segmentation labels image message + public: msgs::Image labelsMapMsg; + + /// \brief Topic suffix to publish the segmentation colored map + public: const std::string topicColoredMapSuffix = "/colored_map"; + + /// \brief Topic suffix to publish the segmentation labels map + public: const std::string topicLabelsMapSuffix = "/labels_map"; + + /// \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 Buffer contains the image data to be saved + public: unsigned char *saveImageBuffer {nullptr}; + + /// \brief Segmentation type (Semantic / Instance) + public: rendering::SegmentationType type + {rendering::SegmentationType::ST_SEMANTIC}; + + /// \brief Connection to the new segmentation frames data + public: common::ConnectionPtr newSegmentationConnection {nullptr}; + + /// \brief Connection to the Manager's scene change event. + public: common::ConnectionPtr sceneChangeConnection {nullptr}; + + /// \brief Just a mutex for thread safety + public: std::mutex mutex; + + /// \brief True to save samples + public: bool saveSamples = false; + + /// \brief Folder to save the image + public: std::string saveImageFolder = "/images"; + + /// \brief Folder to save the segmentation colored maps + public: std::string saveColoredMapsFolder = "/colored_maps"; + + /// \brief Folder to save the segmentation labels maps + public: std::string saveLabelsMapsFolder = "/labels_maps"; + + /// \brief Path directory to where images are saved + public: std::string savePath = "./"; + + /// \brief Prefix of an image name + public: std::string saveImagePrefix = "./"; + + /// \brief Counter used to set the image filename + public: std::uint64_t saveCounter = 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; +}; + +////////////////////////////////////////////////// +SegmentationCameraSensor::SegmentationCameraSensor() + : CameraSensor(), dataPtr(new SegmentationCameraSensorPrivate) +{ +} + +///////////////////////////////////////////////// +SegmentationCameraSensor::~SegmentationCameraSensor() +{ + if (this->dataPtr->segmentationColoredBuffer) + { + delete [] this->dataPtr->segmentationColoredBuffer; + this->dataPtr->segmentationColoredBuffer = nullptr; + } + + if (this->dataPtr->segmentationLabelsBuffer) + { + delete [] this->dataPtr->segmentationLabelsBuffer; + this->dataPtr->segmentationLabelsBuffer = nullptr; + } +} + +///////////////////////////////////////////////// +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); + + 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 segmentation colored map image publisher + this->dataPtr->coloredMapPublisher = + this->dataPtr->node.Advertise( + this->Topic() + this->dataPtr->topicColoredMapSuffix); + + if (!this->dataPtr->coloredMapPublisher) + { + ignerr << "Unable to create publisher on topic [" + << this->Topic() << this->dataPtr->topicColoredMapSuffix << "].\n"; + return false; + } + + igndbg << "Colored map image for [" << this->Name() << "] advertised on [" + << this->Topic() << this->dataPtr->topicColoredMapSuffix << "]\n"; + + // Create the segmentation labels map image publisher + this->dataPtr->labelsMapPublisher = + this->dataPtr->node.Advertise( + this->Topic() + this->dataPtr->topicLabelsMapSuffix); + + if (!this->dataPtr->labelsMapPublisher) + { + ignerr << "Unable to create publisher on topic [" + << this->Topic() << this->dataPtr->topicLabelsMapSuffix << "].\n"; + return false; + } + + igndbg << "Segmentation labels map image for [" << this->Name() + << "] advertised on [" << this->Topic() + << this->dataPtr->topicLabelsMapSuffix << "]\n"; + + // TODO(anyone) Access the info topic from the parent class + if (!this->AdvertiseInfo(this->Topic() + "/camera_info")) + return false; + + if (this->Scene()) + { + if (!this->CreateCamera()) + { + ignerr << "Unable to create segmentation camera sensor\n"; + return false; + } + } + + 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); + + if (this->Scene() != _scene) + { + RenderingSensor::SetScene(_scene); + + if (this->dataPtr->initialized) + this->CreateCamera(); + } +} + +///////////////////////////////////////////////// +bool SegmentationCameraSensor::CreateCamera() +{ + const auto sdfCamera = this->dataPtr->sdfSensor.CameraSensor(); + if (!sdfCamera) + { + ignerr << "Unable to access camera SDF element\n"; + return false; + } + + // Segmentation type + if (sdfCamera->HasSegmentationType()) + { + auto type = sdfCamera->SegmentationType(); + + // 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::ST_SEMANTIC; + else if (type == "instance" || type == "panoptic") + this->dataPtr->type = rendering::SegmentationType::ST_PANOPTIC; + else + { + igndbg << "Wrong type [" << type << + "], type should be semantic or instance or panoptic" << std::endl; + return false; + } + } + + // Camera Info Msg + this->PopulateInfo(sdfCamera); + + // Save frames properties + if (sdfCamera->SaveFrames()) + { + this->dataPtr->savePath = sdfCamera->SaveFramesPath(); + this->dataPtr->saveImagePrefix = this->Name() + "_"; + this->dataPtr->saveSamples = true; + + // Folders paths + this->dataPtr->saveImageFolder = + this->dataPtr->savePath + this->dataPtr->saveImageFolder; + this->dataPtr->saveColoredMapsFolder = + this->dataPtr->savePath + this->dataPtr->saveColoredMapsFolder; + this->dataPtr->saveLabelsMapsFolder = + this->dataPtr->savePath + this->dataPtr->saveLabelsMapsFolder; + + // Set the save counter to be equal number of images in the folder + 1 + // to continue adding to the images in the folder (multi scene datasets) + if (ignition::common::isDirectory(this->dataPtr->saveImageFolder)) + { + common::DirIter endIter; + for (common::DirIter dirIter(this->dataPtr->saveImageFolder); + dirIter != endIter; ++dirIter) + { + this->dataPtr->saveCounter++; + } + } + } + + if (!this->dataPtr->camera) + { + // Create rendering camera + this->dataPtr->camera = this->Scene()->CreateSegmentationCamera( + this->Name()); + + if (this->dataPtr->saveSamples) + { + this->dataPtr->rgbCamera = this->Scene()->CreateCamera( + this->Name() + "_rgbCamera"); + } + } + + // Segmentation properties + this->dataPtr->camera->SetSegmentationType(this->dataPtr->type); + // 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(); + + math::Angle angle = sdfCamera->HorizontalFov(); + if (angle < 0.01 || angle > IGN_PI*2) + { + ignerr << "Invalid horizontal field of view [" << angle << "]\n"; + return false; + } + double aspectRatio = static_cast(width)/height; + + // Set segmentation 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->SetAspectRatio(aspectRatio); + this->dataPtr->camera->SetHFOV(angle); + + // Add the camera to the scene + this->Scene()->RootVisual()->AddChild(this->dataPtr->camera); + + // Add the rendering sensor to handle its render + this->AddSensor(this->dataPtr->camera); + + // Add the rgb camera only if we want to generate dataset / save samples + if (this->dataPtr->saveSamples) + { + // Set rgb camera properties + this->dataPtr->rgbCamera->SetImageFormat(rendering::PF_R8G8B8); + this->dataPtr->rgbCamera->SetImageWidth(width); + this->dataPtr->rgbCamera->SetImageHeight(height); + this->dataPtr->rgbCamera->SetVisibilityMask(sdfCamera->VisibilityMask()); + this->dataPtr->rgbCamera->SetNearClipPlane(sdfCamera->NearClip()); + this->dataPtr->rgbCamera->SetFarClipPlane(sdfCamera->FarClip()); + this->dataPtr->rgbCamera->SetAspectRatio(aspectRatio); + this->dataPtr->rgbCamera->SetHFOV(angle); + + // Add the rgb camera to the rendering pipeline + this->Scene()->RootVisual()->AddChild(this->dataPtr->rgbCamera); + this->AddSensor(this->dataPtr->rgbCamera); + this->dataPtr->image = this->dataPtr->rgbCamera->CreateImage(); + } + + // 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() + const +{ + 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->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); + + // Convert the colored map to labels map + this->dataPtr->camera->LabelMapFromColoredBuffer( + this->dataPtr->segmentationLabelsBuffer); +} + +////////////////////////////////////////////////// +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 are no subscribers nor saving + if (!this->dataPtr->coloredMapPublisher.HasConnections() && + !this->dataPtr->labelsMapPublisher.HasConnections() && + !this->dataPtr->saveSamples) + { + return false; + } + + if (this->dataPtr->saveSamples) + { + // The sensor updates only the segmentation camera with its pose + // as it has the same name, so make rgb camera with the same pose + this->dataPtr->rgbCamera->SetWorldPose( + this->dataPtr->camera->WorldPose()); + } + + // Actual render + this->Render(); + + if (this->dataPtr->saveSamples) + { + // Copy the rgb camera image data + this->dataPtr->rgbCamera->Copy(this->dataPtr->image); + this->dataPtr->saveImageBuffer = this->dataPtr->image.Data(); + } + + if (!this->dataPtr->segmentationColoredBuffer || + !this->dataPtr->segmentationLabelsBuffer) + return false; + + auto width = this->dataPtr->camera->ImageWidth(); + auto height = this->dataPtr->camera->ImageHeight(); + + // create colored map message + this->dataPtr->coloredMapMsg.set_width(width); + this->dataPtr->coloredMapMsg.set_height(height); + // format + this->dataPtr->coloredMapMsg.set_step( + width * rendering::PixelUtil::BytesPerPixel(rendering::PF_R8G8B8)); + this->dataPtr->coloredMapMsg.set_pixel_format_type( + msgs::PixelFormatType::RGB_INT8); + // time stamp + auto stamp = this->dataPtr->coloredMapMsg.mutable_header()->mutable_stamp(); + *stamp = msgs::Convert(_now); + auto frame = this->dataPtr->coloredMapMsg.mutable_header()->add_data(); + frame->set_key("frame_id"); + frame->add_value(this->Name()); + + 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 labels map data + this->dataPtr->labelsMapMsg.set_data(this->dataPtr->segmentationLabelsBuffer, + rendering::PixelUtil::MemorySize(rendering::PF_R8G8B8, + width, height)); + + // 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() > 0u) + { + try + { + this->dataPtr->imageEvent(this->dataPtr->coloredMapMsg); + } + catch(...) + { + ignerr << "Exception thrown in an image callback.\n"; + } + } + + // Save a sample (image & colored map & labels map) + if (this->dataPtr->saveSamples) + this->dataPtr->SaveSample(); + + return true; +} + +///////////////////////////////////////////////// +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(); +} + +///////////////////////////////////////////////// +common::ConnectionPtr SegmentationCameraSensor::ConnectImageCallback( + std::function _callback) +{ + return this->dataPtr->imageEvent.Connect(_callback); +} + +////////////////////////////////////////////////// +bool SegmentationCameraSensorPrivate::SaveSample() +{ + // Attempt to create the directories if they don't exist + if (!ignition::common::isDirectory(this->savePath)) + { + if (!ignition::common::createDirectories(this->savePath)) + return false; + } + if (!ignition::common::isDirectory(this->saveImageFolder)) + { + if (!ignition::common::createDirectories(this->saveImageFolder)) + return false; + } + if (!ignition::common::isDirectory(this->saveColoredMapsFolder)) + { + if (!ignition::common::createDirectories(this->saveColoredMapsFolder)) + return false; + } + if (!ignition::common::isDirectory(this->saveLabelsMapsFolder)) + { + if (!ignition::common::createDirectories(this->saveLabelsMapsFolder)) + return false; + } + + auto width = this->camera->ImageWidth(); + auto height = this->camera->ImageHeight(); + + // Save the images in format of 0000001, 0000002 .. etc + // Useful in sorting them in python + std::stringstream ss; + ss << std::setw(7) << std::setfill('0') << this->saveCounter; + std::string saveCounterString = ss.str(); + + std::string coloredName = "colored_" + saveCounterString + ".png"; + std::string labelsName = "labels_" + saveCounterString + ".png"; + std::string rgbImageName = "image_" + saveCounterString + ".png"; + + // Save rgb image + ignition::common::Image rgbImage; + rgbImage.SetFromData(this->saveImageBuffer, + width, height, ignition::common::Image::RGB_INT8); + + rgbImage.SavePNG( + ignition::common::joinPaths(this->saveImageFolder, rgbImageName)); + + // Save colored map + ignition::common::Image localColoredImage; + localColoredImage.SetFromData(this->segmentationColoredBuffer, + width, height, ignition::common::Image::RGB_INT8); + + localColoredImage.SavePNG( + ignition::common::joinPaths(this->saveColoredMapsFolder, coloredName)); + + // Save labels map + ignition::common::Image localLabelsImage; + localLabelsImage.SetFromData(this->segmentationLabelsBuffer, + width, height, ignition::common::Image::RGB_INT8); + + localLabelsImage.SavePNG( + ignition::common::joinPaths(this->saveLabelsMapsFolder, labelsName)); + + ++this->saveCounter; + return true; +} diff --git a/test/integration/CMakeLists.txt b/test/integration/CMakeLists.txt index 39ee1ef6..acca1b8c 100644 --- a/test/integration/CMakeLists.txt +++ b/test/integration/CMakeLists.txt @@ -6,6 +6,7 @@ set(dri_tests gpu_lidar_sensor.cc rgbd_camera.cc thermal_camera.cc + segmentation_camera.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.cc b/test/integration/segmentation_camera.cc new file mode 100644 index 00000000..2b7960d1 --- /dev/null +++ b/test/integration/segmentation_camera.cc @@ -0,0 +1,373 @@ +/* + * 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; + +/// \brief mutex for thread safety +std::mutex g_mutex; + +/// \brief Segmentation buffer +uint8_t *g_buffer = nullptr; + +/// \brief counter of received segmentation msgs +unsigned int g_counter = 0; + +/// \brief Label of the boxes in the scene +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) +{ + g_mutex.lock(); + + // the image has 3 channels + unsigned int size = _msg.width() * _msg.height() * 3; + + if (!g_buffer) + g_buffer = new uint8_t[size]; + + memcpy(g_buffer, _msg.data().c_str(), size); + g_counter++; + g_mutex.unlock(); +} + +/// \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); + + auto waitTime = std::chrono::duration_cast< std::chrono::milliseconds >( + std::chrono::duration< double >(0.001)); + + unsigned 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, 0u); + ASSERT_NE(nullptr, g_buffer); +} + +/// \brief Build the scene with 3 boxes besides each other +/// 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) +{ + 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(); + + const double unitBoxSize = 1.0; + + // create box visual + rendering::VisualPtr box = _scene->CreateVisual("box_left"); + ASSERT_NE(nullptr, box); + box->AddGeometry(_scene->CreateBox()); + box->SetOrigin(0.0, 0.0, 0.0); + box->SetLocalPosition(leftPosition); + box->SetLocalRotation(0, 0, 0); + box->SetUserData("label", leftBoxLabel); + box->SetLocalScale(unitBoxSize, unitBoxSize, unitBoxSize); + root->AddChild(box); + + // create box visual of same label + rendering::VisualPtr box1 = _scene->CreateVisual("box_right"); + ASSERT_NE(nullptr, box1); + box1->AddGeometry(_scene->CreateBox()); + box1->SetOrigin(0.0, 0.0, 0.0); + box1->SetLocalPosition(rightPosition); + box1->SetLocalRotation(0, 0, 0); + box1->SetUserData("label", rightBoxLabel); + box1->SetLocalScale(unitBoxSize, unitBoxSize, unitBoxSize); + root->AddChild(box1); + + // create box visual of different label + ignition::rendering::VisualPtr box2 = _scene->CreateVisual("box_mid"); + ASSERT_NE(nullptr, box2); + box2->AddGeometry(_scene->CreateBox()); + box2->SetOrigin(0.0, 0.0, 0.0); + box2->SetLocalPosition(middlePosition); + box2->SetLocalRotation(0, 0, 0); + box2->SetUserData("label", middleBoxLabel); + box2->SetLocalScale(unitBoxSize, unitBoxSize, unitBoxSize); + root->AddChild(box2); + + // create box visual of the hidden box + ignition::rendering::VisualPtr hiddenBox = _scene->CreateVisual("box_hidden"); + ASSERT_NE(nullptr, hiddenBox); + 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) +{ + std::string path = ignition::common::joinPaths(PROJECT_SOURCE_PATH, "test", + "sdf", "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"); + + // 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) + { + igndbg << "Engine '" << _renderEngine + << "' is not supported" << std::endl; + return; + } + + ignition::rendering::ScenePtr scene = engine->CreateScene("scene"); + ASSERT_NE(nullptr, 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); + + ASSERT_NE(sensor, nullptr); + sensor->SetScene(scene); + + EXPECT_EQ(width, static_cast(sensor->ImageWidth())); + EXPECT_EQ(height, static_cast(sensor->ImageHeight())); + + auto camera = sensor->SegmentationCamera(); + ASSERT_NE(camera, nullptr); + + camera->SetSegmentationType(rendering::SegmentationType::ST_SEMANTIC); + camera->EnableColoredMap(true); + camera->SetLocalPosition(0.0, 0.0, 0.0); + camera->SetLocalRotation(0.0, 0.0, 0.0); + + // 23 is a random number that is not used for boxes + uint32_t backgroundLabel = 23; + camera->SetBackgroundLabel(backgroundLabel); + + // Topic + std::string topic = + "/test/integration/SegmentationCameraPlugin_imagesWithBuiltinSDF"; + std::string infoTopic = topic + "/camera_info"; + // Get the topic of the labels map + // TODO(anyone) add test coverage for the colored map topic and its data + 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; + + // subscribe to the segmentation camera topic + ignition::transport::Node node; + EXPECT_TRUE(node.Subscribe(topic, &OnNewSegmentationFrame)); + + // wait for a new frame + WaitForNewFrame(mgr); + + // 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 + const uint32_t channels = 3; + uint32_t leftIndex = + static_cast((leftProj.Y() * width + leftProj.X()) * channels); + uint32_t rightIndex = + static_cast((rightProj.Y() * width + rightProj.X()) * channels); + uint32_t middleIndex = + static_cast((middleProj.Y() * width + middleProj.X()) * channels); + + uint8_t leftLabel = g_buffer[leftIndex]; + uint8_t rightLabel = g_buffer[rightIndex]; + uint8_t middleLabel = g_buffer[middleIndex]; + + // check the label + 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; + 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); + EXPECT_EQ(belowBoxes, backgroundLabel); + + // 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(rendering::SegmentationType::ST_PANOPTIC); + + // wait for a new frame + WaitForNewFrame(mgr); + + 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]; + + // check the label + EXPECT_EQ(leftLabel, leftBoxLabel); + EXPECT_EQ(middleLabel, middleBoxLabel); + EXPECT_EQ(rightLabel, rightBoxLabel); + + // instance count + EXPECT_EQ(middleCount, 1); + EXPECT_EQ(rightCount, 1); + EXPECT_EQ(leftCount, 2); + + // 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); + } + } + + // 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/sdf/segmentation_camera_sensor_builtin.sdf b/test/sdf/segmentation_camera_sensor_builtin.sdf new file mode 100644 index 00000000..b40d1505 --- /dev/null +++ b/test/sdf/segmentation_camera_sensor_builtin.sdf @@ -0,0 +1,22 @@ + + + + + + 10 + /test/integration/SegmentationCameraPlugin_imagesWithBuiltinSDF + + 1.05 + + 320 + 240 + + + 0.1 + 1000.0 + + + + + + diff --git a/tutorials.md.in b/tutorials.md.in index a9e4e9be..8d72c9e7 100644 --- a/tutorials.md.in +++ b/tutorials.md.in @@ -8,8 +8,9 @@ Ignition @IGN_DESIGNATION_CAP@ library and how to use the library effectively. 1. \subpage introduction "Introduction" 2. \subpage installation "Installation" -2. \subpage custom_sensors "Custom sensors": Creating your own sensors -3. \subpage thermalcameraigngazebo "Thermal Camera in Ignition Gazebo": Using a thermal camera in Ignition Gazebo to detect objects of specific temperatures in camera images. +3. \subpage custom_sensors "Custom sensors": Creating your own sensors +4. \subpage thermalcameraigngazebo "Thermal Camera in Ignition Gazebo": Using a thermal camera in Ignition Gazebo to detect objects of specific temperatures in camera images. +5. \subpage segmentationcamera_igngazebo "Segmentation Camera in Ignition Gazebo" ## License diff --git a/tutorials/files/segmentation_camera/cars_segmentation.png b/tutorials/files/segmentation_camera/cars_segmentation.png new file mode 100644 index 00000000..cb58579d Binary files /dev/null and b/tutorials/files/segmentation_camera/cars_segmentation.png differ diff --git a/tutorials/files/segmentation_camera/segmentation_dataset.gif b/tutorials/files/segmentation_camera/segmentation_dataset.gif new file mode 100644 index 00000000..ea5d9fda Binary files /dev/null and b/tutorials/files/segmentation_camera/segmentation_dataset.gif differ diff --git a/tutorials/files/segmentation_camera/segmentation_images.png b/tutorials/files/segmentation_camera/segmentation_images.png new file mode 100644 index 00000000..885d2c92 Binary files /dev/null and b/tutorials/files/segmentation_camera/segmentation_images.png differ diff --git a/tutorials/02_install.md b/tutorials/install.md similarity index 100% rename from tutorials/02_install.md rename to tutorials/install.md diff --git a/tutorials/01_intro.md b/tutorials/intro.md similarity index 100% rename from tutorials/01_intro.md rename to tutorials/intro.md diff --git a/tutorials/segmentation_camera.md b/tutorials/segmentation_camera.md new file mode 100644 index 00000000..d793d1e3 --- /dev/null +++ b/tutorials/segmentation_camera.md @@ -0,0 +1,460 @@ +\page segmentationcamera_igngazebo 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](https://ignitionrobotics.org/docs) 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 + + + + + + segmentation + + instance + 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 + + segmentation + + instance + 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 type of segmentation performed by the camera. Use `semantic` for [semantic segmentation](https://www.jeremyjordan.me/semantic-segmentation/). For [panoptic (instance) segmentation](https://hasty.ai/blog/panoptic-segmentation-explained/), use `panoptic` or `instance`. The default value for `` is `semantic`. + * ``: 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. + +#### Label map & Colored map +The segmentation sensor creates 2 maps (or images): + +- The `label map`: For semantic segmentation, each pixel contains the object's label. For panoptic segmentation, each pixel contains the label and object's instance count. +- The `colored map`: A colored version of the label map. In semantic segmentation, all items of the same label will have the same color. In panoptic segmentation, each pixel contains a unique color for each instance in the scene (so, for panoptic segmentation, items of the same label will not have the same color). + +## Assigning a label to a model +Only models with labels (annotated classes) will be visible by the segmentation camera sensor. Unlabeled models will be considered as 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 the plugin to the model's `` tag. +So, in this case, this model has a label of 10. + +You can also attach this plugin to the model's `` tag: + +```xml + + true + -1 -2 0.5 0 0 0 + + ... + + + + + +``` + +If you're including a model from a place like [ignition fuel](https://app.ignitionrobotics.org/dashboard), you can add the label plugin as a child for the `` tag: + +```xml + + -1 0 3 0.0 0.0 1.57 + + https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Construction Cone + + + + + +``` + +## Running an example: +Now that we've discussed how a segmentation camera and models with labels can be specified, let's run an example world that uses the segmentation camera. +Run the following command: +``` +ign gazebo segmentation_camera.sdf +``` + +You should see something similar to this: +@image html files/segmentation_camera/cars_segmentation.png width=850px + +There are 2 segmentation cameras in the SDF world: a semantic segmentation camera, and an instance/panoptic segmentation camera. + +For the instance/panoptic segmentation camera, colored map data is published to the `panoptic/colored_map topic`, and label map data is published to the `panoptic/labels_map` topic: + +```xml + + panoptic + + instance + 1.57 + + 800 + 600 + + + 0.1 + 100 + + + segmentation_data/instance_camera + + + 1 + 30 + true + +``` + +For the semantic segmentation camera, colored map data is published to the `semantic/colored_map` topic, and label map data is published to the `semantic/labels_map` topic: + +```xml + + semantic + + semantic + 1.57 + + 800 + 600 + + + 0.1 + 100 + + + segmentation_data/semantic_camera + + + 1 + 30 + true + +``` + +## Segmentation Dataset Generation +To save the output of the sensor as segmentation dataset samples, we add the `` tag to the `` tag, and we specify the path to save the dataset in. + +```xml + + ... + + ... + + DATSET_PATH + + + +``` + +In the example world we just ran (`segmentation_camera.sdf`), you'll notice that the panoptic camera is saving data to `segmentation_data/instance_camera`, while the semenatic camera is saving data to +`segmentation_data/semantic_camera` (these are relative paths). + +#### Dataset Demo + +Up to this point, we have left simulation paused. +Go ahead and start simulation by pressing the play button at the bottom-left part of the GUI. +You'll see the camera drop, capturing updated segmentation images along the way: + +@image html files/segmentation_camera/segmentation_dataset.gif + +Once the camera has reached the ground plane, you can go ahead and close Ignition Gazebo. +We will now discuss how to visualize the segmentation data that was just generated by Ignition Gazebo. + +## Visualize the segmentation dataset via Python +Put the following code in a Python script called `segmentation_visualizer.py`: + +```python +import cv2 +import argparse +import os +import numpy as np + +# Add the colored map to the image for visualization +def add_colored_to_image(image, colored): + return cv2.addWeighted(cv2.resize(image, (colored.shape[1], colored.shape[0])) + .astype(np.uint8), 1, + colored.astype(np.uint8), .5, + 0, cv2.CV_32F) + +# global labels map for mouse callback +labels_map = None + +# Callback when you click on any pixel in the labels_map image +# Prints the label and instance count of the clicked pixel +def mouse_callback(event, x, y, flags, param): + if event == cv2.EVENT_LBUTTONDOWN: + # instance / panoptic + # label id + label = labels_map[y,x,0] + + # instance count from the other 2 values of the pixel + instance_count = labels_map[y,x,1] * 256 + labels_map[y,x,2] + + print(f'label: {label} .. instance count: {instance_count}') + +cv2.namedWindow('labels_map') +cv2.setMouseCallback('labels_map', mouse_callback) + +# Arg Parser to set the dataset path +parser = argparse.ArgumentParser() +parser.add_argument('--path', type=str, required=True, help='Segmentation Dataset Path') +args = parser.parse_args() + +# dataset path +path = args.path + +# paths of images folders +images_path = os.path.join(path, "images") +labels_map_path = os.path.join(path, "labels_maps") +colored_map_path = os.path.join(path, "colored_maps") + +# list all images paths +images_names = sorted(os.listdir(images_path)) +labels_names = sorted(os.listdir(labels_map_path)) +colored_names = sorted(os.listdir(colored_map_path)) + +# add the root path to images names +images_paths = [os.path.join(images_path, name) for name in images_names] +labels_map_paths = [os.path.join(labels_map_path, name) for name in labels_names] +colored_map_paths = [os.path.join(colored_map_path, name) for name in colored_names] + +for image_path, labels_path, colored_path in zip(images_paths, labels_map_paths, colored_map_paths): + image = cv2.imread(image_path, cv2.IMREAD_COLOR) + labels_map = cv2.imread(labels_path, cv2.IMREAD_COLOR) + colored_map = cv2.imread(colored_path, cv2.IMREAD_COLOR) + + colored_image = add_colored_to_image(image, colored_map) + cv2.imshow("segmentation", colored_image) + + cv2.imshow("image", image) + cv2.imshow("labels_map", labels_map) + cv2.imshow("colored_map", colored_map) + + print("Press Any Key to Continue ... ESC to exit") + if cv2.waitKey(0) == 27: + break + +cv2.destroyAllWindows() +``` + +Make sure you have all of the dependencies that are needed in order to run this Python script: +* https://pypi.org/project/opencv-python/ +* https://numpy.org/install/ + +Run the script, setting the `--path` argument to what you specified for the `` tag in the SDF file. +Since we set the panoptic save path to `segmentation_data/instance_camera`, we'd run the following command to view the panoptic segmentation data: +``` +python3 segmentation_visualizer.py --path segmentation_data/instance_camera +``` + +You will see 4 windows: `image`, `labels_map`, `colored_map`, and `colored_image` (which is a combination of the image and colored_map). + +@image html files/segmentation_camera/segmentation_images.png width=900px + +For panoptic/instance segmentation, to parse the `labels_map`, click on any pixel on the `labels_map` window to see the `label` and `instance count` of that pixel. + + +## Processing the segmentation sensor via ign-transport +It's possible to process the segmentation data in real time via `ign-transport`. +You will need to which topics to subscribe to in order to receive this information. + +Consider the following SDF snippet from the segmentation camera: +```xml + segmentation +``` + +In this scenario, the sensor data will publish the label map data to `segmentation/labels_map`, and the colored map data to `segmentation/colored_map`. +We can write some c++ code that subscribes to these topics: + +```cpp +#include + +#include +#include +#include + +void OnNewLabelMap(const ignition::msgs::Image &_msg) +{ + auto width = _msg.width(); + auto height = _msg.height(); + auto buffer = _msg.data(); + + for (uint32_t i = 0; i < height; ++i) + { + for (uint32_t j = 0; j < width; ++j) + { + auto index = (i * width + j) * 3; + + // label id of the pixel is in the last channel + auto label = buffer[index + 2]; + + // In case of semantic segmentation, instance count is ignored. + // + // For panoptic segmentation, we can get the instance count as follows: + // 16 bit value for instance count + auto instanceCount1 = buffer[index + 1]; + auto instanceCount2 = buffer[index]; + // get the 16-bit decimal value from the two 8-bit values by left-shifting + // instanceCount1 by 8 bits + auto instanceCount = instanceCount1 * 256 + instanceCount2; + } + } +} + +void OnNewColoredMap(const ignition::msgs::Image &_msg) +{ + auto width = _msg.width(); + auto height = _msg.height(); + auto buffer = _msg.data(); + + for (uint32_t i = 0; i < height; ++i) + { + for (uint32_t j = 0; j < width; ++j) + { + auto index = (i * width + j) * 3; + auto r = buffer[index]; + auto g = buffer[index + 1]; + auto b = buffer[index + 2]; + } + } +} + +int main(int argc, char **argv) +{ + ignition::transport::Node node; + + if (!node.Subscribe("/segmentation/colored_map", &OnNewColoredMap) || + !node.Subscribe("/segmentation/labels_map", &OnNewLabelMap)) + { + std::cerr << "Error subscribing to the boundingbox camera topic" + << std::endl; + return -1; + } + + ignition::transport::waitForShutdown(); + return 0; +} +``` + +If you'd like to gain a better understanding of how the subscriber code works, +you can go through the [ign-transport tutorials](https://ignitionrobotics.org/api/transport/11.0/tutorials.html). diff --git a/tutorials/03_thermal_camera.md b/tutorials/thermal_camera.md similarity index 100% rename from tutorials/03_thermal_camera.md rename to tutorials/thermal_camera.md