Introduce CameraCalibrationRequestBus (#342)

* Add services for ROS2CameraSensorComponent
* Introduce CameraCalibrationRequestBus.
---------
Co-authored-by: Adam Krawczyk <[email protected]>
Signed-off-by: Michał Pełka <[email protected]>

Gems/ROS2/Code/Include/ROS2/Camera/CameraCalibrationRequestBus.h

@@ -0,0 +1,44 @@
+/*
+ * Copyright (c) Contributors to the Open 3D Engine Project.
+ * For complete copyright and license terms please see the LICENSE at the root of this distribution.
+ *
+ * SPDX-License-Identifier: Apache-2.0 OR MIT
+ *
+ */
+#pragma once
+
+#include <AzCore/Component/EntityId.h>
+#include <AzCore/EBus/EBus.h>
+#include <AzCore/Interface/Interface.h>
+#include <AzCore/Math/Matrix4x4.h>
+
+namespace ROS2
+{
+    //! Interface allows to obtain intrinsic parameters of the camera. To obtain extrinsic parameters use TransformProviderRequestBus.
+    class CameraCalibrationRequest : public AZ::EBusTraits
+    {
+    public:
+        using BusIdType = AZ::EntityId;
+        static constexpr AZ::EBusAddressPolicy AddressPolicy = AZ::EBusAddressPolicy::ById;
+
+        //! Returns the intrinsic calibration matrix of the camera as:
+        //!  [fx  0 cx]
+        //!  [ 0 fy cy]
+        //!  [ 0  0  1]
+        //! where:
+        //!  - fx, fy : the focal lengths in meters
+        //!  - cx, cy : principal point in pixels.
+        virtual AZ::Matrix3x3 GetCameraMatrix() const = 0;
+
+        //! Returns the width of the camera sensor in pixels
+        virtual int GetWidth() const = 0;
+
+        //! Returns the height of the camera sensor in pixels
+        virtual int GetHeight() const = 0;
+
+        //! Returns the vertical field of view of the camera in degrees
+        virtual float GetVerticalFOV() const = 0;
+    };
+
+    using CameraCalibrationRequestBus = AZ::EBus<CameraCalibrationRequest>;
+} // namespace ROS2

Gems/ROS2/Code/Source/Camera/CameraSensor.cpp

@@ -87,10 +87,16 @@ namespace ROS2
             cameraInfo.distortion_model = sensor_msgs::distortion_models::PLUMB_BOB;
 
             [[maybe_unused]] constexpr size_t expectedMatrixSize = 9;
-            AZ_Assert(cameraIntrinsics.size() == expectedMatrixSize, "camera matrix should have %d elements", expectedMatrixSize);
             AZ_Assert(cameraInfo.k.size() == expectedMatrixSize, "camera matrix should have %d elements", expectedMatrixSize);
-
-            AZStd::copy(cameraIntrinsics.begin(), cameraIntrinsics.end(), cameraInfo.k.begin());
+            cameraInfo.k = { cameraIntrinsics.GetElement(0, 0),
+                             0,
+                             cameraIntrinsics.GetElement(0, 2),
+                             0,
+                             cameraIntrinsics.GetElement(1, 1),
+                             cameraIntrinsics.GetElement(1, 2),
+                             0,
+                             0,
+                             1 };
             cameraInfo.p = { cameraInfo.k[0], cameraInfo.k[1], cameraInfo.k[2], 0, cameraInfo.k[3], cameraInfo.k[4], cameraInfo.k[5], 0,
                              cameraInfo.k[6], cameraInfo.k[7], cameraInfo.k[8], 0 };
             cameraInfo.header = header;

Gems/ROS2/Code/Source/Camera/CameraSensorDescription.cpp

@@ -7,6 +7,7 @@
  */
 
 #include "CameraSensorDescription.h"
+#include "CameraUtilities.h"
 #include <AzCore/Math/MatrixUtils.h>
 
 namespace ROS2
@@ -20,26 +21,14 @@ namespace ROS2
         , m_sensorConfiguration(sensorConfiguration)
         , m_cameraName(cameraName)
         , m_cameraNamespace(effectiveNamespace)
-        , m_viewToClipMatrix(MakeViewToClipMatrix())
-        , m_cameraIntrinsics(MakeCameraIntrinsics())
+        , m_viewToClipMatrix(CameraUtils::MakeClipMatrix(
+              m_cameraConfiguration.m_width, m_cameraConfiguration.m_height, m_cameraConfiguration.m_verticalFieldOfViewDeg))
+        , m_cameraIntrinsics(CameraUtils::MakeCameraIntrinsics(
+              m_cameraConfiguration.m_width, m_cameraConfiguration.m_height, m_cameraConfiguration.m_verticalFieldOfViewDeg))
     {
         ValidateParameters();
     }
 
-    float CameraSensorDescription::GetAspectRatio() const
-    {
-        return static_cast<float>(m_cameraConfiguration.m_width) / static_cast<float>(m_cameraConfiguration.m_height);
-    }
-
-    AZ::Matrix4x4 CameraSensorDescription::MakeViewToClipMatrix() const
-    {
-        const float nearDist = 0.1f, farDist = 100.0f;
-        AZ::Matrix4x4 localViewToClipMatrix;
-        AZ::MakePerspectiveFovMatrixRH(
-            localViewToClipMatrix, AZ::DegToRad(m_cameraConfiguration.m_verticalFieldOfViewDeg), GetAspectRatio(), nearDist, farDist, true);
-        return localViewToClipMatrix;
-    }
-
     void CameraSensorDescription::ValidateParameters() const
     {
         AZ_Assert(
@@ -50,24 +39,4 @@ namespace ROS2
         AZ_Assert(!m_cameraName.empty(), "Camera name cannot be empty");
     }
 
-    AZStd::array<double, 9> CameraSensorDescription::MakeCameraIntrinsics() const
-    {
-        /* Intrinsic camera matrix of the camera image is being created here
-           It is based on other parameters available in the structure - they must be initialized before this function is called
-            Matrix is row-major and has the following form:
-            [fx  0 cx]
-            [ 0 fy cy]
-            [ 0  0  1]
-           Projects 3D points in the camera coordinate frame to 2D pixel
-           coordinates using the focal lengths (fx, fy) and principal point
-           (cx, cy).
-       */
-        const auto w = static_cast<double>(m_cameraConfiguration.m_width);
-        const auto h = static_cast<double>(m_cameraConfiguration.m_height);
-        const double verticalFieldOfView = AZ::DegToRad(m_cameraConfiguration.m_verticalFieldOfViewDeg);
-        const double horizontalFoV = 2.0 * AZStd::atan(AZStd::tan(verticalFieldOfView / 2.0) * GetAspectRatio());
-        const double focalLengthX = w / (2.0 * AZStd::tan(horizontalFoV / 2.0));
-        const double focalLengthY = h / (2.0 * AZStd::tan(verticalFieldOfView / 2.0));
-        return { focalLengthX, 0.0, w / 2.0, 0.0, focalLengthY, h / 2.0, 0.0, 0.0, 1.0 };
-    }
 } // namespace ROS2

Gems/ROS2/Code/Source/Camera/CameraSensorDescription.h

@@ -37,22 +37,15 @@ namespace ROS2
             const CameraSensorConfiguration& configuration,
             const SensorConfiguration& sensorConfiguration);
 
