iOS various updates (#1340)

* Added Data Recording Mode. Added option to filter ARKit localization jumps.

* Implemented max acc relocalization filtering (working on iOS)

* Fixed android build, added re-localization max acceleration parameter

* ARCore Java: Fixed pose of depth not available at stamp requested

* Android log fix

* Added libLAS support

* ios: added LAS support

* updated dep install script to skip libraries already installed

* CameraMobile: Fixed origin not updated if updateOnRender() is used

* Default max opt error increased to 2x to reduce number of loop closures rejected. OptimizerGTSAM: updated gravity noise model to use same sigma for both parameters.

* Updated license

* bump 0.21.7

* updated license date
This commit is contained in:
matlabbe
2024-10-06 17:16:22 -07:00
committed by GitHub
parent 409ef73e56
commit 595f200a89
41 changed files with 2420 additions and 1664 deletions
@@ -56,9 +56,8 @@ public class ARCoreSharedCamera {
};
private static RTABMapActivity mActivity;
public ARCoreSharedCamera(RTABMapActivity c, float arCoreLocalizationFilteringSpeed) {
public ARCoreSharedCamera(RTABMapActivity c) {
mActivity = c;
mARCoreLocalizationFilteringSpeed = arCoreLocalizationFilteringSpeed;
}
// Depth TOF Image.
@@ -72,11 +71,7 @@ public class ARCoreSharedCamera {
// ARCore session that supports camera sharing.
private Session sharedSession;
private Pose previousAnchorPose = null;
private long previousAnchorTimeStamp;
private Pose arCoreCorrection = Pose.IDENTITY;
private Pose odomPose = Pose.IDENTITY;
private float mARCoreLocalizationFilteringSpeed = 1.0f;
// Camera capture session. Used by both non-AR and AR modes.
private CameraCaptureSession captureSession;
@@ -111,6 +106,7 @@ public class ARCoreSharedCamera {
private CaptureRequest.Builder previewCaptureRequestBuilder;
private int cameraTextureId = -1;
private boolean firstFrameReceived = false;
// Image reader that continuously processes CPU images.
public TOF_ImageReader mTOFImageReader = new TOF_ImageReader();
@@ -118,6 +114,8 @@ public class ARCoreSharedCamera {
ByteBuffer mPreviousDepth = null;
double mPreviousDepthStamp = 0.0;
ByteBuffer mPreviousDepth2 = null;
double mPreviousDepthStamp2 = 0.0;
public boolean isDepthSupported() {return mTOFAvailable;}
@@ -633,6 +631,7 @@ public class ARCoreSharedCamera {
}
mTOFImageReader.close();
firstFrameReceived = false;
if(cameraTextureId>=0)
{
@@ -681,14 +680,15 @@ public class ARCoreSharedCamera {
Log.e(TAG, String.format("camera is null!"));
return;
}
boolean lost = false;
if (camera.getTrackingState() != TrackingState.TRACKING) {
final String trackingState = camera.getTrackingState().toString();
Log.e(TAG, String.format("Tracking lost! state=%s", trackingState));
// This will force a new session on the next frame received
RTABMapLib.postCameraPoseEvent(RTABMapActivity.nativeApplication, 0,0,0,0,0,0,0,0);
lost = true;
mActivity.runOnUiThread(new Runnable() {
public void run() {
if(mToast!=null && previousAnchorPose != null)
if(mToast!=null && firstFrameReceived)
{
String msg = "Tracking lost! If you are mapping, you will need to relocalize before continuing.";
if(mToast.getView() == null || !mToast.getView().isShown())
@@ -700,67 +700,14 @@ public class ARCoreSharedCamera {
{
mToast.setText(msg);
}
previousAnchorPose = null;
arCoreCorrection = Pose.IDENTITY;
}
}
});
return;
}
if (frame.getTimestamp() != 0) {
Pose pose = camera.getPose();
// Remove ARCore SLAM corrections by integrating pose from previous frame anchor
if(previousAnchorPose == null || mARCoreLocalizationFilteringSpeed==0)
{
odomPose = pose;
}
else
{
float[] t = previousAnchorPose.inverse().compose(pose).getTranslation();
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)
{
// Only correct the translation to not lose rotation aligned with gravity
arCoreCorrection = arCoreCorrection.compose(previousAnchorPose.compose(pose.inverse()).extractTranslation());
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]));
t = odomPose.getTranslation();
float[] t2 = (arCoreCorrection.compose(pose)).getTranslation();
float[] t3 = previousAnchorPose.getTranslation();
float[] t4 = pose.getTranslation();
Log.e(TAG, String.format("Odom = %f %f %f -> %f %f %f ArCore= %f %f %f -> %f %f %f", t[0], t[1], t[2], t2[0], t2[1], t2[2], t3[0], t3[1], t3[2], t4[0], t4[1], t4[2]));
mActivity.runOnUiThread(new Runnable() {
public void run() {
if(mToast!=null)
{
String msg = String.format("ARCore localization has been suppressed "
+ "because of high speed detected (%f m/s) causing a jump! You can change "
+ "ARCore localization filtering speed in Settings->Mapping if you are "
+ "indeed moving as fast.", speed);
if(mToast.getView() == null || !mToast.getView().isShown())
{
mToast.makeText(mActivity.getApplicationContext(), msg, Toast.LENGTH_LONG).show();
}
else
{
mToast.setText(msg);
}
}
}
});
}
odomPose = arCoreCorrection.compose(pose);
}
previousAnchorPose = pose;
previousAnchorTimeStamp = frame.getTimestamp();
double stamp = (double)frame.getTimestamp()/10e8;
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);
CameraIntrinsics intrinsics = camera.getImageIntrinsics();
try{
Image image = frame.acquireCameraImage();
@@ -803,7 +750,7 @@ public class ARCoreSharedCamera {
camera.getProjectionMatrix(p, 0, 0.1f, 100.0f);
float[] viewMatrix = new float[16];
arCoreCorrection.compose(camera.getDisplayOrientedPose()).inverse().toMatrix(viewMatrix, 0);
camera.getDisplayOrientedPose().inverse().toMatrix(viewMatrix, 0);
float[] quat = new float[4];
rotationMatrixToQuaternion(viewMatrix, quat);
@@ -822,35 +769,47 @@ public class ARCoreSharedCamera {
mPreviousDepth = depth;
mPreviousDepthStamp = depthStamp;
}
if(mPreviousDepth2 == null)
{
mPreviousDepth2 = depth;
mPreviousDepthStamp2 = depthStamp;
}
if(!RTABMapActivity.DISABLE_LOG) Log.d(TAG, String.format("Depth %dx%d len=%dbytes format=%d stamp=%f",
mTOFImageReader.WIDTH, mTOFImageReader.HEIGHT, depth.limit(), ImageFormat.DEPTH16, depthStamp));
if(!RTABMapActivity.DISABLE_LOG) Log.d(TAG, String.format("Depth %dx%d len=%dbytes format=%d stamp=%f previous=%f rgb=%f",
mTOFImageReader.WIDTH, mTOFImageReader.HEIGHT, depth.limit(), ImageFormat.DEPTH16, depthStamp, mPreviousDepthStamp, stamp));
RTABMapLib.postOdometryEventDepth(
RTABMapActivity.nativeApplication,
odomPose.tx(), odomPose.ty(), odomPose.tz(), odomPose.qx(), odomPose.qy(), odomPose.qz(), odomPose.qw(),
lost?0:pose.tx(), lost?0:pose.ty(), lost?0:pose.tz(), lost?0:pose.qx(), lost?0:pose.qy(), lost?0:pose.qz(), lost?0:pose.qw(),
fl[0], fl[1], pp[0], pp[1],
depthIntrinsics[0], depthIntrinsics[1], depthIntrinsics[2], depthIntrinsics[3],
rgbExtrinsics.tx(), rgbExtrinsics.ty(), rgbExtrinsics.tz(), rgbExtrinsics.qx(), rgbExtrinsics.qy(), rgbExtrinsics.qz(), rgbExtrinsics.qw(),
depthExtrinsics.tx(), depthExtrinsics.ty(), depthExtrinsics.tz(), depthExtrinsics.qx(), depthExtrinsics.qy(), depthExtrinsics.qz(), depthExtrinsics.qw(),
stamp,
depthStamp>stamp?mPreviousDepthStamp:depthStamp,
depthStamp<=stamp?depthStamp:mPreviousDepthStamp<=stamp?mPreviousDepthStamp:mPreviousDepthStamp2,
y, u, v, y.limit(), image.getWidth(), image.getHeight(), image.getFormat(),
depthStamp>stamp?mPreviousDepth:depth, depthStamp>stamp?mPreviousDepth.limit():depth.limit(), mTOFImageReader.WIDTH, mTOFImageReader.HEIGHT, ImageFormat.DEPTH16,
depthStamp<=stamp?depth:mPreviousDepthStamp<=stamp?mPreviousDepth:mPreviousDepth2,
depthStamp<=stamp?depth.limit():mPreviousDepthStamp<=stamp?mPreviousDepth.limit():mPreviousDepth2.limit(),
mTOFImageReader.WIDTH, mTOFImageReader.HEIGHT, ImageFormat.DEPTH16,
points, points.limit()/4,
viewMatrix[12], viewMatrix[13], viewMatrix[14], quat[1], quat[2], quat[3], quat[0],
p[0], p[5], p[8], p[9], p[10], p[11], p[14],
texCoord[0],texCoord[1],texCoord[2],texCoord[3],texCoord[4],texCoord[5],texCoord[6],texCoord[7]);
mPreviousDepthStamp = depthStamp;
mPreviousDepth = depth;
// triple buffer in case delay of rgb frame is > 60 ms
if(depthStamp != mPreviousDepthStamp)
{
mPreviousDepthStamp2 = mPreviousDepthStamp;
mPreviousDepth2 = mPreviousDepth;
mPreviousDepthStamp = depthStamp;
mPreviousDepth = depth;
}
}
else
{
RTABMapLib.postOdometryEvent(
RTABMapActivity.nativeApplication,
odomPose.tx(), odomPose.ty(), odomPose.tz(), odomPose.qx(), odomPose.qy(), odomPose.qz(), odomPose.qw(),
lost?0:pose.tx(), lost?0:pose.ty(), lost?0:pose.tz(), lost?0:pose.qx(), lost?0:pose.qy(), lost?0:pose.qz(), lost?0:pose.qw(),
fl[0], fl[1], pp[0], pp[1],
rgbExtrinsics.tx(), rgbExtrinsics.ty(), rgbExtrinsics.tz(), rgbExtrinsics.qx(), rgbExtrinsics.qy(), rgbExtrinsics.qz(), rgbExtrinsics.qw(),
stamp,
@@ -861,6 +820,7 @@ public class ARCoreSharedCamera {
texCoord[0],texCoord[1],texCoord[2],texCoord[3],texCoord[4],texCoord[5],texCoord[6],texCoord[7]);
}
firstFrameReceived = !lost;
image.close();
cloud.close();
@@ -323,6 +323,7 @@ public class RTABMapActivity extends FragmentActivity implements OnClickListener
// touch point.
Display display = getWindowManager().getDefaultDisplay();
display.getSize(mScreenSize);
Log.i(TAG, String.format("Screen resolution: %dx%d", mScreenSize.x, mScreenSize.y));
getWindow().addFlags(WindowManager.LayoutParams.FLAG_KEEP_SCREEN_ON);
getWindow().addFlags(WindowManager.LayoutParams.FLAG_TRANSLUCENT_STATUS);
@@ -1142,6 +1143,7 @@ public class RTABMapActivity extends FragmentActivity implements OnClickListener
RTABMapLib.setDepthFromMotion(nativeApplication, sharedPref.getBoolean(getString(R.string.pref_key_depth_from_motion), Boolean.parseBoolean(getString(R.string.pref_default_depth_from_motion))));
RTABMapLib.setCameraColor(nativeApplication, !sharedPref.getBoolean(getString(R.string.pref_key_fisheye), Boolean.parseBoolean(getString(R.string.pref_default_fisheye))));
RTABMapLib.setAppendMode(nativeApplication, sharedPref.getBoolean(getString(R.string.pref_key_append), Boolean.parseBoolean(getString(R.string.pref_default_append))));
RTABMapLib.setUpstreamRelocalizationAccThr(nativeApplication, Float.parseFloat(sharedPref.getString(getString(R.string.pref_key_arcore_relocalization_acc_thr), getString(R.string.pref_default_arcore_relocalization_acc_thr))));
RTABMapLib.setMappingParameter(nativeApplication, "Rtabmap/DetectionRate", mUpdateRate);
RTABMapLib.setMappingParameter(nativeApplication, "Rtabmap/TimeThr", mTimeThr);
RTABMapLib.setMappingParameter(nativeApplication, "Rtabmap/MemoryThr", memThr);
@@ -1383,9 +1385,7 @@ public class RTABMapActivity extends FragmentActivity implements OnClickListener
if(cameraStartSucess && mCameraDriver == 3)
{
synchronized (this) {
SharedPreferences sharedPref = PreferenceManager.getDefaultSharedPreferences(getActivity());
String arCoreLocalizationFiltering = sharedPref.getString(getString(R.string.pref_key_arcore_localization_filtering_speed), getString(R.string.pref_default_arcore_localization_filtering_speed));
mArCoreCamera = new ARCoreSharedCamera(getActivity(), Float.parseFloat(arCoreLocalizationFiltering));
mArCoreCamera = new ARCoreSharedCamera(getActivity());
mArCoreCamera.setToast(mToast);
mProgressDialog.setTitle("");
mProgressDialog.setMessage(message);
@@ -1775,16 +1775,19 @@ public class RTABMapActivity extends FragmentActivity implements OnClickListener
long currentTime = System.currentTimeMillis()/1000;
if(loopClosureId > 0)
{
if (mToast != null && mToast.getView().isShown()) mToast.cancel();
mToast.setText(String.format("Loop closure detected! (%d/%d inliers)", inliers, matches));
mToast.show();
}
else if(landmarkDetected != 0)
{
if (mToast != null && mToast.getView().isShown()) mToast.cancel();
mToast.setText(String.format("Marker %d detected!", landmarkDetected));
mToast.show();
}
else if(rejected > 0)
{
if (mToast != null && mToast.getView().isShown()) mToast.cancel();
if(inliers >= Integer.parseInt(mMinInliers))
{
if(optimizationMaxError > 0.0f)
@@ -1806,6 +1809,7 @@ public class RTABMapActivity extends FragmentActivity implements OnClickListener
{
if(currentTime - mLastFastMovementNotificationStamp > 3)
{
if (mToast != null && mToast.getView().isShown()) mToast.cancel();
mToast.setText("Move slower... blurry images are not added to map (\"Settings->Mapping...->Maximum Motion Speed\" is enabled).");
mToast.show();
}
@@ -2057,7 +2061,14 @@ public class RTABMapActivity extends FragmentActivity implements OnClickListener
* "Unknown"
*/
String str = null;
if(key.equals("TangoServiceException"))
if(key.equals("UpstreamRelocationFiltered"))
{
if(mItemDebugVisibility != null && mItemDebugVisibility.isChecked()) {
str = String.format("%s re-localization filtered because an acceleration of %s has been detected, which is over current threshold set in the settings.",
mCameraDriver == 2?"AREngine":"ARCore", value);
}
}
else if(key.equals("TangoServiceException"))
str = String.format("Tango service exception: %s", value);
else if(key.equals("FisheyeOverExposed"))
;//str = String.format("The fisheye image is over exposed with average pixel value %s px.", value);
@@ -3024,7 +3035,6 @@ public class RTABMapActivity extends FragmentActivity implements OnClickListener
}
else
{
RTABMapLib.openDatabase(nativeApplication, tmpDatabase, databaseInMemory, false, true);
if(!(mState == State.STATE_CAMERA || mState ==State.STATE_MAPPING))
@@ -76,6 +76,7 @@ public class RTABMapLib
public static native void setDepthFromMotion(long nativeApplication, boolean enabled);
public static native void setCameraColor(long nativeApplication, boolean enabled);
public static native void setAppendMode(long nativeApplication, boolean enabled);
public static native void setUpstreamRelocalizationAccThr(long nativeApplication, float value);
public static native void setDataRecorderMode(long nativeApplication, boolean enabled);
public static native void setMaxCloudDepth(long nativeApplication, float value);
public static native void setMinCloudDepth(long nativeApplication, float value);
@@ -135,7 +136,6 @@ public class RTABMapLib
public static native float getUpdateTime(long nativeApplication);
public static native int getLoopClosureId(long nativeApplication);
public static native void postCameraPoseEvent(long nativeApplication, float x, float y, float z, float qx, float qy, float qz, float qw, double stamp);
public static native void postOdometryEvent(long nativeApplication,
float x, float y, float z, float qx, float qy, float qz, float qw,
float rgb_fx, float rgb_fy, float rgb_cx, float rgb_cy,
@@ -222,7 +222,7 @@ public class SettingsActivity extends PreferenceActivity implements OnSharedPref
((Preference)findPreference(getString(R.string.pref_key_background_color))).setSummary("("+((ListPreference)findPreference(getString(R.string.pref_key_background_color))).getEntry() + ") "+getString(R.string.pref_summary_background_color));
((Preference)findPreference(getString(R.string.pref_key_rendering_texture_decimation))).setSummary("("+((ListPreference)findPreference(getString(R.string.pref_key_rendering_texture_decimation))).getEntry() + ") "+getString(R.string.pref_summary_rendering_texture_decimation));
((Preference)findPreference(getString(R.string.pref_key_arcore_localization_filtering_speed))).setSummary("("+((ListPreference)findPreference(getString(R.string.pref_key_arcore_localization_filtering_speed))).getEntry() + ") "+getString(R.string.pref_summary_arcore_localization_filtering_speed));
((Preference)findPreference(getString(R.string.pref_key_arcore_relocalization_acc_thr))).setSummary("("+((ListPreference)findPreference(getString(R.string.pref_key_arcore_relocalization_acc_thr))).getEntry() + ") "+getString(R.string.pref_summary_arcore_relocalization_acc_thr));
((Preference)findPreference(getString(R.string.pref_key_update_rate))).setSummary("("+((ListPreference)findPreference(getString(R.string.pref_key_update_rate))).getEntry() + ") "+getString(R.string.pref_summary_update_rate));
((Preference)findPreference(getString(R.string.pref_key_max_speed))).setSummary("("+((ListPreference)findPreference(getString(R.string.pref_key_max_speed))).getEntry() + ") "+getString(R.string.pref_summary_max_speed));
((Preference)findPreference(getString(R.string.pref_key_time_thr))).setSummary("("+((ListPreference)findPreference(getString(R.string.pref_key_time_thr))).getEntry() + ") "+getString(R.string.pref_summary_time_thr));
@@ -285,7 +285,7 @@ public class SettingsActivity extends PreferenceActivity implements OnSharedPref
if(key.compareTo(getString(R.string.pref_key_background_color))==0) pref.setSummary("("+((ListPreference)pref).getEntry() + ") "+getString(R.string.pref_summary_background_color));
if(key.compareTo(getString(R.string.pref_key_rendering_texture_decimation))==0) pref.setSummary("("+((ListPreference)pref).getEntry() + ") "+getString(R.string.pref_summary_rendering_texture_decimation));
if(key.compareTo(getString(R.string.pref_key_arcore_localization_filtering_speed))==0) pref.setSummary("("+((ListPreference)pref).getEntry() + ") "+getString(R.string.pref_summary_arcore_localization_filtering_speed));
if(key.compareTo(getString(R.string.pref_key_arcore_relocalization_acc_thr))==0) pref.setSummary("("+((ListPreference)pref).getEntry() + ") "+getString(R.string.pref_summary_arcore_relocalization_acc_thr));
if(key.compareTo(getString(R.string.pref_key_update_rate))==0) pref.setSummary("("+((ListPreference)pref).getEntry() + ") "+getString(R.string.pref_summary_update_rate));
if(key.compareTo(getString(R.string.pref_key_max_speed))==0) pref.setSummary("("+((ListPreference)pref).getEntry() + ") "+getString(R.string.pref_summary_max_speed));
if(key.compareTo(getString(R.string.pref_key_time_thr))==0) pref.setSummary("("+((ListPreference)pref).getEntry() + ") "+getString(R.string.pref_summary_time_thr));