Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Limits camera drift that used to happen when the camera was following a robot. #27

Open
wants to merge 1 commit into
base: forward_branch_wp2
Choose a base branch
from
Open

Limits camera drift that used to happen when the camera was following a robot. #27

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
44 changes: 18 additions & 26 deletions Gems/ROS2/Code/Source/SimulationUtils/FollowingCameraComponent.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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];
Expand All @@ -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();
Expand All @@ -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;
Expand Down Expand Up @@ -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)
Expand Down
16 changes: 4 additions & 12 deletions Gems/ROS2/Code/Source/SimulationUtils/FollowingCameraComponent.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand All @@ -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.
Expand All @@ -69,10 +64,7 @@ namespace ROS2
AZStd::deque<AZStd::pair<AZ::Vector3, float>> m_lastTranslationsBuffer;

//! The smoothing buffer for rotation, the first element is the tangential vector, the second element is the weight.
AZStd::deque<AZStd::pair<AZ::Vector3, float>> m_lastRotationsBuffer;

//! The last translation, used to compute determine the incremental rotation.
AZ::Quaternion m_lastRotation;
AZStd::deque<AZStd::pair<AZ::Quaternion, float>> m_lastRotationsBuffer;

//! Predefined view points.
AZStd::vector<AZ::EntityId> m_predefinedViews;
Expand Down