-        //! Camera image aspect ratio.
-        //! @returns aspect ration, equal to (width / height).
-        float GetAspectRatio() const;
-
         const CameraSensorConfiguration m_cameraConfiguration; //!< Configuration of the camera.
         const SensorConfiguration m_sensorConfiguration; //!< Generic sensor configuration.
         const AZStd::string m_cameraName; //!< Camera name to differentiate cameras in a multi-camera setup.
         const AZStd::string m_cameraNamespace; //!< Effective camera namespace for frames and topics.
 
-
         const AZ::Matrix4x4 m_viewToClipMatrix; //!< Camera view to clip space transform matrix; derived from other parameters.
-        const AZStd::array<double, 9> m_cameraIntrinsics; //!< Camera intrinsics; derived from other parameters.
+        const AZ::Matrix3x3 m_cameraIntrinsics; //!< Camera intrinsics; derived from other parameters.
 
     private:
-        AZ::Matrix4x4 MakeViewToClipMatrix() const;
-        AZStd::array<double, 9> MakeCameraIntrinsics() const;
         void ValidateParameters() const;
     };
 } // namespace ROS2

Gems/ROS2/Code/Source/Camera/CameraUtilities.cpp

@@ -0,0 +1,44 @@
+/*
+ * Copyright (c) Contributors to the Open 3D Engine Project.
+ * For complete copyright and license terms please see the LICENSE at the root of this distribution.
+ *
+ * SPDX-License-Identifier: Apache-2.0 OR MIT
+ *
+ */
+#pragma once
+
+#include <AzCore/Math/Matrix3x3.h>
+#include <AzCore/Math/MatrixUtils.h>
+
+namespace ROS2::CameraUtils
+{
+    float GetAspectRatio(float width, float height)
+    {
+        return width / height;
+    };
+
+    AZ::Matrix3x3 MakeCameraIntrinsics(int width, int height, float verticalFieldOfViewDeg)
+    {
+        const auto w = static_cast<float>(width);
+        const auto h = static_cast<float>(height);
+        const float verticalFieldOfView = AZ::DegToRad(verticalFieldOfViewDeg);
+        const float horizontalFoV = 2.0 * AZStd::atan(AZStd::tan(verticalFieldOfView / 2.0) * GetAspectRatio(width, height));
+        const float focalLengthX = w / (2.0 * AZStd::tan(horizontalFoV / 2.0));
+        const float focalLengthY = h / (2.0 * AZStd::tan(verticalFieldOfView / 2.0));
+        return AZ::Matrix3x3::CreateFromRows({ focalLengthX, 0.f, w / 2.f }, { 0.f, focalLengthY, h / 2.f }, { 0.f, 0.f, 1.f });
+    }
+
+    AZ::Matrix4x4 MakeClipMatrix(int width, int height, float verticalFieldOfViewDeg, float nearDist, float farDist)
+    {
+        AZ::Matrix4x4 localViewToClipMatrix;
+        AZ::MakePerspectiveFovMatrixRH(
+            localViewToClipMatrix,
+            AZ::DegToRad(verticalFieldOfViewDeg),
+            CameraUtils::GetAspectRatio(width, height),
+            nearDist,
+            farDist,
+            true);
+        return localViewToClipMatrix;
+    }
+
+} // namespace ROS2::CameraUtils

