private void fuseEstimates ( ) { // weighted average for angle double x = closeMotionKeyToCurr . c * closeInlierCount + Math . cos ( farAngle ) * farInlierCount ; double y = closeMotionKeyToCurr . s * closeInlierCount + Math . sin ( farAngle ) * farInlierCount ; // update the motion estimate closeMotionKeyToCurr . setYaw ( Math . atan2 ( y , x ) ) ; // save the results closeMotionKeyToCurr . invert ( currToKey ) ; }
Fuse the estimates for yaw from both sets of points using a weighted vector average and save the results into currToKey