From 08eec48044897b79065a3792b32ddf40052da659 Mon Sep 17 00:00:00 2001 From: Pascal Serrarens Date: Wed, 28 Aug 2024 09:42:51 +0200 Subject: [PATCH] Spherical16 support --- LinearAlgebra | 2 +- NetworkPerception.cpp | 6 ++-- NetworkSync.cpp | 2 +- Perception.cpp | 43 ++++++++++++++------------ Perception.h | 71 ++++++++++++++++++++++++------------------- TrackedObject.cpp | 18 ++++++----- TrackedObject.h | 7 +++-- 7 files changed, 81 insertions(+), 68 deletions(-) diff --git a/LinearAlgebra b/LinearAlgebra index e10e010..e0ef43a 160000 --- a/LinearAlgebra +++ b/LinearAlgebra @@ -1 +1 @@ -Subproject commit e10e0100955f8f4dd139f52d71e37d6ace8c5fc0 +Subproject commit e0ef43a69989c88e87b973ab5b45cc3bd474c1bf diff --git a/NetworkPerception.cpp b/NetworkPerception.cpp index 4f24ece..cae6de6 100644 --- a/NetworkPerception.cpp +++ b/NetworkPerception.cpp @@ -121,7 +121,7 @@ void NetworkPerception::ReceiveSphere(unsigned char* data, Roboid* roboid) { Vector3 localPosition = Quaternion::Inverse(roboidOrientation) * deltaPosition; - Spherical position = Spherical(localPosition); + Spherical16 position = Spherical16::FromVector3(localPosition); roboid->perception->AddTrackedObject(this, position, Quaternion::identity, 0x81, 0x81, networkId); @@ -141,7 +141,7 @@ void NetworkPerception::ReceivePoseMsg(unsigned char* data, Roboid* roboid) { return ReceiveSphere(data, roboid); Quaternion roboidOrientation = roboid->GetOrientation(); - Spherical position = Spherical::zero; + Spherical16 position = Spherical16::zero; Quaternion orientation = Quaternion::identity; if ((poseType & NetworkSync::Pose_Position) != 0) { @@ -159,7 +159,7 @@ void NetworkPerception::ReceivePoseMsg(unsigned char* data, Roboid* roboid) { Vector3 localPosition = Quaternion::Inverse(roboidOrientation) * (worldPosition - roboidPosition); - position = Spherical(localPosition); + position = Spherical16::FromVector3(localPosition); } } diff --git a/NetworkSync.cpp b/NetworkSync.cpp index 6548264..5164b6e 100644 --- a/NetworkSync.cpp +++ b/NetworkSync.cpp @@ -215,7 +215,7 @@ void NetworkSync::PublishTrackedObject(Roboid* roboid, Quaternion roboidOrientation = roboid->GetOrientation(); // Vector3 localPosition = object->position.ToVector3(); - Vector3 localPosition = Vector3(object->position); + Vector3 localPosition = object->position.ToVector3(); Vector3 worldPosition = roboidPosition + roboidOrientation * localPosition; Quaternion worldOrientation = roboidOrientation * object->orientation.ToQuaternion(); diff --git a/Perception.cpp b/Perception.cpp index d7a8424..c382d61 100644 --- a/Perception.cpp +++ b/Perception.cpp @@ -76,7 +76,7 @@ float GetPlaneDistance(InterestingThing* plane, float range) { float distance = plane->position.distance; float deltaAngle = - Angle::Normalize((float)plane->position.horizontalAngle.ToFloat() - + Angle::Normalize((float)plane->position.horizontal.ToFloat() - horizontalAngle) .ToFloat(); if (fabsf(deltaAngle) < fabsf(range)) { @@ -122,9 +122,9 @@ float Perception::GetDistance(float horizontalDirection, float range) { if (obj->type == 0x080) { // plane float planeDistance = GetPlaneDistance(obj, horizontalDirection, range); minDistance = fminf(minDistance, planeDistance); - } else if (obj->position.horizontalAngle.ToFloat() > + } else if (obj->position.horizontal.ToFloat() > horizontalDirection - range && - obj->position.horizontalAngle.ToFloat() < + obj->position.horizontal.ToFloat() < horizontalDirection + range) { minDistance = fminf(minDistance, obj->position.distance); } @@ -149,10 +149,8 @@ float Perception::GetDistanceOfType(unsigned char thingType, if (thing->type == 0x080) { // plane float planeDistance = GetPlaneDistance(thing, horizontalAngle, range); minDistance = fminf(minDistance, planeDistance); - } else if (thing->position.horizontalAngle.ToFloat() > - horizontalAngle - range && - thing->position.horizontalAngle.ToFloat() < - horizontalAngle + range) { + } else if (thing->position.horizontal.ToFloat() > horizontalAngle - range && + thing->position.horizontal.ToFloat() < horizontalAngle + range) { minDistance = fminf(minDistance, thing->position.distance); } } @@ -170,8 +168,8 @@ float Perception::GetDistance(float horizontalDirection, InterestingThing* obj = trackedObjects[objIx]; if (obj == nullptr) continue; - if (obj->position.horizontalAngle.ToFloat() > horizontalDirection - range && - obj->position.horizontalAngle.ToFloat() < horizontalDirection + range) { + if (obj->position.horizontal.ToFloat() > horizontalDirection - range && + obj->position.horizontal.ToFloat() < horizontalDirection + range) { minDistance = fminf(minDistance, obj->position.distance); } } @@ -187,8 +185,8 @@ bool Perception::ObjectNearby(float direction, float range) { if (obj == nullptr) continue; - if (obj->position.horizontalAngle.ToFloat() > direction - range && - obj->position.horizontalAngle.ToFloat() < direction + range) { + if (obj->position.horizontal.ToFloat() > direction - range && + obj->position.horizontal.ToFloat() < direction + range) { if (obj->position.distance <= nearbyDistance) return true; } @@ -202,7 +200,8 @@ void Perception::AddTrackedObject(Sensor* sensor, Polar position, unsigned char thingType, unsigned char networkId) { - Spherical sPos = Spherical(position); + Spherical16 sPos = + Spherical16(position.distance, Angle16(position.angle.ToFloat()), 0); Quaternion orientation = Quaternion::identity; AddTrackedObject(sensor, sPos, orientation, thingType, thingType, networkId); /* @@ -282,7 +281,7 @@ void Perception::AddTrackedObject(Sensor* sensor, } InterestingThing* Perception::AddTrackedObject(Sensor* sensor, - Spherical position, + Spherical16 position, Quaternion orientation, unsigned char thingType, unsigned char thingId, @@ -349,7 +348,7 @@ InterestingThing* Perception::AddTrackedObject(Sensor* sensor, InterestingThing* Perception::AddTrackedObject(Sensor* sensor, unsigned char networkId, unsigned char objectId, - Spherical position, + Spherical16 position, Quaternion orientation) { InterestingThing* thing = FindTrackedObject(networkId, objectId); if (thing == nullptr) { @@ -531,7 +530,7 @@ void Perception::UpdatePose(Polar translation) { // (float)thing->position.horizontalAngle, // (float)thing->position.verticalAngle); // Update the closest point to the plane - float angle = (float)thing->position.horizontalAngle.ToFloat() + + float angle = (float)thing->position.horizontal.ToFloat() + translation.angle.ToFloat(); angle = fabsf(angle); @@ -544,9 +543,13 @@ void Perception::UpdatePose(Polar translation) { // (float)thing->position.horizontalAngle, // (float)thing->position.verticalAngle); } else { - Polar horizontalPosition = - Polar(thing->position); // obj->position.ProjectOnHorizontalPlane(); - Spherical newPosition = Spherical(horizontalPosition - translation); + // Polar horizontalPosition = Polar(thing->position); + // // obj->position.ProjectOnHorizontalPlane(); + // Spherical16 newPosition = Spherical16(horizontalPosition - + // translation); + Spherical16 translationS = Spherical16( + translation.distance, Angle16(translation.angle.ToFloat()), 0); + Spherical16 newPosition = thing->position + translationS; thing->position = newPosition; } } @@ -571,8 +574,8 @@ void Perception::UpdatePose(Quaternion rotation) { // (float)thing->position.verticalAngle); // printf("| rotate %f | ", rotationAngle); - thing->position.horizontalAngle = Angle::Normalize( - thing->position.horizontalAngle.ToFloat() - rotationAngle); + thing->position.horizontal = Angle16(Angle16::Normalize( + thing->position.horizontal.ToFloat() - rotationAngle)); // printf("-> %f (%f %f) \n", thing->position.distance, // (float)thing->position.horizontalAngle, diff --git a/Perception.h b/Perception.h index 721c61b..2750ca7 100644 --- a/Perception.h +++ b/Perception.h @@ -15,37 +15,38 @@ class Roboid; /// @brief Module to which keeps track of objects around the roboid class Perception { -public: + public: /// @brief Default Constructor Perception(); /// @brief Create a perception setup with the given Sensors /// @param sensors The Placement of Sensors on the Roboid /// @param sensorCount The number of sensors in the placement array - Perception(Sensor **sensors, unsigned int sensorCount); + Perception(Sensor** sensors, unsigned int sensorCount); /// @brief The roboid of this perception system - Roboid *roboid = nullptr; + Roboid* roboid = nullptr; - unsigned int AddSensor(Sensor *sensor); + unsigned int AddSensor(Sensor* sensor); /// @brief Get the number of Sensors /// @return The number of sensors, zero when no sensors are present unsigned int GetSensorCount(); - Sensor *GetSensor(unsigned int sensorId); + Sensor* GetSensor(unsigned int sensorId); /// @brief Find the first sensor of the given type /// @param sensorType The type of sensor as is defined in the Thing class (for /// example Thing::SensorType) /// @return The first sensor found or a nullptr which no sensor has been found /// of the given type - Sensor *FindSensorOfType(unsigned int sensorType); + Sensor* FindSensorOfType(unsigned int sensorType); /// @brief Gets the distance to the closest object /// @param direction The direction to look for objects /// @param range The range in which objects should be looked for /// @return The distance to the closest object in meters float GetDistance(float direction, float range = 10.0F); - float GetDistanceOfType(unsigned char thingType, float horizontalAngle, + float GetDistanceOfType(unsigned char thingType, + float horizontalAngle, float range = 10.0F); /// @brief Gets the distance to the closest object /// @param horizontalDirection The direction in the horizontal plane to look @@ -56,7 +57,8 @@ public: /// @return The distance to the closest object in meters /// @details The directions can be thought of as the polar angle (vertical) /// and azimuthal angle (horizontal) in the spherical coordinate system. - float GetDistance(float horizontalDirection, float verticalDirection, + float GetDistance(float horizontalDirection, + float verticalDirection, float range = 10.0F); /// @brief Checks if an object is nearby @@ -87,24 +89,29 @@ public: /// @param sensor The sensor which has detected the object /// @param position The position of the sensor in polar coordinates local to /// the roboid - void AddTrackedObject(Sensor *sensor, Polar position, + void AddTrackedObject(Sensor* sensor, + Polar position, unsigned char objectType = 0x00, unsigned char networkId = 0x00); - InterestingThing * - AddTrackedObject(Sensor *sensor, Spherical position, - Quaternion orientation = Quaternion::identity, - unsigned char objectType = 0x00, - unsigned char objectId = 0x00, unsigned networkId = 0x00); + InterestingThing* AddTrackedObject( + Sensor* sensor, + Spherical16 position, + Quaternion orientation = Quaternion::identity, + unsigned char objectType = 0x00, + unsigned char objectId = 0x00, + unsigned networkId = 0x00); - InterestingThing * - AddTrackedObject(Sensor *sensor, unsigned char networkId, - unsigned char objectId, Spherical position, - Quaternion orientation = Quaternion::identity); + InterestingThing* AddTrackedObject( + Sensor* sensor, + unsigned char networkId, + unsigned char objectId, + Spherical16 position, + Quaternion orientation = Quaternion::identity); bool IsInteresting(float distance); - InterestingThing *FindTrackedObject(char objectId); - InterestingThing *FindTrackedObject(unsigned char networkId, + InterestingThing* FindTrackedObject(char objectId); + InterestingThing* FindTrackedObject(unsigned char networkId, unsigned char objectId); /// @brief Retrieve the number of objects currently being tracked by the @@ -117,14 +124,14 @@ public: /// @details The returned array this should never be a nullptr, but /// each array entry may be a nullptr when less than maxObjectCount objects is /// currently being tracked. - InterestingThing **GetTrackedObjects(); + InterestingThing** GetTrackedObjects(); unsigned char ThingsOfType(unsigned char objectType, - InterestingThing *buffer[], + InterestingThing* buffer[], unsigned char bufferSize); - InterestingThing *ThingOfType(unsigned char objectType); + InterestingThing* ThingOfType(unsigned char objectType); - InterestingThing *GetMostInterestingThing(); + InterestingThing* GetMostInterestingThing(); // mainly used for confidence update @@ -154,21 +161,21 @@ public: /// objects float nearbyDistance = 0.02F; -public: + public: /// @brief The Sensors used for Perception - Sensor **sensors = nullptr; + Sensor** sensors = nullptr; /// @brief The number of Sensors used for Perception unsigned int sensorCount = 0; unsigned long lastUpdateTimeMs = 0; unsigned char lastObjectId = 1; - static unsigned char maxObjectCount; // = 7; // 7 is typically the maximum - // number of object which can - // be tracked by a human - InterestingThing **trackedObjects; + static unsigned char maxObjectCount; // = 7; // 7 is typically the maximum + // number of object which can + // be tracked by a human + InterestingThing** trackedObjects; }; -} // namespace RoboidControl -} // namespace Passer +} // namespace RoboidControl +} // namespace Passer using namespace Passer::RoboidControl; diff --git a/TrackedObject.cpp b/TrackedObject.cpp index a9a0ee8..a2a8aa3 100644 --- a/TrackedObject.cpp +++ b/TrackedObject.cpp @@ -6,12 +6,13 @@ InterestingThing::InterestingThing(Sensor* sensor, Polar position) { this->id = 0; this->confidence = maxConfidence; this->sensor = sensor; - this->position = Spherical(position); + this->position = + Spherical16(position.distance, Angle16(position.angle.ToFloat()), 0); this->updated = true; } InterestingThing::InterestingThing(Sensor* sensor, - Spherical position, + Spherical16 position, Quaternion orientation) { this->id = 0; this->confidence = maxConfidence; @@ -44,11 +45,11 @@ bool InterestingThing::IsTheSameAs(InterestingThing* otherObj) { // (float)position.horizontalAngle, (float)position.verticalAngle); if (fabsf(position.distance - otherObj->position.distance) > equalDistance) return false; - if (fabsf(position.horizontalAngle.ToFloat() - - otherObj->position.horizontalAngle.ToFloat()) > equalAngle) + if (fabsf(position.horizontal.ToFloat() - + otherObj->position.horizontal.ToFloat()) > equalAngle) return false; - if (fabsf(position.verticalAngle.ToFloat() - - otherObj->position.verticalAngle.ToFloat()) > equalAngle) + if (fabsf(position.vertical.ToFloat() - + otherObj->position.vertical.ToFloat()) > equalAngle) return false; // printf(" -> yes "); return true; @@ -72,12 +73,13 @@ bool InterestingThing::DegradeConfidence(float deltaTime) { } void InterestingThing::Refresh(Polar position) { - this->position = Spherical(position); + this->position = + Spherical16(position.distance, Angle16(position.angle.ToFloat()), 0); this->confidence = maxConfidence; this->updated = true; } -void InterestingThing::Refresh(Spherical position, Quaternion orientation) { +void InterestingThing::Refresh(Spherical16 position, Quaternion orientation) { this->position = position; float angle; diff --git a/TrackedObject.h b/TrackedObject.h index 45d4e50..57ad432 100644 --- a/TrackedObject.h +++ b/TrackedObject.h @@ -1,5 +1,6 @@ #pragma once +#include "LinearAlgebra/Angle16.h" #include "LinearAlgebra/AngleAxis.h" #include "LinearAlgebra/Polar.h" #include "LinearAlgebra/Quaternion.h" @@ -17,7 +18,7 @@ class InterestingThing { /// @param position The position in polar coordinates local to the roboid InterestingThing(Sensor* sensor, Polar position); InterestingThing(Sensor* sensor, - Spherical position, + Spherical16 position, Quaternion orientation = Quaternion::identity); /// @brief Update the position of the object @@ -25,7 +26,7 @@ class InterestingThing { /// @details This will also update the confidence of the object to the /// maxConfidence value void Refresh(Polar position); - void Refresh(Spherical position, + void Refresh(Spherical16 position, Quaternion orientation = Quaternion::identity); /// @brief Decrease the confidence based on the elapsed time @@ -66,7 +67,7 @@ class InterestingThing { char parentId = 0; /// @brief The current position of the object - Spherical position = Spherical::zero; + Spherical16 position = Spherical16::zero; /// @brief The current orientation of the object AngleAxis orientation = AngleAxis(); // Quaternion orientation = Quaternion::identity;