Gems/ROS2/Code/Source/Camera/CameraUtilities.h

@@ -0,0 +1,36 @@
+/*
+ * Copyright (c) Contributors to the Open 3D Engine Project.
+ * For complete copyright and license terms please see the LICENSE at the root of this distribution.
+ *
+ * SPDX-License-Identifier: Apache-2.0 OR MIT
+ *
+ */
+#pragma once
+
+#include <AzCore/Math/Matrix3x3.h>
+
+//! Namespace contains utility functions for camera.
+namespace ROS2::CameraUtils
+{
+    //! Function computes aspect ratio of the image.
+    //! @param width Width of the image in pixels
+    //! @param height Height of the image in pixels.
+    //! @return Aspect ratio of the image.
+    float GetAspectRatio(float width, float height);
+
+    //! Function computes 3x3 projection matrix (pinhole model) from camera config.
+    //! @param height Height of the image in pixels.
+    //! @param width Width of the image in pixels
+    //! @param verticalFieldOfViewDeg  Vertical field of view of the camera in degrees.
+    //! @return projection matrix for computer vision applications.
+    AZ::Matrix3x3 MakeCameraIntrinsics(int width, int height, float verticalFieldOfViewDeg);
+
+    //! Function computes 4x4 projection matrix (frustum model) from camera config.
+    //! @param height Height of the image in pixels.
+    //! @param height Height of the image in pixels.
+    //! @param verticalFieldOfViewDeg Vertical field of view of the camera in degrees.
+    //! @param farDist Far clipping plane distance in meters.
+    //! @param nearDist Near clipping plane distance in meters.
+    //! @return projection matrix for the rendering.
+    AZ::Matrix4x4 MakeClipMatrix(int width, int height, float verticalFieldOfViewDeg, float nearDist = 0.1f, float farDist = 100.0f);
+} // namespace ROS2::CameraUtils

