From ca8ee0435e7c516ce7baf656aaf6d2744f28ea8b Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Micha=C5=82=20Pe=C5=82ka?= Date: Mon, 24 Jul 2023 11:04:33 +0200 Subject: [PATCH] Limits camera drift that used to happen when the camera was following a robot. MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Michał Pełka --- .../FollowingCameraComponent.cpp | 44 ++++++++----------- .../FollowingCameraComponent.h | 16 ++----- 2 files changed, 22 insertions(+), 38 deletions(-) diff --git a/Gems/ROS2/Code/Source/SimulationUtils/FollowingCameraComponent.cpp b/Gems/ROS2/Code/Source/SimulationUtils/FollowingCameraComponent.cpp index f40dce27f..5f110ab85 100644 --- a/Gems/ROS2/Code/Source/SimulationUtils/FollowingCameraComponent.cpp +++ b/Gems/ROS2/Code/Source/SimulationUtils/FollowingCameraComponent.cpp @@ -79,12 +79,13 @@ namespace ROS2 required.push_back(AZ_CRC("CameraService")); } - void FollowingCameraComponent::Init() - { - } - void FollowingCameraComponent::Activate() { + if (m_predefinedViews.size() == 0) + { + AZ_Warning("FollowingCameraComponent", false, "No predefined views"); + return; + } if (m_defaultView < m_predefinedViews.size()) { m_currentView = m_predefinedViews[m_defaultView]; @@ -103,27 +104,8 @@ namespace ROS2 { // update the smoothing buffer m_lastTranslationsBuffer.push_back(AZStd::make_pair(transform.GetTranslation(), deltaTime)); + m_lastRotationsBuffer.push_back(AZStd::make_pair(transform.GetRotation(), deltaTime)); - if (m_lastRotationsBuffer.empty()) - { - m_lastRotationsBuffer.push_back(AZStd::make_pair(transform.GetRotation().ConvertToScaledAxisAngle(), deltaTime)); - } - else - { - // Note that the tangent needs to be non-normalized (eg. it has to grow into infinity with constant rotation in one direction), - // so the camera won't wind-back when robot is rotating. - // Get incremental rotation - AZ::Quaternion update = transform.GetRotation() * m_lastRotation.GetInverseFull(); - - // compute tangent to incremental rotation - AZ::Vector3 updateTangent = update.ConvertToScaledAxisAngle(); - - // add tangent to the last tangent - const AZ::Vector3& lastAddedTangent = m_lastRotationsBuffer.back().first; - m_lastRotationsBuffer.push_back(AZStd::make_pair(lastAddedTangent + updateTangent, deltaTime)); - } - - m_lastRotation = transform.GetRotation(); if (m_lastTranslationsBuffer.size() > m_smoothingBuffer) { m_lastTranslationsBuffer.pop_front(); @@ -135,6 +117,11 @@ namespace ROS2 } void FollowingCameraComponent::OnTick(float deltaTime, AZ::ScriptTimePoint /*time*/) { + AZ_Warning("FollowingCameraComponent", m_currentView.IsValid(), "View is not valid"); + if (!m_currentView.IsValid()) + { + return; + } // obtain the current view transform AZ::Transform target_local_transform; AZ::Transform target_world_transform; @@ -183,8 +170,13 @@ namespace ROS2 AZ::Quaternion FollowingCameraComponent::SmoothRotation() const { - AZ::Vector3 averagedTangent = AverageVector(m_lastRotationsBuffer); - return AZ::Quaternion::CreateFromScaledAxisAngle(averagedTangent); + AZ::Quaternion q = m_lastRotationsBuffer.front().first; + for (int i = 1; i < m_lastRotationsBuffer.size(); i++) + { + q = q.Slerp(m_lastRotationsBuffer[i].first, m_lastRotationsBuffer[i].second); + + } + return q; } bool FollowingCameraComponent::OnInputChannelEventFiltered(const AzFramework::InputChannel& inputChannel) diff --git a/Gems/ROS2/Code/Source/SimulationUtils/FollowingCameraComponent.h b/Gems/ROS2/Code/Source/SimulationUtils/FollowingCameraComponent.h index 1671792fa..1776a2e58 100644 --- a/Gems/ROS2/Code/Source/SimulationUtils/FollowingCameraComponent.h +++ b/Gems/ROS2/Code/Source/SimulationUtils/FollowingCameraComponent.h @@ -15,12 +15,8 @@ namespace ROS2 { - //! The component used for camera following robots. - //! It allows to switch between different camera that can be attached to entities, and to control the camera with the keyboard. - //! It allows to offset angle around parent's Z axis, and to offset the distance from parent. - //! More over it is smoothing movement of the camera. - //! The theoretical background for handling quaternions as tangent vectors can be found in the paper: - //! "A micro Lie theory for state estimation in robotics" by Joan Solà, J. M. Porta, and Alberto Sanfeliu. + //! The component used for cameras that follow robots. + //! It allows to switch between different cameras attached to entities, and to control the active camera using keyboard. class FollowingCameraComponent : public AZ::Component , public AZ::TickBus::Handler @@ -34,17 +30,16 @@ namespace ROS2 AZ_COMPONENT(FollowingCameraComponent, "{6a21768a-f327-11ed-a05b-0242ac120003}", AZ::Component); // AZ::Component - void Init() override; void Activate() override; void Deactivate() override; + private: // AZ::TickBus overrides void OnTick(float deltaTime, AZ::ScriptTimePoint time) override; // AzFramework::InputChannelEventListener overrides bool OnInputChannelEventFiltered(const AzFramework::InputChannel& inputChannel) override; - private: void OnKeyboardEvent(const AzFramework::InputChannel& inputChannel); //! Compute weighted average of the vectors in the buffer. @@ -69,10 +64,7 @@ namespace ROS2 AZStd::deque> m_lastTranslationsBuffer; //! The smoothing buffer for rotation, the first element is the tangential vector, the second element is the weight. - AZStd::deque> m_lastRotationsBuffer; - - //! The last translation, used to compute determine the incremental rotation. - AZ::Quaternion m_lastRotation; + AZStd::deque> m_lastRotationsBuffer; //! Predefined view points. AZStd::vector m_predefinedViews;