i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-09-13 23:10:20 +08:00
Code Issues Packages Projects Releases Wiki Activity

arcore_java: fixed high speed filtering jumping (#1281)

Browse Source
This commit is contained in:
matlabbe
2024-05-25 15:54:57 -07:00
parent c5c190a2a0
commit 03afb94d92
1 changed files with 4 additions and 3 deletions
Download Patch File Download Diff File Expand all files Collapse all files
app/android/src/com/introlab/rtabmap/ARCoreSharedCamera.java
+4 -3
View File
@@ -701,6 +701,7 @@ public class ARCoreSharedCamera {
mToast.setText(msg); mToast.setText(msg);
} }
previousAnchorPose = null; previousAnchorPose = null;
arCoreCorrection = Pose.IDENTITY;
} }
} }
}); });
@@ -721,7 +722,8 @@ public class ARCoreSharedCamera {
final double speed = Math.sqrt(t[0]*t[0]+t[1]*t[1]+t[2]*t[2])/((double)(frame.getTimestamp()-previousAnchorTimeStamp)/10e8); final double speed = Math.sqrt(t[0]*t[0]+t[1]*t[1]+t[2]*t[2])/((double)(frame.getTimestamp()-previousAnchorTimeStamp)/10e8);
if(speed>=mARCoreLocalizationFilteringSpeed) if(speed>=mARCoreLocalizationFilteringSpeed)
{ {
arCoreCorrection = arCoreCorrection.compose(previousAnchorPose).compose(pose.inverse()); // Only correct the translation to not lose rotation aligned with gravity
arCoreCorrection = arCoreCorrection.compose(previousAnchorPose.compose(pose.inverse()).extractTranslation());
t = arCoreCorrection.getTranslation(); t = arCoreCorrection.getTranslation();
Log.e(TAG, String.format("POTENTIAL TELEPORTATION!!!!!!!!!!!!!! previous anchor moved (speed=%f), new arcorrection: %f %f %f", speed, t[0], t[1], t[2])); Log.e(TAG, String.format("POTENTIAL TELEPORTATION!!!!!!!!!!!!!! previous anchor moved (speed=%f), new arcorrection: %f %f %f", speed, t[0], t[1], t[2]));
@@ -747,7 +749,6 @@ public class ARCoreSharedCamera {
{ {
mToast.setText(msg); mToast.setText(msg);
} }
previousAnchorPose = null;
} }
} }
}); });
@@ -758,7 +759,7 @@ public class ARCoreSharedCamera {
previousAnchorTimeStamp = frame.getTimestamp(); previousAnchorTimeStamp = frame.getTimestamp();
double stamp = (double)frame.getTimestamp()/10e8; double stamp = (double)frame.getTimestamp()/10e8;
if(!RTABMapActivity.DISABLE_LOG) Log.d(TAG, String.format("pose=%f %f %f q=%f %f %f %f stamp=%f", odomPose.tx(), odomPose.ty(), odomPose.tz(), odomPose.qx(), odomPose.qy(), odomPose.qz(), odomPose.qw(), stamp)); if(!RTABMapActivity.DISABLE_LOG) Log.d(TAG, String.format("pose=%f %f %f arcore %f %f %f cor= %f %f %f stamp=%f", odomPose.tx(), odomPose.ty(), odomPose.tz(), pose.tx(), pose.ty(), pose.tz(), arCoreCorrection.tx(), arCoreCorrection.ty(), arCoreCorrection.tz(), stamp));
RTABMapLib.postCameraPoseEvent(RTABMapActivity.nativeApplication, odomPose.tx(), odomPose.ty(), odomPose.tz(), odomPose.qx(), odomPose.qy(), odomPose.qz(), odomPose.qw(), stamp); RTABMapLib.postCameraPoseEvent(RTABMapActivity.nativeApplication, odomPose.tx(), odomPose.ty(), odomPose.tz(), odomPose.qx(), odomPose.qy(), odomPose.qz(), odomPose.qw(), stamp);
CameraIntrinsics intrinsics = camera.getImageIntrinsics(); CameraIntrinsics intrinsics = camera.getImageIntrinsics();
try{ try{