Gems/ROS2/Code/Source/Camera/ROS2CameraSensorComponent.cpp

@@ -56,12 +56,50 @@ namespace ROS2
         const auto* component = Utils::GetGameOrEditorComponent<ROS2FrameComponent>(GetEntity());
         AZ_Assert(component, "Entity has no ROS2FrameComponent");
         m_frameName = component->GetFrameID();
+        ROS2::CameraCalibrationRequestBus::Handler::BusConnect(GetEntityId());
     }
 
     void ROS2CameraSensorComponent::Deactivate()
     {
         m_cameraSensor.reset();
         ROS2SensorComponent::Deactivate();
+        ROS2::CameraCalibrationRequestBus::Handler::BusDisconnect(GetEntityId());
+    }
+
+    AZ::Matrix3x3 ROS2CameraSensorComponent::GetCameraMatrix() const
+    {
+        return CameraUtils::MakeCameraIntrinsics(
+            m_cameraConfiguration.m_width, m_cameraConfiguration.m_height, m_cameraConfiguration.m_verticalFieldOfViewDeg);
+    };
+
+    int ROS2CameraSensorComponent::GetWidth() const
+    {
+        return m_cameraConfiguration.m_width;
+    };
+
+    int ROS2CameraSensorComponent::GetHeight() const
+    {
+        return m_cameraConfiguration.m_height;
+    };
+
+    float ROS2CameraSensorComponent::GetVerticalFOV() const
+    {
+        return m_cameraConfiguration.m_verticalFieldOfViewDeg;
+    };
+
+    void ROS2CameraSensorComponent::GetRequiredServices(AZ::ComponentDescriptor::DependencyArrayType& required)
+    {
+        required.push_back(AZ_CRC("ROS2Frame"));
+    }
+
+    void ROS2CameraSensorComponent::GetIncompatibleServices(AZ::ComponentDescriptor::DependencyArrayType& incompatible)
+    {
+        incompatible.push_back(AZ_CRC_CE("ROS2CameraSensor"));
+    }
+
+    void ROS2CameraSensorComponent::GetProvidedServices(AZ::ComponentDescriptor::DependencyArrayType& provided)
+    {
+        provided.push_back(AZ_CRC_CE("ROS2CameraSensor"));
     }
 
     void ROS2CameraSensorComponent::FrequencyTick()

Gems/ROS2/Code/Source/Camera/ROS2CameraSensorComponent.h

@@ -16,6 +16,7 @@
 
 #include "CameraSensor.h"
 #include "CameraSensorConfiguration.h"
+#include <ROS2/Camera/CameraCalibrationRequestBus.h>
 #include <ROS2/ROS2Bus.h>
 #include <ROS2/Sensor/ROS2SensorComponent.h>
 
@@ -28,19 +29,30 @@ namespace ROS2
     //!   - camera image width and height in pixels
     //!   - camera vertical field of view in degrees
     //! Camera frustum is facing negative Z axis; image plane is parallel to X,Y plane: X - right, Y - up
