Add a float-vector representation to Pose3f
Bug: 188502620
Test: atest --host libheatracking-test
Change-Id: I7e84b08419e9a11920eca5e0aa1fde03e3729f68
diff --git a/media/libheadtracking/Pose-test.cpp b/media/libheadtracking/Pose-test.cpp
index 3ff6a9b..a9e18ce 100644
--- a/media/libheadtracking/Pose-test.cpp
+++ b/media/libheadtracking/Pose-test.cpp
@@ -110,6 +110,29 @@
EXPECT_TRUE(std::get<1>(result));
}
+TEST(Pose, FloatVectorRoundTrip1) {
+ // Rotation vector magnitude must be less than Pi.
+ std::vector<float> vec = { 1, 2, 3, 0.4, 0.5, 0.6};
+ std::optional<Pose3f> pose = Pose3f::fromVector(vec);
+ ASSERT_TRUE(pose.has_value());
+ std::vector<float> reconstructed = pose->toVector();
+ EXPECT_EQ(vec, reconstructed);
+}
+
+TEST(Pose, FloatVectorRoundTrip2) {
+ Pose3f pose({1, 2, 3}, Quaternionf::UnitRandom());
+ std::vector<float> vec = pose.toVector();
+ std::optional<Pose3f> reconstructed = Pose3f::fromVector(vec);
+ ASSERT_TRUE(reconstructed.has_value());
+ EXPECT_EQ(pose, reconstructed.value());
+}
+
+TEST(Pose, FloatVectorInvalid) {
+ EXPECT_FALSE(Pose3f::fromVector({}).has_value());
+ EXPECT_FALSE(Pose3f::fromVector({1, 2, 3, 4, 5}).has_value());
+ EXPECT_FALSE(Pose3f::fromVector({1, 2, 3, 4, 5, 6, 7}).has_value());
+}
+
} // namespace
} // namespace media
} // namespace android
diff --git a/media/libheadtracking/Pose.cpp b/media/libheadtracking/Pose.cpp
index 9eeb2b1..47241ce 100644
--- a/media/libheadtracking/Pose.cpp
+++ b/media/libheadtracking/Pose.cpp
@@ -16,10 +16,25 @@
#include "media/Pose.h"
#include "media/Twist.h"
+#include "QuaternionUtil.h"
namespace android {
namespace media {
+using Eigen::Vector3f;
+
+std::optional<Pose3f> Pose3f::fromVector(const std::vector<float>& vec) {
+ if (vec.size() != 6) {
+ return std::nullopt;
+ }
+ return Pose3f({vec[0], vec[1], vec[2]}, rotationVectorToQuaternion({vec[3], vec[4], vec[5]}));
+}
+
+std::vector<float> Pose3f::toVector() const {
+ Eigen::Vector3f rot = quaternionToRotationVector(mRotation);
+ return {mTranslation[0], mTranslation[1], mTranslation[2], rot[0], rot[1], rot[2]};
+}
+
std::tuple<Pose3f, bool> moveWithRateLimit(const Pose3f& from, const Pose3f& to, float t,
float maxTranslationalVelocity,
float maxRotationalVelocity) {
diff --git a/media/libheadtracking/include/media/Pose.h b/media/libheadtracking/include/media/Pose.h
index 06b33f3..e660bb9 100644
--- a/media/libheadtracking/include/media/Pose.h
+++ b/media/libheadtracking/include/media/Pose.h
@@ -15,6 +15,8 @@
*/
#pragma once
+#include <optional>
+#include <vector>
#include <Eigen/Geometry>
namespace android {
@@ -45,6 +47,22 @@
Pose3f(const Pose3f& other) { *this = other; }
+ /**
+ * Create instance from a vector-of-floats representation.
+ * The vector is expected to have exactly 6 elements, where the first three are a translation
+ * vector and the last three are a rotation vector.
+ *
+ * Returns nullopt if the input vector is illegal.
+ */
+ static std::optional<Pose3f> fromVector(const std::vector<float>& vec);
+
+ /**
+ * Convert instance to a vector-of-floats representation.
+ * The vector will have exactly 6 elements, where the first three are a translation vector and
+ * the last three are a rotation vector.
+ */
+ std::vector<float> toVector() const;
+
Pose3f& operator=(const Pose3f& other) {
mTranslation = other.mTranslation;
mRotation = other.mRotation;