From a039dcfe39d45a57ac6be07ab8efc3743f6043e1 Mon Sep 17 00:00:00 2001 From: Kenneth Bogert Date: Wed, 17 Jun 2015 18:17:45 -0400 Subject: [PATCH] Fix scale estimation calculation It incorrectly used the kalman filter to retrieve the previous visual field position. This set up a circular dependency between the scale parameter and the filter. The fix is to use the previous visual field position as specified in the associated papers. --- src/stateestimation/PTAMWrapper.cpp | 6 ++---- 1 file changed, 2 insertions(+), 4 deletions(-) diff --git a/src/stateestimation/PTAMWrapper.cpp b/src/stateestimation/PTAMWrapper.cpp index 30fb118..8af37e6 100644 --- a/src/stateestimation/PTAMWrapper.cpp +++ b/src/stateestimation/PTAMWrapper.cpp @@ -431,8 +431,6 @@ void PTAMWrapper::HandleFrame() filterPosePostPTAM = filter->getCurrentPoseSpeedAsVec(); pthread_mutex_unlock( &filter->filter_CS ); - TooN::Vector<6> filterPosePostPTAMBackTransformed = filter->backTransformPTAMObservation(filterPosePostPTAM.slice<0,6>()); - // if interval is started: add one step. int includedTime = mimFrameTime_workingCopy - ptamPositionForScaleTakenTimestamp; @@ -451,7 +449,7 @@ void PTAMWrapper::HandleFrame() if(includedTime >= 2000 && framesIncludedForScaleXYZ > 1) // ADD! (if too many, was resetted before...) { - TooN::Vector<3> diffPTAM = filterPosePostPTAMBackTransformed.slice<0,3>() - PTAMPositionForScale; + TooN::Vector<3> diffPTAM = PTAMResult.slice<0,3>() - PTAMPositionForScale; bool zCorrupted, allCorrupted; float pressureStart = 0, pressureEnd = 0; TooN::Vector<3> diffIMU = evalNavQue(ptamPositionForScaleTakenTimestamp - filter->delayVideo + filter->delayXYZ,mimFrameTime_workingCopy - filter->delayVideo + filter->delayXYZ,&zCorrupted, &allCorrupted, &pressureStart, &pressureEnd); @@ -484,7 +482,7 @@ void PTAMWrapper::HandleFrame() if(framesIncludedForScaleXYZ == -1) // RESET! { framesIncludedForScaleXYZ = 0; - PTAMPositionForScale = filterPosePostPTAMBackTransformed.slice<0,3>(); + PTAMPositionForScale = PTAMResult.slice<0,3>(); //predIMUOnlyForScale->resetPos(); // also resetting z corrupted flag etc. (NOT REquired as reset is done in eval) ptamPositionForScaleTakenTimestamp = mimFrameTime_workingCopy; }