-    class ROS2CameraSensorComponent : public ROS2SensorComponent
+    class ROS2CameraSensorComponent
+        : public ROS2SensorComponent
+        , public CameraCalibrationRequestBus::Handler
     {
     public:
         ROS2CameraSensorComponent() = default;
         ROS2CameraSensorComponent(const SensorConfiguration& sensorConfiguration, const CameraSensorConfiguration& cameraConfiguration);
-
+        static void GetRequiredServices(AZ::ComponentDescriptor::DependencyArrayType& required);
+        static void GetIncompatibleServices(AZ::ComponentDescriptor::DependencyArrayType& incompatible);
+        static void GetProvidedServices(AZ::ComponentDescriptor::DependencyArrayType& provided);
         ~ROS2CameraSensorComponent() override = default;
         AZ_COMPONENT(ROS2CameraSensorComponent, "{3C6B8AE6-9721-4639-B8F9-D8D28FD7A071}", ROS2SensorComponent);
         static void Reflect(AZ::ReflectContext* context);
 
+        // AzToolsFramework::Components::EditorComponentBase overrides ..
         void Activate() override;
         void Deactivate() override;
 
+        // CameraCalibrationRequestBus::Handler overrides ...
+        AZ::Matrix3x3 GetCameraMatrix() const override;
+        int GetWidth() const override;
+        int GetHeight() const override;
+        float GetVerticalFOV() const override;
+
     private:
         //! Helper that adds an image source.
         //! @tparam CameraType type of camera sensor (eg 'CameraColorSensor')

Gems/ROS2/Code/Source/Camera/ROS2CameraSensorEditorComponent.cpp

@@ -6,8 +6,9 @@
  *
  */
 
-#include "CameraConstants.h"
 #include "ROS2CameraSensorEditorComponent.h"
+#include "CameraConstants.h"
+#include "CameraUtilities.h"
 #include "ROS2CameraSensorComponent.h"
 #include <AzCore/Component/TransformBus.h>
 #include <ROS2/Frame/ROS2FrameComponent.h>
@@ -62,12 +63,14 @@ namespace ROS2
     {
         AzFramework::EntityDebugDisplayEventBus::Handler::BusConnect(this->GetEntityId());
         AzToolsFramework::Components::EditorComponentBase::Activate();
+        ROS2::CameraCalibrationRequestBus::Handler::BusConnect(GetEntityId());
     }
 
     void ROS2CameraSensorEditorComponent::Deactivate()
     {
         AzToolsFramework::Components::EditorComponentBase::Deactivate();
         AzFramework::EntityDebugDisplayEventBus::Handler::BusDisconnect();
+        ROS2::CameraCalibrationRequestBus::Handler::BusDisconnect(GetEntityId());
     }
 
     void ROS2CameraSensorEditorComponent::GetRequiredServices(AZ::ComponentDescriptor::DependencyArrayType& required)
@@ -80,9 +83,9 @@ namespace ROS2
         incompatible.push_back(AZ_CRC_CE("ROS2CameraSensor"));
     }
 
-    void ROS2CameraSensorEditorComponent::GetProvidedServices(AZ::ComponentDescriptor::DependencyArrayType& required)
+    void ROS2CameraSensorEditorComponent::GetProvidedServices(AZ::ComponentDescriptor::DependencyArrayType& provided)
     {
-        required.push_back(AZ_CRC_CE("ROS2CameraSensor"));
+        provided.push_back(AZ_CRC_CE("ROS2CameraSensor"));
     }
 
     void ROS2CameraSensorEditorComponent::BuildGameEntity(AZ::Entity* gameEntity)
@@ -90,6 +93,29 @@ namespace ROS2
         gameEntity->CreateComponent<ROS2::ROS2CameraSensorComponent>(m_sensorConfiguration, m_cameraSensorConfiguration);
     }
 
