diff --git a/CMakeLists.txt b/CMakeLists.txt index ce3278fe..b1e5668f 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -66,7 +66,7 @@ if ((NOT ${CMAKE_VERSION} VERSION_LESS 3.11) AND (NOT OpenGL_GL_PREFERENCE)) set(OpenGL_GL_PREFERENCE "GLVND") endif() -ign_find_package(ignition-rendering6 REQUIRED OPTIONAL_COMPONENTS ogre ogre2) +ign_find_package(ignition-rendering6 REQUIRED VERSION 6.2 OPTIONAL_COMPONENTS ogre ogre2) set(IGN_RENDERING_VER ${ignition-rendering6_VERSION_MAJOR}) if (TARGET ignition-rendering${IGN_RENDERING_VER}::ogre) diff --git a/include/ignition/sensors/BrownDistortionModel.hh b/include/ignition/sensors/BrownDistortionModel.hh new file mode 100644 index 00000000..388c6d66 --- /dev/null +++ b/include/ignition/sensors/BrownDistortionModel.hh @@ -0,0 +1,87 @@ +/* + * Copyright (C) 2022 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_BROWNDISTORTIONMODEL_HH_ +#define IGNITION_SENSORS_BROWNDISTORTIONMODEL_HH_ + +#include + +#include "ignition/sensors/Distortion.hh" +#include "ignition/sensors/Export.hh" +#include "ignition/sensors/config.hh" +#include "ignition/utils/ImplPtr.hh" + +namespace ignition +{ + namespace sensors + { + // Inline bracket to help doxygen filtering. + inline namespace IGNITION_SENSORS_VERSION_NAMESPACE { + // + // Forward declarations + class BrownDistortionModelPrivate; + + /** \class BrownDistortionModel BrownDistortionModel.hh \ + ignition/sensors/BrownDistortionModel.hh + **/ + /// \brief Brown Distortion Model class + class IGNITION_SENSORS_VISIBLE BrownDistortionModel : public Distortion + { + /// \brief Constructor. + public: BrownDistortionModel(); + + /// \brief Destructor. + public: virtual ~BrownDistortionModel(); + + // Documentation inherited. + public: virtual void Load(const sdf::Camera &_sdf) override; + + /// \brief Get the radial distortion coefficient k1. + /// \return Distortion coefficient k1. + public: double K1() const; + + /// \brief Get the radial distortion coefficient k2. + /// \return Distortion coefficient k2. + public: double K2() const; + + /// \brief Get the radial distortion coefficient k3. + /// \return Distortion coefficient k3. + public: double K3() const; + + /// \brief Get the tangential distortion coefficient p1. + /// \return Distortion coefficient p1. + public: double P1() const; + + /// \brief Get the tangential distortion coefficient p2. + /// \return Distortion coefficient p2. + public: double P2() const; + + /// \brief Get the distortion center. + /// \return Distortion center. + public: math::Vector2d Center() const; + + /// Documentation inherited + public: virtual void Print(std::ostream &_out) const override; + + /// \brief Private data pointer. + IGN_UTILS_IMPL_PTR(dataPtr) + }; + } + } +} + +#endif diff --git a/include/ignition/sensors/Distortion.hh b/include/ignition/sensors/Distortion.hh new file mode 100644 index 00000000..852c4bc9 --- /dev/null +++ b/include/ignition/sensors/Distortion.hh @@ -0,0 +1,108 @@ +/* + * Copyright (C) 2022 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_DISTORTION_HH_ +#define IGNITION_SENSORS_DISTORTION_HH_ + +#include +#include +#include + +#include +#include +#include +#include + +#include + +namespace ignition +{ + namespace sensors + { + // Inline bracket to help doxygen filtering. + inline namespace IGNITION_SENSORS_VERSION_NAMESPACE { + // Forward declarations + class DistortionPrivate; + + /// \class DistortionFactory Distortion.hh ignition/sensors/Distortion.hh + /// \brief Use this distortion manager for creating and loading distortion + /// models. + class IGNITION_SENSORS_VISIBLE DistortionFactory + { + /// \brief Load a distortion model based on the input sdf parameters and + /// sensor type. + /// \param[in] _sdf Distortion sdf parameters. + /// \param[in] _sensorType Type of sensor. This is currently used to + /// distinguish between image and non image sensors in order to create + /// the appropriate distortion model. + /// \return Pointer to the distortion model created. + public: static DistortionPtr NewDistortionModel(sdf::ElementPtr _sdf, + const std::string &_sensorType = ""); + + /// \brief Load a distortion model based on the input sdf parameters and + /// sensor type. + /// \param[in] _sdf Distortion sdf parameters. + /// \param[in] _sensorType Type of sensor. This is currently used to + /// distinguish between image and non image sensors in order to create + /// the appropriate distortion model. + /// \return Pointer to the distortion model created. + public: static DistortionPtr NewDistortionModel( + const sdf::Camera &_sdf, + const std::string &_sensorType = ""); + }; + + /// \brief Which distortion types we support + enum class IGNITION_SENSORS_VISIBLE DistortionType : int + { + NONE = 0, + CUSTOM = 1, + BROWN = 2 + }; + + /// \class Distortion Distortion.hh ignition/sensors/Distortion.hh + /// \brief Distortion models for sensor output signals. + class IGNITION_SENSORS_VISIBLE Distortion + { + /// \brief Constructor. This should not be called directly unless creating + /// an empty distortion model. Use DistortionFactory::NewDistortionModel + /// to instantiate a new distortion model. + /// \param[in] _type Type of distortion model. + /// \sa DistortionFactory::NewDistortionModel + public: explicit Distortion(DistortionType _type); + + /// \brief Destructor. + public: virtual ~Distortion(); + + /// \brief Load distortion parameters from sdf. + /// \param[in] _sdf SDF Distortion DOM object. + public: virtual void Load(const sdf::Camera &_sdf); + + /// \brief Accessor for DistortionType. + /// \return Type of distortion currently in use. + public: DistortionType Type() const; + + /// \brief Output information about the distortion model. + /// \param[in] _out Output stream + public: virtual void Print(std::ostream &_out) const; + + /// \brief Private data pointer + IGN_UTILS_IMPL_PTR(dataPtr) + }; + } + } +} +#endif diff --git a/include/ignition/sensors/ImageBrownDistortionModel.hh b/include/ignition/sensors/ImageBrownDistortionModel.hh new file mode 100644 index 00000000..aee103c9 --- /dev/null +++ b/include/ignition/sensors/ImageBrownDistortionModel.hh @@ -0,0 +1,76 @@ +/* + * Copyright (C) 2022 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_IMAGEBROWNDISTORTIONMODEL_HH_ +#define IGNITION_SENSORS_IMAGEBROWNDISTORTIONMODEL_HH_ + +#include + +// TODO(WilliamLewww): Remove these pragmas once ign-rendering is disabling the +// warnings +#ifdef _WIN32 +#pragma warning(push) +#pragma warning(disable: 4251) +#endif +#include +#ifdef _WIN32 +#pragma warning(pop) +#endif + +#include "ignition/sensors/config.hh" +#include "ignition/sensors/BrownDistortionModel.hh" +#include "ignition/sensors/rendering/Export.hh" + +namespace ignition +{ + namespace sensors + { + // Inline bracket to help doxygen filtering. + inline namespace IGNITION_SENSORS_VERSION_NAMESPACE { + // Forward declarations + class ImageBrownDistortionModelPrivate; + + /** \class ImageBrownDistortionModel ImageBrownDistortionModel.hh \ + ignition/sensors/ImageBrownDistortionModel.hh + **/ + /// \brief Distortion Model class for image sensors + class IGNITION_SENSORS_RENDERING_VISIBLE ImageBrownDistortionModel : + public BrownDistortionModel + { + /// \brief Constructor. + public: ImageBrownDistortionModel(); + + /// \brief Destructor. + public: virtual ~ImageBrownDistortionModel(); + + // Documentation inherited. + public: virtual void Load(const sdf::Camera &_sdf) override; + + // Documentation inherited. + public: virtual void SetCamera(rendering::CameraPtr _camera); + + /// Documentation inherited + public: virtual void Print(std::ostream &_out) const override; + + /// \brief Private data pointer. + private: ImageBrownDistortionModelPrivate *dataPtr = nullptr; + }; + } + } +} + +#endif diff --git a/include/ignition/sensors/ImageDistortion.hh b/include/ignition/sensors/ImageDistortion.hh new file mode 100644 index 00000000..0e39b8c7 --- /dev/null +++ b/include/ignition/sensors/ImageDistortion.hh @@ -0,0 +1,67 @@ +/* + * Copyright (C) 2022 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_IMAGEDISTORTION_HH_ +#define IGNITION_SENSORS_IMAGEDISTORTION_HH_ + +#include + +#include + +#include "ignition/sensors/config.hh" +#include "ignition/sensors/SensorTypes.hh" +#include "ignition/sensors/rendering/Export.hh" + +namespace ignition +{ + namespace sensors + { + // Inline bracket to help doxygen filtering. + inline namespace IGNITION_SENSORS_VERSION_NAMESPACE { + // Forward declarations + class DistortionPrivate; + + /// \class ImageDistortionFactory ImageDistortion.hh + /// ignition/sensors/ImageDistortion.hh + /// \brief Use this distortion manager for creating and loading distortion + /// models. + class IGNITION_SENSORS_RENDERING_VISIBLE ImageDistortionFactory + { + /// \brief Load a distortion model based on the input sdf parameters and + /// sensor type. + /// \param[in] _sdf Distortion sdf parameters. + /// \param[in] _sensorType Type of sensor. This is currently used to + /// distinguish between image and non image sensors in order to create + /// the appropriate distortion model. + /// \return Pointer to the distortion model created. + public: static DistortionPtr NewDistortionModel(sdf::ElementPtr _sdf, + const std::string &_sensorType = ""); + + /// \brief Load a distortion model based on the input sdf parameters and + /// sensor type. + /// \param[in] _sdf Distortion sdf parameters. + /// \param[in] _sensorType Type of sensor. This is currently used to + /// distinguish between image and non image sensors in order to create + /// the appropriate distortion model. + /// \return Pointer to the distortion model created. + public: static DistortionPtr NewDistortionModel(const sdf::Camera &_sdf, + const std::string &_sensorType = ""); + }; + } + } +} +#endif diff --git a/include/ignition/sensors/Sensor.hh b/include/ignition/sensors/Sensor.hh index f004e0a8..bba3f0de 100644 --- a/include/ignition/sensors/Sensor.hh +++ b/include/ignition/sensors/Sensor.hh @@ -229,5 +229,4 @@ namespace ignition } } } - #endif diff --git a/include/ignition/sensors/SensorTypes.hh b/include/ignition/sensors/SensorTypes.hh index 63345045..0272df28 100644 --- a/include/ignition/sensors/SensorTypes.hh +++ b/include/ignition/sensors/SensorTypes.hh @@ -42,6 +42,9 @@ namespace ignition class GaussianNoiseModel; class ImageGaussianNoiseModel; class Noise; + class Distortion; + class BrownDistortionModel; + class ImageBrownDistortionModel; class Sensor; /// \def SensorPtr @@ -68,6 +71,19 @@ namespace ignition typedef std::shared_ptr ImageGaussianNoiseModelPtr; + /// \def DistortionPtr + /// \brief Shared pointer to Distortion + typedef std::shared_ptr DistortionPtr; + + /// \def DistortionPtr + /// \brief Shared pointer to Distortion + typedef std::shared_ptr BrownDistortionModelPtr; + + /// \def DistortionPtr + /// \brief Shared pointer to Distortion + typedef std::shared_ptr + ImageBrownDistortionModelPtr; + /// \def Sensor_V /// \brief Vector of Sensor shared pointers typedef std::vector Sensor_V; @@ -196,6 +212,31 @@ namespace ignition }; /// \} + + /// \def SensorDistortionType + /// \brief Eumeration of all sensor noise types + enum SensorDistortionType + { + /// \internal + /// \brief Indicator used to create an iterator over the enum. Do not + /// use this. + SENSOR_DISTORTION_TYPE_BEGIN = 0, + + /// \brief Noise streams for the Camera sensor + /// \sa CameraSensor + NO_DISTORTION = SENSOR_DISTORTION_TYPE_BEGIN, + + /// \brief Noise streams for the Camera sensor + /// \sa CameraSensor + CAMERA_DISTORTION = 1, + + /// \internal + /// \brief Indicator used to create an iterator over the enum. Do not + /// use this. + SENSOR_DISTORTION_TYPE_END + }; + /// \} + /// \brief SensorCategory is used to categorize sensors. This is used to /// put sensors into different threads. enum SensorCategory diff --git a/src/BrownDistortionModel.cc b/src/BrownDistortionModel.cc new file mode 100644 index 00000000..2ef6a879 --- /dev/null +++ b/src/BrownDistortionModel.cc @@ -0,0 +1,123 @@ +/* + * Copyright (C) 2022 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. + * +*/ +#ifdef _WIN32 + // Ensure that Winsock2.h is included before Windows.h, which can get + // pulled in by anybody (e.g., Boost). + #include +#endif + +#include "ignition/sensors/BrownDistortionModel.hh" + +#include +#include +#include + +using namespace ignition; +using namespace sensors; + +class ignition::sensors::BrownDistortionModel::Implementation +{ + /// \brief The radial distortion coefficient k1. + public: double k1 = 0.0; + + /// \brief The radial distortion coefficient k2. + public: double k2 = 0.0; + + /// \brief The radial distortion coefficient k3. + public: double k3 = 0.0; + + /// \brief The radial distortion coefficient p1. + public: double p1 = 0.0; + + /// \brief The radial distortion coefficient p2. + public: double p2 = 0.0; + + /// \brief The distortion center. + public: math::Vector2d lensCenter = {0.5, 0.5}; +}; + +////////////////////////////////////////////////// +BrownDistortionModel::BrownDistortionModel() + : Distortion(DistortionType::BROWN), + dataPtr(utils::MakeImpl()) +{ +} + +////////////////////////////////////////////////// +BrownDistortionModel::~BrownDistortionModel() +{ +} + +////////////////////////////////////////////////// +void BrownDistortionModel::Load(const sdf::Camera &_sdf) +{ + Distortion::Load(_sdf); + + this->dataPtr->k1 = _sdf.DistortionK1(); + this->dataPtr->k2 = _sdf.DistortionK2(); + this->dataPtr->k3 = _sdf.DistortionK3(); + this->dataPtr->p1 = _sdf.DistortionP1(); + this->dataPtr->p2 = _sdf.DistortionP2(); + this->dataPtr->lensCenter = _sdf.DistortionCenter(); +} + +////////////////////////////////////////////////// +double BrownDistortionModel::K1() const +{ + return this->dataPtr->k1; +} + +////////////////////////////////////////////////// +double BrownDistortionModel::K2() const +{ + return this->dataPtr->k2; +} + +////////////////////////////////////////////////// +double BrownDistortionModel::K3() const +{ + return this->dataPtr->k3; +} + +////////////////////////////////////////////////// +double BrownDistortionModel::P1() const +{ + return this->dataPtr->p1; +} + +////////////////////////////////////////////////// +double BrownDistortionModel::P2() const +{ + return this->dataPtr->p2; +} + +////////////////////////////////////////////////// +math::Vector2d BrownDistortionModel::Center() const +{ + return this->dataPtr->lensCenter; +} + +////////////////////////////////////////////////// +void BrownDistortionModel::Print(std::ostream &_out) const +{ + _out << "Distortion, k1[" << this->dataPtr->k1 << "], " + << "k2[" << this->dataPtr->k2 << "] " + << "k3[" << this->dataPtr->k2 << "] " + << "p1[" << this->dataPtr->p1 << "] " + << "p2[" << this->dataPtr->p2 << "] " + << "lensCenter[" << this->dataPtr->lensCenter << "]"; +} diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index 451d9bc2..76a20b9e 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -3,6 +3,8 @@ set (sources Sensor.cc Noise.cc GaussianNoiseModel.cc + Distortion.cc + BrownDistortionModel.cc PointCloudUtil.cc SensorFactory.cc SensorTypes.cc @@ -14,6 +16,8 @@ set(rendering_sources RenderingEvents.cc ImageGaussianNoiseModel.cc ImageNoise.cc + ImageBrownDistortionModel.cc + ImageDistortion.cc ) set (gtest_sources diff --git a/src/CameraSensor.cc b/src/CameraSensor.cc index a41040fd..5dbee6e0 100644 --- a/src/CameraSensor.cc +++ b/src/CameraSensor.cc @@ -36,6 +36,8 @@ #include #include "ignition/sensors/CameraSensor.hh" +#include "ignition/sensors/ImageBrownDistortionModel.hh" +#include "ignition/sensors/ImageDistortion.hh" #include "ignition/sensors/ImageGaussianNoiseModel.hh" #include "ignition/sensors/ImageNoise.hh" #include "ignition/sensors/Manager.hh" @@ -82,6 +84,9 @@ class ignition::sensors::CameraSensorPrivate /// \brief Noise added to sensor data public: std::map noises; + /// \brief Distortion added to sensor data + public: DistortionPtr distortion; + /// \brief Event that is used to trigger callbacks when a new image /// is generated public: ignition::common::EventT< @@ -181,9 +186,14 @@ bool CameraSensor::CreateCamera() this->dataPtr->camera->SetAspectRatio(static_cast(width)/height); this->dataPtr->camera->SetHFOV(angle); - // \todo(nkoenig) Port Distortion class - // This->dataPtr->distortion.reset(new Distortion()); - // This->dataPtr->distortion->Load(this->sdf->GetElement("distortion")); + if (cameraSdf->Element()->HasElement("distortion")) { + this->dataPtr->distortion = + ImageDistortionFactory::NewDistortionModel(*cameraSdf, "camera"); + this->dataPtr->distortion->Load(*cameraSdf); + + std::dynamic_pointer_cast( + this->dataPtr->distortion)->SetCamera(this->dataPtr->camera); + } sdf::PixelFormatType pixelFormat = cameraSdf->PixelFormat(); switch (pixelFormat) diff --git a/src/Distortion.cc b/src/Distortion.cc new file mode 100644 index 00000000..3e97eda4 --- /dev/null +++ b/src/Distortion.cc @@ -0,0 +1,132 @@ +/* + * Copyright (C) 2022 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. + * +*/ + +#ifdef _WIN32 + // Ensure that Winsock2.h is included before Windows.h, which can get + // pulled in by anybody (e.g., Boost). + #include +#endif + +#include + +#include + +#include "ignition/sensors/BrownDistortionModel.hh" +#include "ignition/sensors/Distortion.hh" + +using namespace ignition; +using namespace sensors; + +class ignition::sensors::Distortion::Implementation +{ + /// \brief Which type of distortion we're applying + public: DistortionType type = DistortionType::NONE; + + /// \brief Camera sdf element. + public: sdf::Camera cameraDom; +}; + +////////////////////////////////////////////////// +DistortionPtr DistortionFactory::NewDistortionModel(const sdf::Camera &_sdf, + const std::string &_sensorType) +{ + // TODO(WilliamLewww): support different distortion models + DistortionType distortionType = DistortionType::BROWN; + + DistortionPtr distortion; + + // Check for 'brown' distortion. + if (distortionType == DistortionType::BROWN) + { + if (_sensorType == "camera") + { + ignerr << "Image distortion requested. " + << "Please use ImageDistortionFactory::DistortionModel instead" + << std::endl; + return distortion; + } + else + distortion.reset(new BrownDistortionModel()); + + IGN_ASSERT(distortion->Type() == DistortionType::BROWN, + "Distortion type should be 'brown'"); + } + else if (distortionType == DistortionType::NONE) + { + // Return empty distortion if 'none' or 'custom' is specified. + // if 'custom', the type will be set once the user calls the + // SetCustomDistortionCallback function. + distortion.reset(new Distortion(DistortionType::NONE)); + IGN_ASSERT(distortion->Type() == DistortionType::NONE, + "Distortion type should be 'none'"); + } + else + { + ignerr << "Unrecognized distortion type" << std::endl; + return DistortionPtr(); + } + distortion->Load(_sdf); + + return distortion; +} + +////////////////////////////////////////////////// +DistortionPtr DistortionFactory::NewDistortionModel(sdf::ElementPtr _sdf, + const std::string &_sensorType) +{ + // TODO(WilliamLewww): create a distortion SDF to support different + // distortion models + IGN_ASSERT(_sdf != nullptr, "camera sdf is null"); + IGN_ASSERT(_sdf->GetName() == "camera", "Not a camera SDF element"); + sdf::Camera cameraDom; + cameraDom.Load(_sdf); + + return NewDistortionModel(cameraDom, _sensorType); +} + +////////////////////////////////////////////////// +Distortion::Distortion(DistortionType _type) + : dataPtr(utils::MakeImpl()) +{ + this->dataPtr->type = _type; +} + +////////////////////////////////////////////////// +Distortion::~Distortion() +{ +} + +////////////////////////////////////////////////// +void Distortion::Load(const sdf::Camera &_sdf) +{ + this->dataPtr->cameraDom = _sdf; +} + +////////////////////////////////////////////////// +DistortionType Distortion::Type() const +{ + return this->dataPtr->type; +} + +////////////////////////////////////////////////// +void Distortion::Print(std::ostream &_out) const +{ + _out << "Distortion with type[" << static_cast(this->dataPtr->type) + << "] " + << "does not have an overloaded Print function. " + << "No more information is available."; +} diff --git a/src/ImageBrownDistortionModel.cc b/src/ImageBrownDistortionModel.cc new file mode 100644 index 00000000..a7289ff8 --- /dev/null +++ b/src/ImageBrownDistortionModel.cc @@ -0,0 +1,132 @@ +/* + * Copyright (C) 2022 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. + * +*/ +#ifdef _WIN32 + // Ensure that Winsock2.h is included before Windows.h, which can get + // pulled in by anybody (e.g., Boost). + #include +#endif + +#include + +// TODO(WilliamLewww): Remove these pragmas once ign-rendering is disabling the +// warnings +#ifdef _WIN32 +#pragma warning(push) +#pragma warning(disable: 4251) +#endif +#include +#include +#include +#include +#ifdef _WIN32 +#pragma warning(pop) +#endif + +#include "ignition/sensors/ImageBrownDistortionModel.hh" + +using namespace ignition; +using namespace sensors; + +class ignition::sensors::ImageBrownDistortionModelPrivate +{ + /// \brief The radial distortion coefficient k1. + public: double k1 = 0.0; + + /// \brief The radial distortion coefficient k2. + public: double k2 = 0.0; + + /// \brief The radial distortion coefficient k3. + public: double k3 = 0.0; + + /// \brief The radial distortion coefficient p1. + public: double p1 = 0.0; + + /// \brief The radial distortion coefficient p2. + public: double p2 = 0.0; + + /// \brief The distortion center. + public: math::Vector2d lensCenter = {0.5, 0.5}; + + /// \brief The distortion pass. + public: rendering::DistortionPassPtr distortionPass; +}; + +////////////////////////////////////////////////// +ImageBrownDistortionModel::ImageBrownDistortionModel() + : BrownDistortionModel(), dataPtr(new ImageBrownDistortionModelPrivate()) +{ +} + +////////////////////////////////////////////////// +ImageBrownDistortionModel::~ImageBrownDistortionModel() +{ + delete this->dataPtr; + this->dataPtr = nullptr; +} + +////////////////////////////////////////////////// +void ImageBrownDistortionModel::Load(const sdf::Camera &_sdf) +{ + Distortion::Load(_sdf); + + this->dataPtr->k1 = _sdf.DistortionK1(); + this->dataPtr->k2 = _sdf.DistortionK2(); + this->dataPtr->k3 = _sdf.DistortionK3(); + this->dataPtr->p1 = _sdf.DistortionP1(); + this->dataPtr->p2 = _sdf.DistortionP2(); + this->dataPtr->lensCenter = _sdf.DistortionCenter(); +} + +////////////////////////////////////////////////// +void ImageBrownDistortionModel::SetCamera(rendering::CameraPtr _camera) +{ + if (!_camera) + { + ignerr << "Unable to apply distortion, camera is null\n"; + return; + } + + rendering::RenderEngine *engine = _camera->Scene()->Engine(); + rendering::RenderPassSystemPtr rpSystem = engine->RenderPassSystem(); + if (rpSystem) + { + // add distortion pass + rendering::RenderPassPtr distortionPass = + rpSystem->Create(); + this->dataPtr->distortionPass = + std::dynamic_pointer_cast(distortionPass); + this->dataPtr->distortionPass->SetK1(this->dataPtr->k1); + this->dataPtr->distortionPass->SetK2(this->dataPtr->k2); + this->dataPtr->distortionPass->SetK3(this->dataPtr->k3); + this->dataPtr->distortionPass->SetP1(this->dataPtr->p1); + this->dataPtr->distortionPass->SetP2(this->dataPtr->p2); + this->dataPtr->distortionPass->SetCenter(this->dataPtr->lensCenter); + this->dataPtr->distortionPass->SetEnabled(true); + _camera->AddRenderPass(this->dataPtr->distortionPass); + } +} + +////////////////////////////////////////////////// +void ImageBrownDistortionModel::Print(std::ostream &_out) const +{ + _out << "Image distortion, k1[" << this->dataPtr->k1 << "], " + << "k2[" << this->dataPtr->k2 << "] " + << "k3[" << this->dataPtr->k2 << "] " + << "p1[" << this->dataPtr->p1 << "] " + << "p2[" << this->dataPtr->p2 << "] " + << "lensCenter[" << this->dataPtr->lensCenter << "]"; +} diff --git a/src/ImageDistortion.cc b/src/ImageDistortion.cc new file mode 100644 index 00000000..aac3282d --- /dev/null +++ b/src/ImageDistortion.cc @@ -0,0 +1,85 @@ +/* + * Copyright (C) 2022 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. + * +*/ + +#ifdef _WIN32 + // Ensure that Winsock2.h is included before Windows.h, which can get + // pulled in by anybody (e.g., Boost). + #include +#endif + +#include "ignition/common/Console.hh" + +#include "ignition/sensors/ImageDistortion.hh" +#include "ignition/sensors/ImageBrownDistortionModel.hh" + +using namespace ignition; +using namespace sensors; + +////////////////////////////////////////////////// +DistortionPtr ImageDistortionFactory::NewDistortionModel( + const sdf::Camera &_sdf, + const std::string &_sensorType) +{ + DistortionType distortionType = DistortionType::BROWN; + + DistortionPtr distortion; + + // Check for 'brown' distortion. + if (distortionType == DistortionType::BROWN) + { + if (_sensorType == "camera") + { + distortion.reset(new ImageBrownDistortionModel()); + } + else + distortion.reset(new BrownDistortionModel()); + + IGN_ASSERT(distortion->Type() == DistortionType::BROWN, + "Distortion type should be 'brown'"); + } + else if (distortionType == DistortionType::NONE) + { + // Return empty distortion if 'none' or 'custom' is specified. + // if 'custom', the type will be set once the user calls the + // SetCustomDistortionCallback function. + distortion.reset(new Distortion(DistortionType::NONE)); + IGN_ASSERT(distortion->Type() == DistortionType::NONE, + "Distortion type should be 'none'"); + } + else + { + ignerr << "Unrecognized distortion type" << std::endl; + return DistortionPtr(); + } + distortion->Load(_sdf); + + return distortion; +} + +////////////////////////////////////////////////// +DistortionPtr ImageDistortionFactory::NewDistortionModel(sdf::ElementPtr _sdf, + const std::string &_sensorType) +{ + // TODO(WilliamLewww): create a distortion SDF to support different + // distortion models + IGN_ASSERT(_sdf != nullptr, "camera sdf is null"); + IGN_ASSERT(_sdf->GetName() == "camera", "Not a camera SDF element"); + sdf::Camera cameraDom; + cameraDom.Load(_sdf); + + return NewDistortionModel(cameraDom, _sensorType); +} diff --git a/test/integration/CMakeLists.txt b/test/integration/CMakeLists.txt index cfaeccc9..d456de87 100644 --- a/test/integration/CMakeLists.txt +++ b/test/integration/CMakeLists.txt @@ -3,6 +3,7 @@ set(TEST_TYPE "INTEGRATION") set(dri_tests camera.cc depth_camera.cc + distortion_camera.cc gpu_lidar_sensor.cc rgbd_camera.cc thermal_camera.cc diff --git a/test/integration/distortion_camera.cc b/test/integration/distortion_camera.cc new file mode 100644 index 00000000..1f8f3255 --- /dev/null +++ b/test/integration/distortion_camera.cc @@ -0,0 +1,153 @@ +/* + * Copyright (C) 2022 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 + +// TODO(WilliamLewww): Remove these pragmas once ign-rendering is disabling the +// warnings +#ifdef _WIN32 +#pragma warning(push) +#pragma warning(disable: 4251) +#endif +#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" + +class DistortionCameraSensorTest: public testing::Test, + public testing::WithParamInterface +{ + // Documentation inherited + protected: void SetUp() override + { + ignition::common::Console::SetVerbosity(4); + } + + // Create a Camera sensor from a SDF and gets a image message + public: void ImagesWithBuiltinSDF(const std::string &_renderEngine); +}; + +void DistortionCameraSensorTest::ImagesWithBuiltinSDF( + const std::string &_renderEngine) +{ + // get the darn test data + std::string path = ignition::common::joinPaths(PROJECT_SOURCE_PATH, "test", + "sdf", "distortion_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"); + + if (_renderEngine == "ogre2") + { + igndbg << "Distortion camera not supported yet in rendering engine: " + << _renderEngine << 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"); + + // do the test + ignition::sensors::Manager mgr; + + ignition::sensors::CameraSensor *sensor = + mgr.CreateSensor(sensorPtr); + ASSERT_NE(sensor, nullptr); + sensor->SetScene(scene); + + ASSERT_NE(sensor->RenderingCamera(), nullptr); + EXPECT_NE(sensor->Id(), sensor->RenderingCamera()->Id()); + EXPECT_EQ(256u, sensor->ImageWidth()); + EXPECT_EQ(257u, sensor->ImageHeight()); + + std::string topic = + "/test/integration/DistortionCameraPlugin_imagesWithBuiltinSDF"; + WaitForMessageTestHelper helper(topic); + + // Update once to create image + mgr.RunOnce(std::chrono::steady_clock::duration::zero()); + + EXPECT_TRUE(helper.WaitForMessage()) << helper; + + // test removing sensor + // first make sure the sensor objects do exist + auto sensorId = sensor->Id(); + auto cameraId = sensor->RenderingCamera()->Id(); + EXPECT_EQ(sensor, mgr.Sensor(sensorId)); + EXPECT_EQ(sensor->RenderingCamera(), scene->SensorById(cameraId)); + // remove and check sensor objects no longer exist in both sensors and + // rendering + EXPECT_TRUE(mgr.Remove(sensorId)); + EXPECT_EQ(nullptr, mgr.Sensor(sensorId)); + EXPECT_EQ(nullptr, scene->SensorById(cameraId)); + + // Clean up + engine->DestroyScene(scene); + ignition::rendering::unloadEngine(engine->Name()); +} + +////////////////////////////////////////////////// +TEST_P(DistortionCameraSensorTest, ImagesWithBuiltinSDF) +{ + ImagesWithBuiltinSDF(GetParam()); +} + +INSTANTIATE_TEST_CASE_P(DistortionCameraSensor, DistortionCameraSensorTest, + 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/distortion_camera_sensor_builtin.sdf b/test/sdf/distortion_camera_sensor_builtin.sdf new file mode 100644 index 00000000..41e847de --- /dev/null +++ b/test/sdf/distortion_camera_sensor_builtin.sdf @@ -0,0 +1,33 @@ + + + + + + 1 + + 0.927295218 + + 256 + 257 + R8G8B8 + + + 0.1 + 100 + + + -0.1349 + -0.51868 + -0.001 + 0 + 0 +
0.5 0.5
+ + + 1 + 30 + /test/integration/DistortionCameraPlugin_imagesWithBuiltinSDF + + + +