Donate to e Foundation | Murena handsets with /e/OS | Own a part of Murena! Learn more

Commit 35a1fcb1 authored by Ytai Ben-Tsvi's avatar Ytai Ben-Tsvi
Browse files

Add a float-vector representation to Pose3f 

Bug: 188502620
Test: atest --host libheatracking-test
Change-Id: I7e84b08419e9a11920eca5e0aa1fde03e3729f68
parent a16a9df3

media/libheadtracking/Pose-test.cpp

+23 −0
Original line number Diff line number Diff line
@@ -110,6 +110,29 @@ TEST(Pose, MoveWithRateLimit_RotationLimit) { 
    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

media/libheadtracking/Pose.cpp

+15 −0
Original line number Diff line number Diff line
@@ -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) {

media/libheadtracking/include/media/Pose.h

+18 −0
Original line number Diff line number Diff line
@@ -15,6 +15,8 @@ 
 */

#pragma once



#include <optional>

#include <vector>

#include <Eigen/Geometry>



namespace android {
@@ -45,6 +47,22 @@ class Pose3f { 


    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;