+    AZ::Matrix3x3 ROS2CameraSensorEditorComponent::GetCameraMatrix() const
+    {
+        return CameraUtils::MakeCameraIntrinsics(
+            m_cameraSensorConfiguration.m_width,
+            m_cameraSensorConfiguration.m_height,
+            m_cameraSensorConfiguration.m_verticalFieldOfViewDeg);
+    };
+
+    int ROS2CameraSensorEditorComponent::GetWidth() const
+    {
+        return m_cameraSensorConfiguration.m_width;
+    };
+
+    int ROS2CameraSensorEditorComponent::GetHeight() const
+    {
+        return m_cameraSensorConfiguration.m_height;
+    };
+
+    float ROS2CameraSensorEditorComponent::GetVerticalFOV() const
+    {
+        return m_cameraSensorConfiguration.m_verticalFieldOfViewDeg;
+    };
+
     void ROS2CameraSensorEditorComponent::DisplayEntityViewport(
         [[maybe_unused]] const AzFramework::ViewportInfo& viewportInfo, AzFramework::DebugDisplayRequests& debugDisplay)
     {

Gems/ROS2/Code/Source/Camera/ROS2CameraSensorEditorComponent.h

@@ -13,6 +13,7 @@
 #include <AzToolsFramework/ToolsComponents/EditorComponentBase.h>
 
 #include "CameraSensorConfiguration.h"
+#include <ROS2/Camera/CameraCalibrationRequestBus.h>
 #include <ROS2/Frame/NamespaceConfiguration.h>
 #include <ROS2/Frame/ROS2Transform.h>
 #include <ROS2/Sensor/SensorConfiguration.h>
@@ -24,6 +25,7 @@ namespace ROS2
     //! Component draws camera frustrum in the Editor
     class ROS2CameraSensorEditorComponent
         : public AzToolsFramework::Components::EditorComponentBase
+        , public CameraCalibrationRequestBus::Handler
         , protected AzFramework::EntityDebugDisplayEventBus::Handler
     {
     public:
@@ -33,7 +35,7 @@ namespace ROS2
         static void Reflect(AZ::ReflectContext* context);
         static void GetRequiredServices(AZ::ComponentDescriptor::DependencyArrayType& required);
         static void GetIncompatibleServices(AZ::ComponentDescriptor::DependencyArrayType& incompatible);
-        static void GetProvidedServices(AZ::ComponentDescriptor::DependencyArrayType& required);
+        static void GetProvidedServices(AZ::ComponentDescriptor::DependencyArrayType& provided);
 
         void Activate() override;
         void Deactivate() override;
@@ -41,6 +43,12 @@ namespace ROS2
         // AzToolsFramework::Components::EditorComponentBase overrides
         void BuildGameEntity(AZ::Entity* gameEntity) override;
 
+        // CameraCalibrationRequestBus::Handler overrides
+        AZ::Matrix3x3 GetCameraMatrix() const override;
+        int GetWidth() const override;
+        int GetHeight() const override;
+        float GetVerticalFOV() const override;
+
     private:
         // EntityDebugDisplayEventBus::Handler overrides
         void DisplayEntityViewport(const AzFramework::ViewportInfo& viewportInfo, AzFramework::DebugDisplayRequests& debugDisplay) override;

Gems/ROS2/Code/ros2_files.cmake

@@ -20,6 +20,8 @@ set(FILES
         Source/Camera/CameraSensorConfiguration.h
         Source/Camera/ROS2CameraSensorComponent.cpp
         Source/Camera/ROS2CameraSensorComponent.h
+        Source/Camera/CameraUtilities.cpp
+        Source/Camera/CameraUtilities.h
         Source/Clock/PhysicallyStableClock.cpp
         Source/Clock/SimulationClock.cpp
         Source/Communication/QoS.cpp

Gems/ROS2/Code/ros2_header_files.cmake

@@ -4,6 +4,7 @@
 # SPDX-License-Identifier: Apache-2.0 OR MIT
 
 set(FILES
+        Include/ROS2/Camera/CameraCalibrationRequestBus.h
         Include/ROS2/Camera/CameraPostProcessingRequestBus.h
         Include/ROS2/Clock/PhysicallyStableClock.h
         Include/ROS2/Clock/SimulationClock.h