mirror of
https://github.com/introlab/rtabmap.git
synced 2026-09-08 12:30:20 +08:00
Compare commits
3
Commits
| Author | SHA1 | Date | |
|---|---|---|---|
|
|
e4873e770a | ||
|
|
3148ae2d76 | ||
|
|
8819b42f24 |
@@ -57,7 +57,7 @@
|
||||
android:excludeFromRecents="true"
|
||||
android:exported="false"
|
||||
android:launchMode="singleTop"
|
||||
android:theme="@android:style/Theme.Material.Light.Dialog.Alert" />
|
||||
android:theme="@style/ThemeApp" />
|
||||
|
||||
<provider
|
||||
android:name="android.support.v4.content.FileProvider"
|
||||
|
||||
@@ -1,588 +0,0 @@
|
||||
package com.introlab.rtabmap;
|
||||
|
||||
import java.nio.ByteBuffer;
|
||||
import java.nio.FloatBuffer;
|
||||
import java.util.ArrayList;
|
||||
import java.util.Arrays;
|
||||
import java.util.EnumSet;
|
||||
import java.util.List;
|
||||
import java.util.concurrent.atomic.AtomicBoolean;
|
||||
|
||||
import com.google.ar.core.Camera;
|
||||
import com.google.ar.core.CameraIntrinsics;
|
||||
import com.google.ar.core.Config;
|
||||
import com.google.ar.core.Frame;
|
||||
import com.google.ar.core.ImageMetadata;
|
||||
import com.google.ar.core.PointCloud;
|
||||
import com.google.ar.core.Pose;
|
||||
import com.google.ar.core.Session;
|
||||
import com.google.ar.core.SharedCamera;
|
||||
import com.google.ar.core.TrackingState;
|
||||
import com.google.ar.core.exceptions.CameraNotAvailableException;
|
||||
import com.google.ar.core.exceptions.NotYetAvailableException;
|
||||
import com.google.ar.core.exceptions.UnavailableException;
|
||||
|
||||
import android.content.Context;
|
||||
import android.graphics.ImageFormat;
|
||||
import android.hardware.camera2.CameraAccessException;
|
||||
import android.hardware.camera2.CameraCaptureSession;
|
||||
import android.hardware.camera2.CameraCharacteristics;
|
||||
import android.hardware.camera2.CameraDevice;
|
||||
import android.hardware.camera2.CameraManager;
|
||||
import android.hardware.camera2.CaptureFailure;
|
||||
import android.hardware.camera2.CaptureRequest;
|
||||
import android.hardware.camera2.TotalCaptureResult;
|
||||
import android.media.Image;
|
||||
import android.opengl.GLES20;
|
||||
import android.opengl.GLSurfaceView;
|
||||
import android.os.Handler;
|
||||
import android.os.HandlerThread;
|
||||
import android.support.annotation.NonNull;
|
||||
import android.util.Log;
|
||||
import android.view.Surface;
|
||||
|
||||
public class ARCoreSharedCamera {
|
||||
|
||||
public static final String TAG = ARCoreSharedCamera.class.getSimpleName();
|
||||
|
||||
|
||||
private static RTABMapActivity mActivity;
|
||||
public ARCoreSharedCamera(RTABMapActivity c) {
|
||||
mActivity = c;
|
||||
}
|
||||
|
||||
// Depth TOF Image.
|
||||
// Use 240 * 180 for now, hardcoded for Huawei P30 Pro
|
||||
private static final int DEPTH_WIDTH = 240;
|
||||
private static final int DEPTH_HEIGHT = 180;
|
||||
|
||||
// GL Surface used to draw camera preview image.
|
||||
public GLSurfaceView surfaceView;
|
||||
|
||||
// ARCore session that supports camera sharing.
|
||||
private Session sharedSession;
|
||||
|
||||
// Camera capture session. Used by both non-AR and AR modes.
|
||||
private CameraCaptureSession captureSession;
|
||||
|
||||
// Reference to the camera system service.
|
||||
private CameraManager cameraManager;
|
||||
|
||||
// Camera device. Used by both non-AR and AR modes.
|
||||
private CameraDevice cameraDevice;
|
||||
|
||||
// Looper handler thread.
|
||||
private HandlerThread backgroundThread;
|
||||
// Looper handler.
|
||||
private Handler backgroundHandler;
|
||||
|
||||
// ARCore shared camera instance, obtained from ARCore session that supports sharing.
|
||||
private SharedCamera sharedCamera;
|
||||
|
||||
// Camera ID for the camera used by ARCore.
|
||||
private String cameraId;
|
||||
|
||||
private AtomicBoolean mReady = new AtomicBoolean(false);
|
||||
|
||||
// Camera preview capture request builder
|
||||
private CaptureRequest.Builder previewCaptureRequestBuilder;
|
||||
|
||||
private int cameraTextureId = -1;
|
||||
|
||||
// Image reader that continuously processes CPU images.
|
||||
public TOF_ImageReader mTOFImageReader = new TOF_ImageReader();
|
||||
private boolean mTOFAvailable = false;
|
||||
|
||||
public boolean isDepthSupported() {return mTOFAvailable;}
|
||||
|
||||
// Camera device state callback.
|
||||
private final CameraDevice.StateCallback cameraDeviceCallback =
|
||||
new CameraDevice.StateCallback() {
|
||||
@Override
|
||||
public void onOpened(@NonNull CameraDevice cameraDevice) {
|
||||
Log.d(TAG, "Camera device ID " + cameraDevice.getId() + " opened.");
|
||||
ARCoreSharedCamera.this.cameraDevice = cameraDevice;
|
||||
createCameraPreviewSession();
|
||||
}
|
||||
|
||||
@Override
|
||||
public void onClosed(@NonNull CameraDevice cameraDevice) {
|
||||
Log.d(TAG, "Camera device ID " + cameraDevice.getId() + " closed.");
|
||||
ARCoreSharedCamera.this.cameraDevice = null;
|
||||
}
|
||||
|
||||
@Override
|
||||
public void onDisconnected(@NonNull CameraDevice cameraDevice) {
|
||||
Log.w(TAG, "Camera device ID " + cameraDevice.getId() + " disconnected.");
|
||||
cameraDevice.close();
|
||||
ARCoreSharedCamera.this.cameraDevice = null;
|
||||
}
|
||||
|
||||
@Override
|
||||
public void onError(@NonNull CameraDevice cameraDevice, int error) {
|
||||
Log.e(TAG, "Camera device ID " + cameraDevice.getId() + " error " + error);
|
||||
cameraDevice.close();
|
||||
ARCoreSharedCamera.this.cameraDevice = null;
|
||||
}
|
||||
};
|
||||
|
||||
// Repeating camera capture session state callback.
|
||||
CameraCaptureSession.StateCallback cameraCaptureCallback =
|
||||
new CameraCaptureSession.StateCallback() {
|
||||
|
||||
// Called when the camera capture session is first configured after the app
|
||||
// is initialized, and again each time the activity is resumed.
|
||||
@Override
|
||||
public void onConfigured(@NonNull CameraCaptureSession session) {
|
||||
Log.d(TAG, "Camera capture session configured.");
|
||||
captureSession = session;
|
||||
setRepeatingCaptureRequest();
|
||||
}
|
||||
|
||||
@Override
|
||||
public void onSurfacePrepared(
|
||||
@NonNull CameraCaptureSession session, @NonNull Surface surface) {
|
||||
Log.d(TAG, "Camera capture surface prepared.");
|
||||
}
|
||||
|
||||
@Override
|
||||
public void onReady(@NonNull CameraCaptureSession session) {
|
||||
Log.d(TAG, "Camera capture session ready.");
|
||||
}
|
||||
|
||||
@Override
|
||||
public void onActive(@NonNull CameraCaptureSession session) {
|
||||
Log.d(TAG, "Camera capture session active.");
|
||||
resumeARCore();
|
||||
}
|
||||
|
||||
@Override
|
||||
public void onClosed(@NonNull CameraCaptureSession session) {
|
||||
Log.d(TAG, "Camera capture session closed.");
|
||||
}
|
||||
|
||||
@Override
|
||||
public void onConfigureFailed(@NonNull CameraCaptureSession session) {
|
||||
Log.e(TAG, "Failed to configure camera capture session.");
|
||||
}
|
||||
};
|
||||
|
||||
// Repeating camera capture session capture callback.
|
||||
private final CameraCaptureSession.CaptureCallback captureSessionCallback =
|
||||
new CameraCaptureSession.CaptureCallback() {
|
||||
|
||||
@Override
|
||||
public void onCaptureCompleted(
|
||||
@NonNull CameraCaptureSession session,
|
||||
@NonNull CaptureRequest request,
|
||||
@NonNull TotalCaptureResult result) {
|
||||
Log.i(TAG, "onCaptureCompleted");
|
||||
}
|
||||
|
||||
//@Override // android 23
|
||||
public void onCaptureBufferLost(
|
||||
@NonNull CameraCaptureSession session,
|
||||
@NonNull CaptureRequest request,
|
||||
@NonNull Surface target,
|
||||
long frameNumber) {
|
||||
Log.e(TAG, "onCaptureBufferLost: " + frameNumber);
|
||||
}
|
||||
|
||||
@Override
|
||||
public void onCaptureFailed(
|
||||
@NonNull CameraCaptureSession session,
|
||||
@NonNull CaptureRequest request,
|
||||
@NonNull CaptureFailure failure) {
|
||||
Log.e(TAG, "onCaptureFailed: " + failure.getFrameNumber() + " " + failure.getReason());
|
||||
}
|
||||
|
||||
@Override
|
||||
public void onCaptureSequenceAborted(
|
||||
@NonNull CameraCaptureSession session, int sequenceId) {
|
||||
Log.e(TAG, "onCaptureSequenceAborted: " + sequenceId + " " + session);
|
||||
}
|
||||
};
|
||||
|
||||
private void resumeARCore() {
|
||||
// Ensure that session is valid before triggering ARCore resume. Handles the case where the user
|
||||
// manually uninstalls ARCore while the app is paused and then resumes.
|
||||
if (sharedSession == null) {
|
||||
return;
|
||||
}
|
||||
|
||||
try {
|
||||
Log.i(TAG, "Resume ARCore.");
|
||||
// Resume ARCore.
|
||||
sharedSession.resume();
|
||||
// Set capture session callback while in AR mode.
|
||||
sharedCamera.setCaptureCallback(captureSessionCallback, backgroundHandler);
|
||||
} catch (CameraNotAvailableException e) {
|
||||
Log.e(TAG, "Failed to resume ARCore session", e);
|
||||
return;
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
|
||||
// Called when starting non-AR mode or switching to non-AR mode.
|
||||
// Also called when app starts in AR mode, or resumes in AR mode.
|
||||
private void setRepeatingCaptureRequest() {
|
||||
try {
|
||||
captureSession.setRepeatingRequest(
|
||||
previewCaptureRequestBuilder.build(), captureSessionCallback, backgroundHandler);
|
||||
} catch (CameraAccessException e) {
|
||||
Log.e(TAG, "Failed to set repeating request", e);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
private void createCameraPreviewSession() {
|
||||
Log.e(TAG, "createCameraPreviewSession: " + "starting camera preview session.");
|
||||
try {
|
||||
// Note that isGlAttached will be set to true in AR mode in onDrawFrame().
|
||||
sharedSession.setCameraTextureName(cameraTextureId);
|
||||
|
||||
// Create an ARCore compatible capture request using `TEMPLATE_RECORD`.
|
||||
previewCaptureRequestBuilder = cameraDevice.createCaptureRequest(CameraDevice.TEMPLATE_RECORD);
|
||||
|
||||
// Build surfaces list, starting with ARCore provided surfaces.
|
||||
List<Surface> surfaceList = sharedCamera.getArCoreSurfaces();
|
||||
Log.e(TAG, " createCameraPreviewSession: " + "surfaceList: sharedCamera.getArCoreSurfaces(): " + surfaceList.size());
|
||||
|
||||
// Add a CPU image reader surface. On devices that don't support CPU image access, the image
|
||||
// may arrive significantly later, or not arrive at all.
|
||||
if (mTOFAvailable) surfaceList.add(mTOFImageReader.imageReader.getSurface());
|
||||
// Surface list should now contain three surfacemReadymReadys:
|
||||
// 0. sharedCamera.getSurfaceTexture()
|
||||
// 1. …
|
||||
// 2. depthImageReader.getSurface()
|
||||
|
||||
// Add ARCore surfaces and CPU image surface targets.
|
||||
for (Surface surface : surfaceList) {
|
||||
previewCaptureRequestBuilder.addTarget(surface);
|
||||
}
|
||||
|
||||
// Wrap our callback in a shared camera callback.
|
||||
CameraCaptureSession.StateCallback wrappedCallback = sharedCamera.createARSessionStateCallback(cameraCaptureCallback, backgroundHandler);
|
||||
|
||||
// Create camera capture session for camera preview using ARCore wrapped callback.
|
||||
cameraDevice.createCaptureSession(surfaceList, wrappedCallback, backgroundHandler);
|
||||
|
||||
mReady.set(true);
|
||||
} catch (CameraAccessException e) {
|
||||
Log.e(TAG, "CameraAccessException", e);
|
||||
}
|
||||
|
||||
|
||||
}
|
||||
|
||||
// Start background handler thread, used to run callbacks without blocking UI thread.
|
||||
private void startBackgroundThread() {
|
||||
backgroundThread = new HandlerThread("sharedCameraBackground");
|
||||
backgroundThread.start();
|
||||
backgroundHandler = new Handler(backgroundThread.getLooper());
|
||||
mTOFImageReader.startBackgroundThread();
|
||||
}
|
||||
|
||||
// Stop background handler thread.
|
||||
private void stopBackgroundThread() {
|
||||
if (backgroundThread != null) {
|
||||
backgroundThread.quitSafely();
|
||||
try {
|
||||
backgroundThread.join();
|
||||
backgroundThread = null;
|
||||
backgroundHandler = null;
|
||||
} catch (InterruptedException e) {
|
||||
Log.e(TAG, "Interrupted while trying to join background handler thread", e);
|
||||
}
|
||||
}
|
||||
mTOFImageReader.stopBackgroundThread();
|
||||
}
|
||||
|
||||
private long mPreviousTime = 0;
|
||||
|
||||
// Perform various checks, then open camera device and create CPU image reader.
|
||||
public boolean openCamera() {
|
||||
|
||||
close();
|
||||
|
||||
startBackgroundThread();
|
||||
|
||||
mPreviousTime = System.currentTimeMillis();
|
||||
|
||||
if(cameraTextureId == -1)
|
||||
{
|
||||
int[] textures = new int[1];
|
||||
GLES20.glGenTextures(1, textures, 0);
|
||||
cameraTextureId = textures[0];
|
||||
}
|
||||
|
||||
Log.v(TAG + " opencamera: ", "Perform various checks, then open camera device and create CPU image reader.");
|
||||
// Don't open camera if already opened.
|
||||
if (cameraDevice != null) {
|
||||
return false;
|
||||
}
|
||||
|
||||
if (sharedSession == null) {
|
||||
try {
|
||||
// Create ARCore session that supports camera sharing.
|
||||
sharedSession = new Session(mActivity, EnumSet.of(Session.Feature.SHARED_CAMERA));
|
||||
} catch (UnavailableException e) {
|
||||
Log.e(TAG, "Failed to create ARCore session that supports camera sharing", e);
|
||||
return false;
|
||||
}
|
||||
|
||||
// Enable auto focus mode while ARCore is running.
|
||||
Config config = sharedSession.getConfig();
|
||||
config.setFocusMode(Config.FocusMode.FIXED);
|
||||
config.setUpdateMode(Config.UpdateMode.LATEST_CAMERA_IMAGE);
|
||||
config.setPlaneFindingMode(Config.PlaneFindingMode.DISABLED);
|
||||
config.setLightEstimationMode(Config.LightEstimationMode.DISABLED);
|
||||
//config.setCloudAnchorMode(Config.CloudAnchorMode.ENABLED);
|
||||
sharedSession.configure(config);
|
||||
|
||||
}
|
||||
|
||||
// Store the ARCore shared camera reference.
|
||||
sharedCamera = sharedSession.getSharedCamera();
|
||||
// Store the ID of the camera used by ARCore.
|
||||
cameraId = sharedSession.getCameraConfig().getCameraId();
|
||||
initCamera(mActivity, cameraId, 1);
|
||||
ArrayList<String> resolutions;
|
||||
|
||||
mTOFAvailable = false;
|
||||
|
||||
resolutions = getResolutions(mActivity, cameraId, ImageFormat.DEPTH16);
|
||||
if (resolutions != null) {
|
||||
for( String temp : resolutions) {
|
||||
Log.e(TAG + "DEPTH16 resolution: ", temp);
|
||||
};
|
||||
if (resolutions.size()>0) mTOFAvailable = true;
|
||||
}
|
||||
|
||||
// Color CPU Image.
|
||||
// Use the currently configured CPU image size.
|
||||
//Size desiredCPUImageSize = sharedSession.getCameraConfig().getImageSize();
|
||||
|
||||
if (mTOFAvailable) mTOFImageReader.createImageReader(DEPTH_WIDTH, DEPTH_HEIGHT);
|
||||
|
||||
// When ARCore is running, make sure it also updates our CPU image surface.
|
||||
if (mTOFAvailable) {
|
||||
sharedCamera.setAppSurfaces(this.cameraId, Arrays.asList(mTOFImageReader.imageReader.getSurface()));
|
||||
}
|
||||
|
||||
try {
|
||||
|
||||
// Wrap our callback in a shared camera callback.
|
||||
CameraDevice.StateCallback wrappedCallback = sharedCamera.createARDeviceStateCallback(cameraDeviceCallback, backgroundHandler);
|
||||
|
||||
// Store a reference to the camera system service.
|
||||
cameraManager = (CameraManager) mActivity.getSystemService(Context.CAMERA_SERVICE);
|
||||
|
||||
// Get the characteristics for the ARCore camera.
|
||||
//CameraCharacteristics characteristics = cameraManager.getCameraCharacteristics(this.cameraId);
|
||||
|
||||
// Open the camera device using the ARCore wrapped callback.
|
||||
cameraManager.openCamera(cameraId, wrappedCallback, backgroundHandler);
|
||||
|
||||
} catch (CameraAccessException e) {
|
||||
Log.e(TAG, "Failed to open camera", e);
|
||||
return false;
|
||||
} catch (IllegalArgumentException e) {
|
||||
Log.e(TAG, "Failed to open camera", e);
|
||||
return false;
|
||||
} catch (SecurityException e) {
|
||||
Log.e(TAG, "Failed to open camera", e);
|
||||
return false;
|
||||
}
|
||||
|
||||
Log.i(TAG, " opencamera: TOF_available: " + mTOFAvailable);
|
||||
return true;
|
||||
}
|
||||
|
||||
|
||||
// Close the camera device.
|
||||
public void close() {
|
||||
|
||||
if (sharedSession != null) {
|
||||
sharedSession.pause();
|
||||
}
|
||||
|
||||
if (captureSession != null) {
|
||||
captureSession.close();
|
||||
captureSession = null;
|
||||
}
|
||||
if (cameraDevice != null) {
|
||||
cameraDevice.close();
|
||||
}
|
||||
|
||||
if (mTOFImageReader.imageReader != null) {
|
||||
mTOFImageReader.imageReader.close();
|
||||
mTOFImageReader.imageReader = null;
|
||||
}
|
||||
|
||||
if(cameraTextureId>=0)
|
||||
{
|
||||
GLES20.glDeleteTextures(1, new int[] {cameraTextureId}, 0);
|
||||
}
|
||||
|
||||
stopBackgroundThread();
|
||||
}
|
||||
|
||||
/*************************************************** ONDRAWFRAME ARCORE ************************************************************* */
|
||||
|
||||
// Draw frame when in AR mode. Called on the GL thread.
|
||||
public void updateGL() throws CameraNotAvailableException {
|
||||
|
||||
if(!mReady.get())
|
||||
{
|
||||
return;
|
||||
}
|
||||
|
||||
if (mTOFAvailable && mTOFImageReader.frameCount == 0) return;
|
||||
|
||||
// Perform ARCore per-frame update.
|
||||
Frame frame = null;
|
||||
try {
|
||||
frame = sharedSession.update();
|
||||
} catch (Exception e) {
|
||||
e.printStackTrace();
|
||||
return;
|
||||
}
|
||||
|
||||
Camera camera = null;
|
||||
if (frame != null) {
|
||||
camera = frame.getCamera();
|
||||
}else
|
||||
{
|
||||
return;
|
||||
}
|
||||
|
||||
if (camera == null) return;
|
||||
// If not tracking, don't draw 3D objects.
|
||||
if (camera.getTrackingState() == TrackingState.PAUSED) return;
|
||||
|
||||
if (frame.getTimestamp() != 0) {
|
||||
|
||||
Pose pose = camera.getPose();
|
||||
if(!RTABMapActivity.DISABLE_LOG) Log.d(TAG, String.format("pose=%f %f %f q=%f %f %f %f", pose.tx(), pose.ty(), pose.tz(), pose.qx(), pose.qy(), pose.qz(), pose.qw()));
|
||||
RTABMapLib.postCameraPoseEvent(RTABMapActivity.nativeApplication, pose.tx(), pose.ty(), pose.tz(), pose.qx(), pose.qy(), pose.qz(), pose.qw());
|
||||
|
||||
int rateMs = 100; // send images at most 10 Hz
|
||||
if(System. currentTimeMillis() - mPreviousTime < rateMs)
|
||||
{
|
||||
return;
|
||||
}
|
||||
mPreviousTime = System. currentTimeMillis();
|
||||
|
||||
CameraIntrinsics intrinsics = camera.getImageIntrinsics();
|
||||
try{
|
||||
Image image = frame.acquireCameraImage();
|
||||
PointCloud cloud = frame.acquirePointCloud();
|
||||
FloatBuffer points = cloud.getPoints();
|
||||
|
||||
if (image.getFormat() != ImageFormat.YUV_420_888) {
|
||||
throw new IllegalArgumentException(
|
||||
"Expected image in YUV_420_888 format, got format " + image.getFormat());
|
||||
}
|
||||
|
||||
if(!RTABMapActivity.DISABLE_LOG)
|
||||
{
|
||||
for(int i =0;i<image.getPlanes().length;++i)
|
||||
{
|
||||
Log.d(TAG, String.format("Plane[%d] pixel stride = %d, row stride = %d", i, image.getPlanes()[i].getPixelStride(), image.getPlanes()[i].getRowStride()));
|
||||
}
|
||||
}
|
||||
|
||||
float[] fl = intrinsics.getFocalLength();
|
||||
float[] pp = intrinsics.getPrincipalPoint();
|
||||
if(!RTABMapActivity.DISABLE_LOG) Log.d(TAG, String.format("fx=%f fy=%f cx=%f cy=%f", fl[0], fl[1], pp[0], pp[1]));
|
||||
|
||||
ByteBuffer y = image.getPlanes()[0].getBuffer().asReadOnlyBuffer();
|
||||
ByteBuffer u = image.getPlanes()[1].getBuffer().asReadOnlyBuffer();
|
||||
ByteBuffer v = image.getPlanes()[2].getBuffer().asReadOnlyBuffer();
|
||||
|
||||
double stamp = (double)image.getTimestamp()/10e8;
|
||||
if(!RTABMapActivity.DISABLE_LOG) Log.d(TAG, String.format("RGB %dx%d len=%dbytes format=%d =%f",
|
||||
image.getWidth(), image.getHeight(), y.limit(), image.getFormat(), stamp));
|
||||
|
||||
if(mTOFAvailable)
|
||||
{
|
||||
if(!RTABMapActivity.DISABLE_LOG) Log.d(TAG, String.format("Depth %dx%d len=%dbytes format=%d stamp=%f",
|
||||
mTOFImageReader.WIDTH, mTOFImageReader.HEIGHT, mTOFImageReader.depth16_raw.limit(), ImageFormat.DEPTH16, (double)mTOFImageReader.timestamp/10e9));
|
||||
|
||||
RTABMapLib.postOdometryEvent(
|
||||
RTABMapActivity.nativeApplication,
|
||||
pose.tx(), pose.ty(), pose.tz(), pose.qx(), pose.qy(), pose.qz(), pose.qw(),
|
||||
fl[0], fl[1], pp[0], pp[1], stamp,
|
||||
y, u, v, y.limit(), image.getWidth(), image.getHeight(), image.getFormat(),
|
||||
mTOFImageReader.depth16_raw, mTOFImageReader.depth16_raw.limit(), mTOFImageReader.WIDTH, mTOFImageReader.HEIGHT, ImageFormat.DEPTH16,
|
||||
points, points.limit()/4);
|
||||
}
|
||||
else
|
||||
{
|
||||
ByteBuffer bb = ByteBuffer.allocate(0);
|
||||
RTABMapLib.postOdometryEvent(
|
||||
RTABMapActivity.nativeApplication,
|
||||
pose.tx(), pose.ty(), pose.tz(), pose.qx(), pose.qy(), pose.qz(), pose.qw(),
|
||||
fl[0], fl[1], pp[0], pp[1], stamp,
|
||||
y, u, v, y.limit(), image.getWidth(), image.getHeight(), image.getFormat(),
|
||||
bb, 0, 0, 0, ImageFormat.DEPTH16,
|
||||
points, points.limit()/4);
|
||||
}
|
||||
|
||||
image.close();
|
||||
cloud.close();
|
||||
|
||||
} catch (NotYetAvailableException e) {
|
||||
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/********************************************************************************************************************* */
|
||||
/*************************************************** End ************************************************************* */
|
||||
/********************************************************************************************************************* */
|
||||
|
||||
|
||||
public ArrayList<String> getResolutions (Context context, String cameraId,int imageFormat){
|
||||
Log.v(TAG + "getResolutions:", " cameraId:" + cameraId + " imageFormat: " + imageFormat);
|
||||
|
||||
ArrayList<String> output = new ArrayList<String>();
|
||||
try {
|
||||
CameraManager manager = (CameraManager) context.getSystemService(Context.CAMERA_SERVICE);
|
||||
CameraCharacteristics characteristics = manager.getCameraCharacteristics(cameraId);
|
||||
|
||||
for (android.util.Size s : characteristics.get(CameraCharacteristics.SCALER_STREAM_CONFIGURATION_MAP).getOutputSizes(imageFormat)) {
|
||||
output.add(s.getWidth() + "x" + s.getHeight());
|
||||
}
|
||||
} catch (Exception e) {
|
||||
e.printStackTrace();
|
||||
}
|
||||
return output;
|
||||
}
|
||||
|
||||
|
||||
public void initCamera (Context context, String cameraId,int index){
|
||||
boolean ok = false;
|
||||
try {
|
||||
int current = 0;
|
||||
CameraManager manager = (CameraManager) context.getSystemService(Context.CAMERA_SERVICE);
|
||||
CameraCharacteristics characteristics = manager.getCameraCharacteristics(cameraId);
|
||||
for (android.util.Size s : characteristics.get(CameraCharacteristics.SCALER_STREAM_CONFIGURATION_MAP).getOutputSizes(ImageFormat.DEPTH16)) {
|
||||
ok = true;
|
||||
if (current == index)
|
||||
break;
|
||||
else ;
|
||||
current++;
|
||||
}
|
||||
} catch (Exception e) {
|
||||
e.printStackTrace();
|
||||
}
|
||||
if (!ok) {
|
||||
Log.e(TAG + " initCamera", "Depth sensor not found!");
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
}
|
||||
@@ -90,9 +90,9 @@ import android.widget.SeekBar.OnSeekBarChangeListener;
|
||||
import android.widget.Toast;
|
||||
import android.widget.ToggleButton;
|
||||
|
||||
import com.google.ar.core.ArCoreApk;
|
||||
//import com.google.ar.core.ArCoreApk;
|
||||
import com.google.atap.tangoservice.Tango;
|
||||
import com.huawei.hiar.AREnginesApk;
|
||||
//import com.huawei.hiar.AREnginesApk;
|
||||
|
||||
|
||||
// The main activity of the application. This activity shows debug information
|
||||
@@ -254,7 +254,7 @@ public class RTABMapActivity extends FragmentActivity implements OnClickListener
|
||||
|
||||
GestureDetector mGesDetect = null;
|
||||
|
||||
ARCoreSharedCamera mArCoreCamera = null;
|
||||
//ARCoreSharedCamera mArCoreCamera = null;
|
||||
int mCameraDriver = 0;
|
||||
|
||||
//Tango Service connection.
|
||||
@@ -586,8 +586,8 @@ public class RTABMapActivity extends FragmentActivity implements OnClickListener
|
||||
String cameraDriverStr = sharedPref.getString(getString(R.string.pref_key_camera_driver), getString(R.string.pref_default_camera_driver));
|
||||
mCameraDriver = Integer.parseInt(cameraDriverStr);
|
||||
|
||||
isArCoreAvailable();
|
||||
isArEngineAvailable();
|
||||
//isArCoreAvailable();
|
||||
//isArEngineAvailable();
|
||||
}
|
||||
|
||||
// Should be called only if read/write permissions are granted!
|
||||
@@ -613,7 +613,7 @@ public class RTABMapActivity extends FragmentActivity implements OnClickListener
|
||||
Log.i(TAG, String.format("updateCameraDriverSettings() mCameraDriver=%d RTABMapLib.isBuiltWith(%d)=%d", mCameraDriver, mCameraDriver, RTABMapLib.isBuiltWith(nativeApplication, mCameraDriver)?1:0));
|
||||
|
||||
SharedPreferences sharedPref = PreferenceManager.getDefaultSharedPreferences(this);
|
||||
|
||||
/*
|
||||
if(mCameraDriver == 0 && (!CheckTangoCoreVersion(MIN_TANGO_CORE_VERSION) || !RTABMapLib.isBuiltWith(nativeApplication, 0)))
|
||||
{
|
||||
if(mIsAREngineAvailable && RTABMapLib.isBuiltWith(nativeApplication, 2))
|
||||
@@ -659,9 +659,9 @@ public class RTABMapActivity extends FragmentActivity implements OnClickListener
|
||||
editor.putString(getString(R.string.pref_key_camera_driver), "3");
|
||||
editor.commit();
|
||||
}
|
||||
}
|
||||
}*/
|
||||
}
|
||||
|
||||
/*
|
||||
private void isArCoreAvailable() {
|
||||
ArCoreApk.Availability availability = ArCoreApk.getInstance().checkAvailability(this);
|
||||
if (availability.isTransient()) {
|
||||
@@ -713,7 +713,7 @@ public class RTABMapActivity extends FragmentActivity implements OnClickListener
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
*/
|
||||
@Override
|
||||
public void onDestroy() {
|
||||
super.onDestroy();
|
||||
@@ -1199,7 +1199,7 @@ public class RTABMapActivity extends FragmentActivity implements OnClickListener
|
||||
mToast.makeText(this, "Current camera driver selected is Tango, but Tango service binding failed. Abort scanning...", mToast.LENGTH_LONG).show();
|
||||
}
|
||||
}
|
||||
}
|
||||
}/*
|
||||
else if(mCameraDriver == 1 || mCameraDriver == 2 || mCameraDriver == 3)
|
||||
{
|
||||
if((mCameraDriver == 1 || mCameraDriver == 3) && !mIsARCoreAvailable)
|
||||
@@ -1281,7 +1281,7 @@ public class RTABMapActivity extends FragmentActivity implements OnClickListener
|
||||
}
|
||||
});
|
||||
bindThread.start();
|
||||
}
|
||||
}*/
|
||||
else
|
||||
{
|
||||
mToast.makeText(this, "Supported camera driver not found! Cannot start a new scan.", mToast.LENGTH_LONG).show();
|
||||
@@ -2274,14 +2274,14 @@ public class RTABMapActivity extends FragmentActivity implements OnClickListener
|
||||
}
|
||||
}
|
||||
|
||||
if(mArCoreCamera != null)
|
||||
/* if(mArCoreCamera != null)
|
||||
{
|
||||
synchronized (this) {
|
||||
mRenderer.setCamera(null);
|
||||
mArCoreCamera.close();
|
||||
mArCoreCamera = null;
|
||||
}
|
||||
}
|
||||
}*/
|
||||
|
||||
Thread stopThread = new Thread(new Runnable() {
|
||||
public void run() {
|
||||
@@ -2314,14 +2314,14 @@ public class RTABMapActivity extends FragmentActivity implements OnClickListener
|
||||
|
||||
updateState(State.STATE_IDLE);
|
||||
|
||||
if(mArCoreCamera != null)
|
||||
/*if(mArCoreCamera != null)
|
||||
{
|
||||
synchronized (this) {
|
||||
mRenderer.setCamera(null);
|
||||
mArCoreCamera.close();
|
||||
mArCoreCamera = null;
|
||||
}
|
||||
}
|
||||
}*/
|
||||
|
||||
Thread stopThread = new Thread(new Runnable() {
|
||||
public void run() {
|
||||
|
||||
@@ -43,7 +43,7 @@ public class Renderer implements GLSurfaceView.Renderer {
|
||||
private float mSurfaceHeight = 0.0f;
|
||||
private float mTextColor = 1.0f;
|
||||
private int mOffset = 0;
|
||||
private ARCoreSharedCamera mCamera = null;
|
||||
//private ARCoreSharedCamera mCamera = null;
|
||||
|
||||
private Vector<TextObject> mTexts;
|
||||
|
||||
@@ -73,10 +73,10 @@ public class Renderer implements GLSurfaceView.Renderer {
|
||||
mOffset = offset;
|
||||
}
|
||||
|
||||
public void setCamera(ARCoreSharedCamera camera)
|
||||
{
|
||||
mCamera = camera;
|
||||
}
|
||||
//public void setCamera(ARCoreSharedCamera camera)
|
||||
//{
|
||||
// mCamera = camera;
|
||||
//}
|
||||
|
||||
// Render loop of the Gl context.
|
||||
public void onDrawFrame(GL10 useGLES20instead) {
|
||||
@@ -86,10 +86,10 @@ public class Renderer implements GLSurfaceView.Renderer {
|
||||
{
|
||||
try
|
||||
{
|
||||
if(mCamera!=null)
|
||||
{
|
||||
mCamera.updateGL();
|
||||
}
|
||||
// if(mCamera!=null)
|
||||
// {
|
||||
// mCamera.updateGL();
|
||||
// }
|
||||
|
||||
final int value = RTABMapLib.render(mActivity.nativeApplication);
|
||||
|
||||
|
||||
@@ -361,7 +361,7 @@ public class SettingsActivity extends PreferenceActivity implements OnSharedPref
|
||||
ed.commit(); //save it.
|
||||
}
|
||||
|
||||
@Override
|
||||
|
||||
public void onRequestPermissionsResult(int requestCode, String[] permissions, int[] results) {
|
||||
|
||||
switch (requestCode) {
|
||||
|
||||
@@ -1,87 +0,0 @@
|
||||
package com.introlab.rtabmap;
|
||||
|
||||
|
||||
import android.graphics.ImageFormat;
|
||||
import android.media.Image;
|
||||
import android.media.ImageReader;
|
||||
import android.os.Handler;
|
||||
import android.os.HandlerThread;
|
||||
import android.util.Log;
|
||||
|
||||
import java.nio.ByteBuffer;
|
||||
|
||||
public class TOF_ImageReader implements ImageReader.OnImageAvailableListener {
|
||||
|
||||
public int WIDTH;
|
||||
public int HEIGHT;
|
||||
public ImageReader imageReader;
|
||||
public int frameCount = 0;
|
||||
public long timestamp;
|
||||
|
||||
// Looper handler thread.
|
||||
private HandlerThread backgroundThread;
|
||||
// Looper handler.
|
||||
private Handler backgroundHandler;
|
||||
|
||||
public ByteBuffer depth16_raw;
|
||||
|
||||
TOF_ImageReader(){
|
||||
}
|
||||
|
||||
public void createImageReader(int width, int height){
|
||||
this.WIDTH = width;
|
||||
this.HEIGHT = height;
|
||||
this.imageReader =
|
||||
ImageReader.newInstance(
|
||||
width,
|
||||
height,
|
||||
ImageFormat.DEPTH16,
|
||||
2);
|
||||
this.imageReader.setOnImageAvailableListener(this, this.backgroundHandler);
|
||||
}
|
||||
|
||||
// CPU image reader callback.
|
||||
@Override
|
||||
public void onImageAvailable(ImageReader imageReader) {
|
||||
Image image = imageReader.acquireLatestImage();
|
||||
if (image == null) {
|
||||
Log.w("RTABMapActivity", "onImageAvailable: Skipping null image.");
|
||||
return;
|
||||
}
|
||||
else{
|
||||
if(image.getFormat() == ImageFormat.DEPTH16){
|
||||
this.timestamp = image.getTimestamp();
|
||||
depth16_raw = image.getPlanes()[0].getBuffer().asReadOnlyBuffer();
|
||||
// copy raw undecoded DEPTH16 format depth data to NativeBuffer
|
||||
frameCount++;
|
||||
}
|
||||
else{
|
||||
Log.w("RTABMapActivity", "onImageAvailable: depth image not in DEPTH16 format, skipping image");
|
||||
}
|
||||
}
|
||||
image.close();
|
||||
}
|
||||
|
||||
// Start background handler thread, used to run callbacks without blocking UI thread.
|
||||
public void startBackgroundThread() {
|
||||
this.backgroundThread = new HandlerThread("DepthDecoderThread");
|
||||
this.backgroundThread.start();
|
||||
this.backgroundHandler = new Handler(backgroundThread.getLooper());
|
||||
}
|
||||
|
||||
// Stop background handler thread.
|
||||
public void stopBackgroundThread() {
|
||||
if (this.backgroundThread != null) {
|
||||
this.backgroundThread.quitSafely();
|
||||
try {
|
||||
this.backgroundThread.join();
|
||||
this.backgroundThread = null;
|
||||
this.backgroundHandler = null;
|
||||
} catch (InterruptedException e) {
|
||||
Log.e("RTABMapActivity", "Interrupted while trying to join depth background handler thread", e);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user