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

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

Improve recentering logic 

Allow recentering to be used to "repair" discontinuities in the input
stream by completely resetting the state. That includes considering
the next sample that arrives as the new baseline.

Test: atest --host libheadtracking-test
Bug: 188502620
Change-Id: Ib85679ce8f82c0059bd7260edf73418ab7ba9494
parent 779d1eec

media/libheadtracking/HeadTrackingProcessor-test.cpp

+14 −0
Original line number Diff line number Diff line
@@ -48,6 +48,11 @@ TEST(HeadTrackingProcessor, BasicComposition) { 


    std::unique_ptr<HeadTrackingProcessor> processor =

            createHeadTrackingProcessor(Options{}, HeadTrackingMode::SCREEN_RELATIVE);



    // Establish a baseline for the drift compensators.

    processor->setWorldToHeadPose(0, Pose3f(), Twist3f());

    processor->setWorldToScreenPose(0, Pose3f());



    processor->setWorldToHeadPose(0, worldToHead, Twist3f());

    processor->setWorldToScreenPose(0, worldToScreen);

    processor->setScreenToStagePose(screenToStage);
@@ -76,6 +81,11 @@ TEST(HeadTrackingProcessor, Prediction) { 


    std::unique_ptr<HeadTrackingProcessor> processor = createHeadTrackingProcessor(

            Options{.predictionDuration = 2.f}, HeadTrackingMode::WORLD_RELATIVE);



    // Establish a baseline for the drift compensators.

    processor->setWorldToHeadPose(0, Pose3f(), Twist3f());

    processor->setWorldToScreenPose(0, Pose3f());



    processor->setWorldToHeadPose(0, worldToHead, headTwist);

    processor->setWorldToScreenPose(0, worldToScreen);

    processor->calculate(0);
@@ -100,6 +110,10 @@ TEST(HeadTrackingProcessor, SmoothModeSwitch) { 
    std::unique_ptr<HeadTrackingProcessor> processor = createHeadTrackingProcessor(

            Options{.maxTranslationalVelocity = 1}, HeadTrackingMode::STATIC);



    // Establish a baseline for the drift compensators.

    processor->setWorldToHeadPose(0, Pose3f(), Twist3f());

    processor->setWorldToScreenPose(0, Pose3f());



    processor->calculate(0);



    processor->setDesiredMode(HeadTrackingMode::WORLD_RELATIVE);

media/libheadtracking/PoseDriftCompensator-test.cpp

+23 −9
Original line number Diff line number Diff line
@@ -39,17 +39,22 @@ TEST(PoseDriftCompensator, NoDrift) { 
    Pose3f pose2({4, 5, 6}, Quaternionf::UnitRandom());

    PoseDriftCompensator comp(Options{});



    // First pose sets the baseline.

    comp.setInput(1000, pose1);

    EXPECT_EQ(comp.getOutput(), pose1);

    EXPECT_EQ(comp.getOutput(), Pose3f());



    comp.setInput(2000, pose2);

    EXPECT_EQ(comp.getOutput(), pose2);

    EXPECT_EQ(comp.getOutput(), pose1.inverse() * pose2);



    // Recentering resets the baseline.

    comp.recenter();

    EXPECT_EQ(comp.getOutput(), Pose3f());



    comp.setInput(3000, pose1);

    EXPECT_EQ(comp.getOutput(), pose2.inverse() * pose1);

    EXPECT_EQ(comp.getOutput(), Pose3f());



    comp.setInput(4000, pose2);

    EXPECT_EQ(comp.getOutput(), pose1.inverse() * pose2);

}



TEST(PoseDriftCompensator, NoDriftZeroTime) {
@@ -58,16 +63,19 @@ TEST(PoseDriftCompensator, NoDriftZeroTime) { 
    PoseDriftCompensator comp(Options{});



    comp.setInput(1000, pose1);

    EXPECT_EQ(comp.getOutput(), pose1);

    EXPECT_EQ(comp.getOutput(), Pose3f());



    comp.setInput(1000, pose2);

    EXPECT_EQ(comp.getOutput(), pose2);

    EXPECT_EQ(comp.getOutput(), pose1.inverse() * pose2);



    comp.recenter();

    EXPECT_EQ(comp.getOutput(), Pose3f());



    comp.setInput(1000, pose1);

    EXPECT_EQ(comp.getOutput(), pose2.inverse() * pose1);

    EXPECT_EQ(comp.getOutput(), Pose3f());



    comp.setInput(1000, pose2);

    EXPECT_EQ(comp.getOutput(), pose1.inverse() * pose2);

}



TEST(PoseDriftCompensator, Asymptotic) {
@@ -92,16 +100,19 @@ TEST(PoseDriftCompensator, Fast) { 
            Options{.translationalDriftTimeConstant = 1e7, .rotationalDriftTimeConstant = 1e7});



    comp.setInput(0, pose1);

    EXPECT_EQ(comp.getOutput(), pose1);

    EXPECT_EQ(comp.getOutput(), Pose3f());



    comp.setInput(1, pose2);

    EXPECT_EQ(comp.getOutput(), pose2);

    EXPECT_EQ(comp.getOutput(), pose1.inverse() * pose2);



    comp.recenter();

    EXPECT_EQ(comp.getOutput(), Pose3f());



    comp.setInput(2, pose1);

    EXPECT_EQ(comp.getOutput(), pose2.inverse() * pose1);

    EXPECT_EQ(comp.getOutput(), Pose3f());



    comp.setInput(3, pose2);

    EXPECT_EQ(comp.getOutput(), pose1.inverse() * pose2);

}



TEST(PoseDriftCompensator, Drift) {
@@ -109,6 +120,9 @@ TEST(PoseDriftCompensator, Drift) { 
    PoseDriftCompensator comp(

            Options{.translationalDriftTimeConstant = 500, .rotationalDriftTimeConstant = 1000});



    // Establish a baseline.

    comp.setInput(1000, Pose3f());



    // Initial pose is used as is.

    comp.setInput(1000, pose1);

    EXPECT_EQ(comp.getOutput(), pose1);

media/libheadtracking/PoseDriftCompensator.cpp

+3 −4
Original line number Diff line number Diff line
@@ -29,10 +29,8 @@ using Eigen::Vector3f; 
PoseDriftCompensator::PoseDriftCompensator(const Options& options) : mOptions(options) {}



void PoseDriftCompensator::setInput(int64_t timestamp, const Pose3f& input) {

    if (!mTimestamp.has_value()) {

        // First input sample sets the output directly.

        mOutput = input;

    } else {

    if (mTimestamp.has_value()) {

        // Avoid computation upon first input (only sets the initial state).

        Pose3f prevInputToInput = mPrevInput.inverse() * input;

        mOutput = scale(mOutput, timestamp - mTimestamp.value()) * prevInputToInput;

    }
@@ -41,6 +39,7 @@ void PoseDriftCompensator::setInput(int64_t timestamp, const Pose3f& input) { 
}



void PoseDriftCompensator::recenter() {

    mTimestamp.reset();

    mOutput = Pose3f();

}