i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-10-07 02:27:47 +08:00
Code Issues Packages Projects Releases Wiki Activity

Refactored Tango and ARCore texture management. Tango: added support with First-person AR mode. ARCoreJava: Added localization filtering speed option (avoid ARCore jumps), also manage when ARCore is lost (new session is created, relocalization needed to continue mapping).

Browse Source
This commit is contained in:
matlabbe
2021-06-15 20:45:45 -04:00
parent 696216b35b
commit a9d4fc9291
14 changed files with 619 additions and 418 deletions
Download Patch File Download Diff File Expand all files Collapse all files
app/android/src/com/introlab/rtabmap/ARCoreSharedCamera.java
+135 -14
View File
@@ -8,7 +8,10 @@ import java.util.EnumSet;
import java.util.List;
import java.util.concurrent.atomic.AtomicBoolean;
import com.google.ar.core.Anchor;
import com.google.ar.core.Camera;
import com.google.ar.core.CameraConfig;
import com.google.ar.core.CameraConfigFilter;
import com.google.ar.core.CameraIntrinsics;
import com.google.ar.core.Config;
import com.google.ar.core.Coordinates2d;
@@ -41,6 +44,7 @@ import android.os.HandlerThread;
import android.support.annotation.NonNull;
import android.util.Log;
import android.view.Surface;
import android.widget.Toast;
public class ARCoreSharedCamera {
@@ -52,8 +56,9 @@ public class ARCoreSharedCamera {
};
private static RTABMapActivity mActivity;
public ARCoreSharedCamera(RTABMapActivity c) {
public ARCoreSharedCamera(RTABMapActivity c, float arCoreLocalizationFilteringSpeed) {
mActivity = c;
mARCoreLocalizationFilteringSpeed = arCoreLocalizationFilteringSpeed;
}
// Depth TOF Image.
@@ -66,6 +71,12 @@ 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;
@@ -83,6 +94,8 @@ public class ARCoreSharedCamera {
// ARCore shared camera instance, obtained from ARCore session that supports sharing.
private SharedCamera sharedCamera;
private Toast mToast = null;
// Camera ID for the camera used by ARCore.
private String cameraId;
@@ -105,6 +118,10 @@ public class ARCoreSharedCamera {
public boolean isDepthSupported() {return mTOFAvailable;}
public void setToast(Toast toast)
{
mToast = toast;
}
// Camera device state callback.
private final CameraDevice.StateCallback cameraDeviceCallback =
@@ -355,6 +372,32 @@ public class ARCoreSharedCamera {
Log.e(TAG, "Failed to create ARCore session that supports camera sharing", e);
return false;
}
// First obtain the session handle before getting the list of various camera configs.
// Create filter here with desired fps filters.
CameraConfigFilter cameraConfigFilter = new CameraConfigFilter(sharedSession);
CameraConfig[] cameraConfigs = sharedSession.getSupportedCameraConfigs(cameraConfigFilter).toArray(new CameraConfig[0]);
Log.i(TAG, "Size of supported CameraConfigs list is " + cameraConfigs.length);
// Determine the highest and lowest CPU resolutions.
int highestResolutionIndex=-1;
int highestResolution = 0;
for(int i=0; i<cameraConfigs.length; ++i)
{
Log.i(TAG, "Camera ID: " + cameraConfigs[i].getCameraId());
Log.i(TAG, "Resolution: " + cameraConfigs[i].getImageSize().getWidth() + "x" + cameraConfigs[i].getImageSize().getHeight());
if(highestResolution == 0 || highestResolution < cameraConfigs[i].getImageSize().getWidth())
{
highestResolutionIndex = i;
highestResolution = cameraConfigs[i].getImageSize().getWidth();
}
}
if(highestResolutionIndex>=0)
{
//Log.i(TAG, "Setting camera resolution to " + cameraConfigs[highestResolutionIndex].getImageSize().getWidth() + "x" + cameraConfigs[highestResolutionIndex].getImageSize().getHeight());
//FIXME: Can we make it work to use HD rgb images? To avoid this error "CaptureRequest contains unconfigured Input/Output Surface!"
//sharedSession.setCameraConfig(cameraConfigs[highestResolutionIndex]);
}
// Enable auto focus mode while ARCore is running.
Config config = sharedSession.getConfig();
@@ -371,11 +414,11 @@ public class ARCoreSharedCamera {
sharedCamera = sharedSession.getSharedCamera();
// Store the ID of the camera used by ARCore.
cameraId = sharedSession.getCameraConfig().getCameraId();
Log.d(TAG, "Shared camera ID: " + cameraId);
mTOFAvailable = false;
// Store a reference to the camera system service.
cameraManager = (CameraManager) mActivity.getSystemService(Context.CAMERA_SERVICE);
@@ -573,7 +616,8 @@ public class ARCoreSharedCamera {
Log.w(TAG, "close()");
if (sharedSession != null) {
sharedSession.pause();
sharedSession.close();
sharedSession = null;
}
if (captureSession != null) {
@@ -582,6 +626,7 @@ public class ARCoreSharedCamera {
}
if (cameraDevice != null) {
cameraDevice.close();
cameraDevice = null;
}
mTOFImageReader.close();
@@ -596,14 +641,15 @@ public class ARCoreSharedCamera {
public void setDisplayGeometry(int rotation, int width, int height)
{
sharedSession.setDisplayGeometry(rotation, width, height);
if(sharedSession!=null)
sharedSession.setDisplayGeometry(rotation, width, height);
}
/*************************************************** ONDRAWFRAME ARCORE ************************************************************* */
// Draw frame when in AR mode. Called on the GL thread.
public void updateGL() throws CameraNotAvailableException {
if(!mReady.get())
if(!mReady.get() || sharedSession == null)
{
return;
}
@@ -624,18 +670,93 @@ public class ARCoreSharedCamera {
camera = frame.getCamera();
}else
{
Log.e(TAG, String.format("frame.getCamera() null!"));
return;
}
if (camera == null) return;
// If not tracking, don't draw 3D objects.
if (camera.getTrackingState() == TrackingState.PAUSED) return;
if (camera == null) {
Log.e(TAG, String.format("camera is null!"));
return;
}
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);
mActivity.runOnUiThread(new Runnable() {
public void run() {
if(mToast!=null && previousAnchorPose != null)
{
String msg = "Tracking lost! If you are mapping, you will need to relocalize before continuing.";
if(!mToast.getView().isShown())
{
mToast.makeText(mActivity.getApplicationContext(),
msg, Toast.LENGTH_LONG).show();
}
else
{
mToast.setText(msg);
}
previousAnchorPose = null;
}
}
});
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)
{
arCoreCorrection = arCoreCorrection.compose(previousAnchorPose).compose(pose.inverse());
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().isShown())
{
mToast.makeText(mActivity.getApplicationContext(), msg, Toast.LENGTH_LONG).show();
}
else
{
mToast.setText(msg);
}
previousAnchorPose = null;
}
}
});
}
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 q=%f %f %f %f stamp=%f", pose.tx(), pose.ty(), pose.tz(), pose.qx(), pose.qy(), pose.qz(), pose.qw(), stamp));
RTABMapLib.postCameraPoseEvent(RTABMapActivity.nativeApplication, pose.tx(), pose.ty(), pose.tz(), pose.qx(), pose.qy(), pose.qz(), pose.qw(), stamp);
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));
RTABMapLib.postCameraPoseEvent(RTABMapActivity.nativeApplication, odomPose.tx(), odomPose.ty(), odomPose.tz(), odomPose.qx(), odomPose.qy(), odomPose.qz(), odomPose.qw(), stamp);
CameraIntrinsics intrinsics = camera.getImageIntrinsics();
try{
@@ -679,7 +800,7 @@ public class ARCoreSharedCamera {
camera.getProjectionMatrix(p, 0, 0.1f, 100.0f);
float[] viewMatrix = new float[16];
camera.getViewMatrix(viewMatrix, 0);
arCoreCorrection.compose(camera.getDisplayOrientedPose()).inverse().toMatrix(viewMatrix, 0);
float[] quat = new float[4];
rotationMatrixToQuaternion(viewMatrix, quat);
@@ -698,7 +819,7 @@ public class ARCoreSharedCamera {
RTABMapLib.postOdometryEventDepth(
RTABMapActivity.nativeApplication,
pose.tx(), pose.ty(), pose.tz(), pose.qx(), pose.qy(), pose.qz(), pose.qw(),
odomPose.tx(), odomPose.ty(), odomPose.tz(), odomPose.qx(), odomPose.qy(), odomPose.qz(), odomPose.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(),
@@ -716,7 +837,7 @@ public class ARCoreSharedCamera {
{
RTABMapLib.postOdometryEvent(
RTABMapActivity.nativeApplication,
pose.tx(), pose.ty(), pose.tz(), pose.qx(), pose.qy(), pose.qz(), pose.qw(),
odomPose.tx(), odomPose.ty(), odomPose.tz(), odomPose.qx(), odomPose.qy(), odomPose.qz(), odomPose.qw(),
fl[0], fl[1], pp[0], pp[1],
rgbExtrinsics.tx(), rgbExtrinsics.ty(), rgbExtrinsics.tz(), rgbExtrinsics.qx(), rgbExtrinsics.qy(), rgbExtrinsics.qz(), rgbExtrinsics.qw(),
stamp,
app/android/src/com/introlab/rtabmap/RTABMapActivity.java
+4 -1
View File
@@ -1236,7 +1236,10 @@ public class RTABMapActivity extends FragmentActivity implements OnClickListener
if(cameraStartSucess && mCameraDriver == 3)
{
synchronized (this) {
mArCoreCamera = new ARCoreSharedCamera(getActivity());
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.setToast(mToast);
mProgressDialog.setTitle("");
mProgressDialog.setMessage(message);
mProgressDialog.show();
app/android/src/com/introlab/rtabmap/Renderer.java
+13 -4
View File
@@ -91,17 +91,23 @@ public class Renderer implements GLSurfaceView.Renderer {
synchronized (this) {
if(mActivity.nativeApplication != 0)
{
int step = 0;
try
{
if(mCamera!=null)
{
step=1;
mCamera.updateGL();
}
step=2;
final int value = RTABMapLib.render(mActivity.nativeApplication);
if(mTextManager!=null)
{
step=3;
if(mTextChanged)
{
mTextChanged = false;
@@ -125,7 +131,7 @@ public class Renderer implements GLSurfaceView.Renderer {
Matrix.translateM(mvp, 0, mtrxProjectionAndView, 0, 0, mOffset, 0);
mTextManager.Draw(mvp);
}
step=4;
if(value != 0 && mProgressDialog != null && mProgressDialog.isShowing())
{
mActivity.runOnUiThread(new Runnable() {
@@ -142,7 +148,7 @@ public class Renderer implements GLSurfaceView.Renderer {
public void run() {
if(mToast!=null)
{
mToast.makeText(mActivity, String.format("Out of Memory!"), Toast.LENGTH_SHORT).show();
mToast.makeText(mActivity.getApplicationContext(), String.format("Out of Memory!"), Toast.LENGTH_SHORT).show();
}
}
});
@@ -153,7 +159,7 @@ public class Renderer implements GLSurfaceView.Renderer {
public void run() {
if(mToast!=null)
{
mToast.makeText(mActivity, String.format("Rendering Error!"), Toast.LENGTH_SHORT).show();
mToast.makeText(mActivity.getApplicationContext(), String.format("Rendering Error!"), Toast.LENGTH_SHORT).show();
}
}
});
@@ -161,11 +167,14 @@ public class Renderer implements GLSurfaceView.Renderer {
}
catch(final Exception e)
{
final int stepF = step;
mActivity.runOnUiThread(new Runnable() {
public void run() {
if(mToast!=null)
{
mToast.makeText(mActivity, String.format("Rendering error! %s", e.getMessage()), Toast.LENGTH_SHORT).show();
String msg = String.format("Rendering error! (exception=%s) step=%d", e.getMessage(), stepF);
Log.e("RTABMapActivity", msg);
mToast.makeText(mActivity.getApplicationContext(), msg, Toast.LENGTH_LONG).show();
}
}
app/android/src/com/introlab/rtabmap/SettingsActivity.java
+2
View File
@@ -222,6 +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_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));
@@ -284,6 +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_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));