Camera sensor fix (#291)

* Camera sensor projection matrix
* Moving ROS2Frame service into ROS2CameraSensorComponent
* Proper implementation of camera sensor

Co-authored-by: Michał Pełka <[email protected]>
Signed-off-by: Paweł Lech <[email protected]>

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

@@ -23,6 +23,7 @@ namespace ROS2
 {
     CameraSensorDescription::CameraSensorDescription(const AZStd::string& cameraName, float verticalFov, int width, int height)
         : m_verticalFieldOfViewDeg(verticalFov)
+        , m_verticalFieldOfViewRad(AZ::DegToRad(m_verticalFieldOfViewDeg))
         , m_width(width)
         , m_height(height)
         , m_cameraName(cameraName)
@@ -63,9 +64,9 @@ namespace ROS2
         // (cx, cy).
         const auto w = static_cast<double>(m_width);
         const auto h = static_cast<double>(m_height);
-        const double horizontalFoV = 2.0 * AZStd::atan(AZStd::tan(m_verticalFieldOfViewDeg / 2.0) / m_aspectRatio);
+        const double horizontalFoV = 2.0 * AZStd::atan(AZStd::tan(m_verticalFieldOfViewRad / 2.0) * m_aspectRatio);
         const double focalLengthX = w / (2.0 * AZStd::tan(horizontalFoV / 2.0));
-        const double focalLengthY = h / (2.0 * AZStd::tan(m_verticalFieldOfViewDeg / 2.0));
+        const double focalLengthY = h / (2.0 * AZStd::tan(m_verticalFieldOfViewRad / 2.0));
         return { focalLengthX, 0.0, w / 2.0, 0.0, focalLengthY, h / 2.0, 0.0, 0.0, 1.0 };
     }
 
@@ -127,7 +128,7 @@ namespace ROS2
     void CameraSensor::RequestFrame(
         const AZ::Transform& cameraPose, std::function<void(const AZ::RPI::AttachmentReadback::ReadbackResult& result)> callback)
     {
-        AZ::Transform inverse = cameraPose.GetInverse();
+        AZ::Transform inverse = (cameraPose * kAtomToRos).GetInverse();
         m_view->SetWorldToViewMatrix(AZ::Matrix4x4::CreateFromQuaternionAndTranslation(inverse.GetRotation(), inverse.GetTranslation()));
 
         AZ::Render::FrameCaptureId captureId = AZ::Render::InvalidFrameCaptureId;

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

@@ -27,6 +27,7 @@ namespace ROS2
         CameraSensorDescription(const AZStd::string& cameraName, float verticalFov, int width, int height);
 
         const float m_verticalFieldOfViewDeg; //!< camera vertical field of view
+        double m_verticalFieldOfViewRad;
         const int m_width; //!< camera image width in pixels
         const int m_height; //!< camera image height in pixels
         const AZStd::string m_cameraName; //!< camera name to differentiate cameras in a multi-camera setup
@@ -71,6 +72,8 @@ namespace ROS2
         AZ::RPI::RenderPipelinePtr m_pipeline;
         AZ::RPI::ViewPtr m_view;
         AZ::RPI::Scene* m_scene = nullptr;
+        const AZ::Transform kAtomToRos{ AZ::Transform::CreateFromQuaternion(
+            AZ::Quaternion::CreateFromMatrix3x3(AZ::Matrix3x3::CreateFromRows({ 1, 0, 0 }, { 0, -1, 0 }, { 0, 0, -1 }))) };
     };
 
 } // namespace ROS2

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

@@ -16,6 +16,9 @@
 #include <AzCore/Serialization/EditContext.h>
 #include <AzCore/Serialization/SerializeContext.h>
 
+#include <AzToolsFramework/Entity/EditorEntityHelpers.h>
+#include <AzToolsFramework/ToolsComponents/TransformComponent.h>
+
 #include <sensor_msgs/distortion_models.hpp>
 #include <sensor_msgs/image_encodings.hpp>
 
@@ -142,6 +145,9 @@ namespace ROS2
                 cameraInfo.height = descriptor.m_size.m_height;
                 cameraInfo.distortion_model = sensor_msgs::distortion_models::PLUMB_BOB;
                 cameraInfo.k = m_cameraSensor->GetCameraSensorDescription().m_cameraIntrinsics;
+                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 };
+
                 m_cameraInfoPublisher->publish(cameraInfo);
             });
     }

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

@@ -15,6 +15,9 @@
 
 #include <AzCore/Component/Component.h>
 
+#include <ROS2/Frame/NamespaceConfiguration.h>
+#include <ROS2/Frame/ROS2Transform.h>
+
 #include "CameraSensor.h"
 
 namespace ROS2