mirror of
https://github.com/introlab/rtabmap.git
synced 2026-10-07 02:27:47 +08:00
Added depth/rgb registration option
This commit is contained in:
matlabbe
@@ -11,8 +11,8 @@ 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.Coordinates2d;
|
||||
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;
|
||||
@@ -29,6 +29,7 @@ import android.hardware.camera2.CameraCaptureSession;
|
||||
import android.hardware.camera2.CameraCharacteristics;
|
||||
import android.hardware.camera2.CameraDevice;
|
||||
import android.hardware.camera2.CameraManager;
|
||||
import android.hardware.camera2.CameraMetadata;
|
||||
import android.hardware.camera2.CaptureFailure;
|
||||
import android.hardware.camera2.CaptureRequest;
|
||||
import android.hardware.camera2.TotalCaptureResult;
|
||||
@@ -45,7 +46,11 @@ public class ARCoreSharedCamera {
|
||||
|
||||
public static final String TAG = ARCoreSharedCamera.class.getSimpleName();
|
||||
|
||||
|
||||
private static final float[] QUAD_COORDS =
|
||||
new float[] {
|
||||
-1.0f, -1.0f, +1.0f, -1.0f, -1.0f, +1.0f, +1.0f, +1.0f,
|
||||
};
|
||||
|
||||
private static RTABMapActivity mActivity;
|
||||
public ARCoreSharedCamera(RTABMapActivity c) {
|
||||
mActivity = c;
|
||||
@@ -53,8 +58,8 @@ public class ARCoreSharedCamera {
|
||||
|
||||
// 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;
|
||||
private int depthWidth = 640;
|
||||
private int depthHeight = 480;
|
||||
|
||||
// GL Surface used to draw camera preview image.
|
||||
public GLSurfaceView surfaceView;
|
||||
@@ -81,6 +86,11 @@ public class ARCoreSharedCamera {
|
||||
|
||||
// Camera ID for the camera used by ARCore.
|
||||
private String cameraId;
|
||||
private String depthCameraId;
|
||||
|
||||
private Pose rgbExtrinsics;
|
||||
private Pose depthExtrinsics;
|
||||
private float[] depthIntrinsics = null;
|
||||
|
||||
private AtomicBoolean mReady = new AtomicBoolean(false);
|
||||
|
||||
@@ -95,6 +105,7 @@ public class ARCoreSharedCamera {
|
||||
|
||||
public boolean isDepthSupported() {return mTOFAvailable;}
|
||||
|
||||
|
||||
// Camera device state callback.
|
||||
private final CameraDevice.StateCallback cameraDeviceCallback =
|
||||
new CameraDevice.StateCallback() {
|
||||
@@ -176,7 +187,7 @@ public class ARCoreSharedCamera {
|
||||
@NonNull CameraCaptureSession session,
|
||||
@NonNull CaptureRequest request,
|
||||
@NonNull TotalCaptureResult result) {
|
||||
Log.i(TAG, "onCaptureCompleted");
|
||||
//Log.i(TAG, "onCaptureCompleted");
|
||||
}
|
||||
|
||||
//@Override // android 23
|
||||
@@ -251,7 +262,7 @@ public class ARCoreSharedCamera {
|
||||
|
||||
// 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());
|
||||
if (mTOFAvailable && cameraId.compareTo(depthCameraId) == 0) surfaceList.add(mTOFImageReader.imageReader.getSurface());
|
||||
// Surface list should now contain three surfacemReadymReadys:
|
||||
// 0. sharedCamera.getSurfaceTexture()
|
||||
// 1. …
|
||||
@@ -272,8 +283,6 @@ public class ARCoreSharedCamera {
|
||||
} catch (CameraAccessException e) {
|
||||
Log.e(TAG, "CameraAccessException", e);
|
||||
}
|
||||
|
||||
|
||||
}
|
||||
|
||||
// Start background handler thread, used to run callbacks without blocking UI thread.
|
||||
@@ -301,6 +310,25 @@ public class ARCoreSharedCamera {
|
||||
|
||||
private long mPreviousTime = 0;
|
||||
|
||||
/**
|
||||
* Return true if the given array contains the given integer.
|
||||
*
|
||||
* @param modes array to check.
|
||||
* @param mode integer to get for.
|
||||
* @return true if the array contains the given integer, otherwise false.
|
||||
*/
|
||||
private static boolean contains(int[] modes, int mode) {
|
||||
if (modes == null) {
|
||||
return false;
|
||||
}
|
||||
for (int i : modes) {
|
||||
if (i == mode) {
|
||||
return true;
|
||||
}
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
// Perform various checks, then open camera device and create CPU image reader.
|
||||
public boolean openCamera() {
|
||||
|
||||
@@ -317,7 +345,7 @@ public class ARCoreSharedCamera {
|
||||
cameraTextureId = textures[0];
|
||||
}
|
||||
|
||||
Log.v(TAG + " opencamera: ", "Perform various checks, then open camera device and create CPU image reader.");
|
||||
Log.v(TAG, "Perform various checks, then open camera device and create CPU image reader.");
|
||||
// Don't open camera if already opened.
|
||||
if (cameraDevice != null) {
|
||||
return false;
|
||||
@@ -335,10 +363,10 @@ public class ARCoreSharedCamera {
|
||||
// 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.setUpdateMode(Config.UpdateMode.BLOCKING);
|
||||
config.setPlaneFindingMode(Config.PlaneFindingMode.DISABLED);
|
||||
config.setLightEstimationMode(Config.LightEstimationMode.DISABLED);
|
||||
//config.setCloudAnchorMode(Config.CloudAnchorMode.ENABLED);
|
||||
config.setCloudAnchorMode(Config.CloudAnchorMode.DISABLED);
|
||||
sharedSession.configure(config);
|
||||
|
||||
}
|
||||
@@ -347,27 +375,128 @@ public class ARCoreSharedCamera {
|
||||
sharedCamera = sharedSession.getSharedCamera();
|
||||
// Store the ID of the camera used by ARCore.
|
||||
cameraId = sharedSession.getCameraConfig().getCameraId();
|
||||
initCamera(mActivity, cameraId, 1);
|
||||
ArrayList<String> resolutions;
|
||||
|
||||
Log.d(TAG, "Shared camera ID: " + cameraId);
|
||||
|
||||
mTOFAvailable = false;
|
||||
|
||||
// Store a reference to the camera system service.
|
||||
cameraManager = (CameraManager) mActivity.getSystemService(Context.CAMERA_SERVICE);
|
||||
|
||||
depthCameraId = null;
|
||||
// show all cameras
|
||||
try {
|
||||
// Find a CameraDevice that supports DEPTH16 captures, and configure state.
|
||||
for (String tmpCameraId : cameraManager.getCameraIdList()) {
|
||||
CameraCharacteristics characteristics = cameraManager.getCameraCharacteristics(tmpCameraId);
|
||||
|
||||
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;
|
||||
Log.i(TAG, "Camera " + tmpCameraId + " extrinsics:");
|
||||
|
||||
float[] translation = characteristics.get(CameraCharacteristics.LENS_POSE_TRANSLATION);
|
||||
if(translation != null)
|
||||
{
|
||||
Log.i(TAG, String.format("Translation (x,y,z): %f,%f,%f", translation[0], translation[1], translation[2]));
|
||||
}
|
||||
float[] rotation = characteristics.get(CameraCharacteristics.LENS_POSE_ROTATION);
|
||||
if(rotation != null)
|
||||
{
|
||||
Log.i(TAG, String.format("Rotation (qx,qy,qz,qw): %f,%f,%f,%f", rotation[0], rotation[1], rotation[2], rotation[3]));
|
||||
}
|
||||
|
||||
if(tmpCameraId.compareTo(cameraId)==0 && translation!=null && rotation!=null)
|
||||
{
|
||||
rgbExtrinsics = new Pose(translation, rotation);
|
||||
Log.i(TAG,"Set rgb extrinsics!");
|
||||
}
|
||||
|
||||
if (!contains(characteristics.get(CameraCharacteristics.REQUEST_AVAILABLE_CAPABILITIES), CameraCharacteristics.REQUEST_AVAILABLE_CAPABILITIES_DEPTH_OUTPUT) ||
|
||||
characteristics.get(CameraCharacteristics.LENS_FACING) == CameraMetadata.LENS_FACING_FRONT) {
|
||||
continue;
|
||||
}
|
||||
Log.i(TAG, "Camera " + tmpCameraId + " has depth output available");
|
||||
|
||||
depthCameraId = tmpCameraId;
|
||||
if(translation!=null && rotation != null)
|
||||
{
|
||||
depthExtrinsics = new Pose(translation, rotation);
|
||||
Log.i(TAG,"Set depth extrinsics!");
|
||||
}
|
||||
depthIntrinsics = characteristics.get(CameraCharacteristics.LENS_INTRINSIC_CALIBRATION);
|
||||
Log.i(TAG, String.format("Intrinsics (fx,fy,cx,cy,s): %f,%f,%f,%f,%f",
|
||||
depthIntrinsics[0],
|
||||
depthIntrinsics[1],
|
||||
depthIntrinsics[2],
|
||||
depthIntrinsics[3],
|
||||
depthIntrinsics[4]));
|
||||
}
|
||||
} catch (CameraAccessException e) {
|
||||
e.printStackTrace();
|
||||
}
|
||||
if(rgbExtrinsics == null)
|
||||
{
|
||||
float[] translation = {0,0,0};
|
||||
float[] rotation = {0,0,0,1};
|
||||
rgbExtrinsics = new Pose(translation, rotation);
|
||||
}
|
||||
if(depthExtrinsics == null)
|
||||
{
|
||||
depthExtrinsics = rgbExtrinsics;
|
||||
}
|
||||
|
||||
if(depthCameraId != null)
|
||||
{
|
||||
ArrayList<String> resolutions = getResolutions(mActivity, depthCameraId, ImageFormat.DEPTH16);
|
||||
if (resolutions != null) {
|
||||
float[] newDepthIntrinsics = null;
|
||||
int largestWidth = 0;
|
||||
for( String temp : resolutions) {
|
||||
Log.i(TAG, "DEPTH16 resolution: " + temp);
|
||||
depthWidth = Integer.parseInt(temp.split("x")[0]);
|
||||
depthHeight = Integer.parseInt(temp.split("x")[1]);
|
||||
if(depthIntrinsics != null)
|
||||
{
|
||||
if(depthIntrinsics[0] != 0)
|
||||
{
|
||||
if(largestWidth == 0 && depthWidth>largestWidth)
|
||||
{
|
||||
largestWidth = depthWidth; // intrinsics should match this resolution
|
||||
}
|
||||
// Samsung Galaxy Note10+: take smallest resolution and match the intrinsics
|
||||
if(depthWidth < largestWidth)
|
||||
{
|
||||
float scale = (float)depthWidth/(float)largestWidth;
|
||||
newDepthIntrinsics = depthIntrinsics.clone();
|
||||
newDepthIntrinsics[0] *= scale;
|
||||
newDepthIntrinsics[1] *= scale;
|
||||
newDepthIntrinsics[2] *= scale;
|
||||
newDepthIntrinsics[3] *= scale;
|
||||
}
|
||||
}
|
||||
else if(depthWidth ==240 && depthHeight==180)
|
||||
{
|
||||
// Huawei P30 Pro: only 240x180 is working
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
if (resolutions.size()>0) {
|
||||
mTOFAvailable = true;
|
||||
if(newDepthIntrinsics!=null) {
|
||||
depthIntrinsics = newDepthIntrinsics;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
Log.i(TAG, "TOF_available: " + mTOFAvailable);
|
||||
|
||||
// Color CPU Image.
|
||||
// Use the currently configured CPU image size.
|
||||
//Size desiredCPUImageSize = sharedSession.getCameraConfig().getImageSize();
|
||||
|
||||
if (mTOFAvailable) mTOFImageReader.createImageReader(DEPTH_WIDTH, DEPTH_HEIGHT);
|
||||
if (mTOFAvailable) mTOFImageReader.createImageReader(depthWidth, depthHeight);
|
||||
|
||||
// When ARCore is running, make sure it also updates our CPU image surface.
|
||||
if (mTOFAvailable) {
|
||||
if (mTOFAvailable && cameraId.compareTo(depthCameraId) == 0) {
|
||||
sharedCamera.setAppSurfaces(this.cameraId, Arrays.asList(mTOFImageReader.imageReader.getSurface()));
|
||||
}
|
||||
|
||||
@@ -376,15 +505,14 @@ public class ARCoreSharedCamera {
|
||||
// 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);
|
||||
|
||||
if(mTOFAvailable && cameraId.compareTo(depthCameraId) != 0)
|
||||
{
|
||||
cameraManager.openCamera(depthCameraId, mTOFImageReader.cameraDeviceCallback, mTOFImageReader.backgroundHandler);
|
||||
}
|
||||
|
||||
} catch (CameraAccessException e) {
|
||||
Log.e(TAG, "Failed to open camera", e);
|
||||
return false;
|
||||
@@ -395,14 +523,58 @@ public class ARCoreSharedCamera {
|
||||
Log.e(TAG, "Failed to open camera", e);
|
||||
return false;
|
||||
}
|
||||
|
||||
Log.i(TAG, " opencamera: TOF_available: " + mTOFAvailable);
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
public static void rotationMatrixToQuaternion(float[] R, float[] q) {
|
||||
final float m00 = R[0];
|
||||
final float m10 = R[1];
|
||||
final float m20 = R[2];
|
||||
final float m01 = R[4];
|
||||
final float m11 = R[5];
|
||||
final float m21 = R[6];
|
||||
final float m02 = R[8];
|
||||
final float m12 = R[9];
|
||||
final float m22 = R[10];
|
||||
|
||||
float tr = m00 + m11 + m22;
|
||||
|
||||
if (tr > 0) {
|
||||
float S = (float) Math.sqrt(tr + 1.0) * 2; // S=4*qw
|
||||
q[0] = 0.25f * S;/* w w w.j ava 2s.co m*/
|
||||
q[1] = (m21 - m12) / S;
|
||||
q[2] = (m02 - m20) / S;
|
||||
q[3] = (m10 - m01) / S;
|
||||
} else if ((m00 > m11) & (m00 > m22)) {
|
||||
float S = (float) Math.sqrt(1.0 + m00 - m11 - m22) * 2; //
|
||||
// S=4*q[1]
|
||||
q[0] = (m21 - m12) / S;
|
||||
q[1] = 0.25f * S;
|
||||
q[2] = (m01 + m10) / S;
|
||||
q[3] = (m02 + m20) / S;
|
||||
} else if (m11 > m22) {
|
||||
float S = (float) Math.sqrt(1.0 + m11 - m00 - m22) * 2; //
|
||||
// S=4*q[2]
|
||||
q[0] = (m02 - m20) / S;
|
||||
q[1] = (m01 + m10) / S;
|
||||
q[2] = 0.25f * S;
|
||||
q[3] = (m12 + m21) / S;
|
||||
} else {
|
||||
float S = (float) Math.sqrt(1.0 + m22 - m00 - m11) * 2; //
|
||||
// S=4*q[3]
|
||||
q[0] = (m10 - m01) / S;
|
||||
q[1] = (m02 + m20) / S;
|
||||
q[2] = (m12 + m21) / S;
|
||||
q[3] = 0.25f * S;
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
|
||||
// Close the camera device.
|
||||
public void close() {
|
||||
Log.w(TAG, "close()");
|
||||
|
||||
if (sharedSession != null) {
|
||||
sharedSession.pause();
|
||||
@@ -415,11 +587,8 @@ public class ARCoreSharedCamera {
|
||||
if (cameraDevice != null) {
|
||||
cameraDevice.close();
|
||||
}
|
||||
|
||||
if (mTOFImageReader.imageReader != null) {
|
||||
mTOFImageReader.imageReader.close();
|
||||
mTOFImageReader.imageReader = null;
|
||||
}
|
||||
|
||||
mTOFImageReader.close();
|
||||
|
||||
if(cameraTextureId>=0)
|
||||
{
|
||||
@@ -429,8 +598,12 @@ public class ARCoreSharedCamera {
|
||||
stopBackgroundThread();
|
||||
}
|
||||
|
||||
/*************************************************** ONDRAWFRAME ARCORE ************************************************************* */
|
||||
|
||||
public void setDisplayGeometry(int rotation, int width, int height)
|
||||
{
|
||||
sharedSession.setDisplayGeometry(rotation, width, height);
|
||||
}
|
||||
|
||||
/*************************************************** ONDRAWFRAME ARCORE ************************************************************* */
|
||||
// Draw frame when in AR mode. Called on the GL thread.
|
||||
public void updateGL() throws CameraNotAvailableException {
|
||||
|
||||
@@ -463,12 +636,12 @@ public class ARCoreSharedCamera {
|
||||
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());
|
||||
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);
|
||||
|
||||
int rateMs = 100; // send images at most 10 Hz
|
||||
int rateMs = 100; // send images at most 10 Hz (to save battery)
|
||||
if(System. currentTimeMillis() - mPreviousTime < rateMs)
|
||||
{
|
||||
return;
|
||||
@@ -478,6 +651,7 @@ public class ARCoreSharedCamera {
|
||||
CameraIntrinsics intrinsics = camera.getImageIntrinsics();
|
||||
try{
|
||||
Image image = frame.acquireCameraImage();
|
||||
if(!RTABMapActivity.DISABLE_LOG) Log.d(TAG, String.format("frame=%d vs image=%d", frame.getTimestamp(), image.getTimestamp()));
|
||||
PointCloud cloud = frame.acquirePointCloud();
|
||||
FloatBuffer points = cloud.getPoints();
|
||||
|
||||
@@ -502,33 +676,66 @@ public class ARCoreSharedCamera {
|
||||
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",
|
||||
if(!RTABMapActivity.DISABLE_LOG) Log.d(TAG, String.format("RGB %dx%d len=%dbytes format=%d stamp=%f",
|
||||
image.getWidth(), image.getHeight(), y.limit(), image.getFormat(), stamp));
|
||||
|
||||
float[] texCoord = new float[8];
|
||||
frame.transformCoordinates2d(
|
||||
Coordinates2d.OPENGL_NORMALIZED_DEVICE_COORDINATES,
|
||||
QUAD_COORDS,
|
||||
Coordinates2d.TEXTURE_NORMALIZED,
|
||||
texCoord);
|
||||
|
||||
float[] p = new float[16];
|
||||
camera.getProjectionMatrix(p, 0, 0.1f, 100.0f);
|
||||
|
||||
float[] viewMatrix = new float[16];
|
||||
camera.getViewMatrix(viewMatrix, 0);
|
||||
float[] quat = new float[4];
|
||||
rotationMatrixToQuaternion(viewMatrix, quat);
|
||||
|
||||
|
||||
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));
|
||||
ByteBuffer depth;
|
||||
double depthStamp;
|
||||
synchronized (mTOFImageReader) {
|
||||
depth = mTOFImageReader.depth16_raw;
|
||||
depthStamp = (double)mTOFImageReader.timestamp/10e8;
|
||||
}
|
||||
|
||||
RTABMapLib.postOdometryEvent(
|
||||
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));
|
||||
|
||||
RTABMapLib.postOdometryEventDepth(
|
||||
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,
|
||||
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,
|
||||
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);
|
||||
depth, depth.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]);
|
||||
}
|
||||
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,
|
||||
fl[0], fl[1], pp[0], pp[1],
|
||||
rgbExtrinsics.tx(), rgbExtrinsics.ty(), rgbExtrinsics.tz(), rgbExtrinsics.qx(), rgbExtrinsics.qy(), rgbExtrinsics.qz(), rgbExtrinsics.qw(),
|
||||
stamp,
|
||||
y, u, v, y.limit(), image.getWidth(), image.getHeight(), image.getFormat(),
|
||||
bb, 0, 0, 0, ImageFormat.DEPTH16,
|
||||
points, points.limit()/4);
|
||||
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]);
|
||||
}
|
||||
|
||||
image.close();
|
||||
@@ -545,8 +752,8 @@ public class ARCoreSharedCamera {
|
||||
/********************************************************************************************************************* */
|
||||
|
||||
|
||||
public ArrayList<String> getResolutions (Context context, String cameraId,int imageFormat){
|
||||
Log.v(TAG + "getResolutions:", " cameraId:" + cameraId + " imageFormat: " + imageFormat);
|
||||
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 {
|
||||
@@ -562,27 +769,4 @@ public class ARCoreSharedCamera {
|
||||
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!");
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
}
|
||||
|
||||
@@ -0,0 +1,166 @@
|
||||
package com.introlab.rtabmap;
|
||||
|
||||
/*
|
||||
* Copyright 2017 Google LLC
|
||||
*
|
||||
* Licensed under the Apache License, Version 2.0 (the "License");
|
||||
* you may not use this file except in compliance with the License.
|
||||
* You may obtain a copy of the License at
|
||||
*
|
||||
* http://www.apache.org/licenses/LICENSE-2.0
|
||||
*
|
||||
* Unless required by applicable law or agreed to in writing, software
|
||||
* distributed under the License is distributed on an "AS IS" BASIS,
|
||||
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
* See the License for the specific language governing permissions and
|
||||
* limitations under the License.
|
||||
*/
|
||||
|
||||
import android.app.Activity;
|
||||
import android.content.Context;
|
||||
import android.hardware.camera2.CameraAccessException;
|
||||
import android.hardware.camera2.CameraCharacteristics;
|
||||
import android.hardware.camera2.CameraManager;
|
||||
import android.hardware.display.DisplayManager;
|
||||
import android.hardware.display.DisplayManager.DisplayListener;
|
||||
import android.view.Display;
|
||||
import android.view.Surface;
|
||||
import android.view.WindowManager;
|
||||
import com.google.ar.core.Session;
|
||||
|
||||
/**
|
||||
* Helper to track the display rotations. In particular, the 180 degree rotations are not notified
|
||||
* by the onSurfaceChanged() callback, and thus they require listening to the android display
|
||||
* events.
|
||||
*/
|
||||
public final class DisplayRotationHelper implements DisplayListener {
|
||||
private boolean viewportChanged;
|
||||
private int viewportWidth;
|
||||
private int viewportHeight;
|
||||
private final Display display;
|
||||
private final DisplayManager displayManager;
|
||||
private final CameraManager cameraManager;
|
||||
|
||||
/**
|
||||
* Constructs the DisplayRotationHelper but does not register the listener yet.
|
||||
*
|
||||
* @param context the Android {@link Context}.
|
||||
*/
|
||||
public DisplayRotationHelper(Context context) {
|
||||
displayManager = (DisplayManager) context.getSystemService(Context.DISPLAY_SERVICE);
|
||||
cameraManager = (CameraManager) context.getSystemService(Context.CAMERA_SERVICE);
|
||||
WindowManager windowManager = (WindowManager) context.getSystemService(Context.WINDOW_SERVICE);
|
||||
display = windowManager.getDefaultDisplay();
|
||||
}
|
||||
|
||||
/** Registers the display listener. Should be called from {@link Activity#onResume()}. */
|
||||
public void onResume() {
|
||||
displayManager.registerDisplayListener(this, null);
|
||||
}
|
||||
|
||||
/** Unregisters the display listener. Should be called from {@link Activity#onPause()}. */
|
||||
public void onPause() {
|
||||
displayManager.unregisterDisplayListener(this);
|
||||
}
|
||||
|
||||
/**
|
||||
* Records a change in surface dimensions. This will be later used by {@link
|
||||
* #updateSessionIfNeeded(Session)}. Should be called from {@link
|
||||
* android.opengl.GLSurfaceView.Renderer
|
||||
* #onSurfaceChanged(javax.microedition.khronos.opengles.GL10, int, int)}.
|
||||
*
|
||||
* @param width the updated width of the surface.
|
||||
* @param height the updated height of the surface.
|
||||
*/
|
||||
public void onSurfaceChanged(int width, int height) {
|
||||
viewportWidth = width;
|
||||
viewportHeight = height;
|
||||
viewportChanged = true;
|
||||
}
|
||||
|
||||
/**
|
||||
* Updates the session display geometry if a change was posted either by {@link
|
||||
* #onSurfaceChanged(int, int)} call or by {@link #onDisplayChanged(int)} system callback. This
|
||||
* function should be called explicitly before each call to {@link Session#update()}. This
|
||||
* function will also clear the 'pending update' (viewportChanged) flag.
|
||||
*
|
||||
* @param session the {@link Session} object to update if display geometry changed.
|
||||
*/
|
||||
public void updateSessionIfNeeded(ARCoreSharedCamera session) {
|
||||
if (viewportChanged) {
|
||||
int displayRotation = display.getRotation();
|
||||
session.setDisplayGeometry(displayRotation, viewportWidth, viewportHeight);
|
||||
viewportChanged = false;
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns the aspect ratio of the GL surface viewport while accounting for the display rotation
|
||||
* relative to the device camera sensor orientation.
|
||||
*/
|
||||
public float getCameraSensorRelativeViewportAspectRatio(String cameraId) {
|
||||
float aspectRatio;
|
||||
int cameraSensorToDisplayRotation = getCameraSensorToDisplayRotation(cameraId);
|
||||
switch (cameraSensorToDisplayRotation) {
|
||||
case 90:
|
||||
case 270:
|
||||
aspectRatio = (float) viewportHeight / (float) viewportWidth;
|
||||
break;
|
||||
case 0:
|
||||
case 180:
|
||||
aspectRatio = (float) viewportWidth / (float) viewportHeight;
|
||||
break;
|
||||
default:
|
||||
throw new RuntimeException("Unhandled rotation: " + cameraSensorToDisplayRotation);
|
||||
}
|
||||
return aspectRatio;
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns the rotation of the back-facing camera with respect to the display. The value is one of
|
||||
* 0, 90, 180, 270.
|
||||
*/
|
||||
public int getCameraSensorToDisplayRotation(String cameraId) {
|
||||
CameraCharacteristics characteristics;
|
||||
try {
|
||||
characteristics = cameraManager.getCameraCharacteristics(cameraId);
|
||||
} catch (CameraAccessException e) {
|
||||
throw new RuntimeException("Unable to determine display orientation", e);
|
||||
}
|
||||
|
||||
// Camera sensor orientation.
|
||||
int sensorOrientation = characteristics.get(CameraCharacteristics.SENSOR_ORIENTATION);
|
||||
|
||||
// Current display orientation.
|
||||
int displayOrientation = toDegrees(display.getRotation());
|
||||
|
||||
// Make sure we return 0, 90, 180, or 270 degrees.
|
||||
return (sensorOrientation - displayOrientation + 360) % 360;
|
||||
}
|
||||
|
||||
private int toDegrees(int rotation) {
|
||||
switch (rotation) {
|
||||
case Surface.ROTATION_0:
|
||||
return 0;
|
||||
case Surface.ROTATION_90:
|
||||
return 90;
|
||||
case Surface.ROTATION_180:
|
||||
return 180;
|
||||
case Surface.ROTATION_270:
|
||||
return 270;
|
||||
default:
|
||||
throw new RuntimeException("Unknown rotation " + rotation);
|
||||
}
|
||||
}
|
||||
|
||||
@Override
|
||||
public void onDisplayAdded(int displayId) {}
|
||||
|
||||
@Override
|
||||
public void onDisplayRemoved(int displayId) {}
|
||||
|
||||
@Override
|
||||
public void onDisplayChanged(int displayId) {
|
||||
viewportChanged = true;
|
||||
}
|
||||
}
|
||||
@@ -104,7 +104,7 @@ public class RTABMapActivity extends FragmentActivity implements OnClickListener
|
||||
|
||||
// Tag for debug logging.
|
||||
public static final String TAG = RTABMapActivity.class.getSimpleName();
|
||||
public static boolean DISABLE_LOG = true;
|
||||
public static boolean DISABLE_LOG = false;
|
||||
|
||||
// The minimum Tango Core version required from this application.
|
||||
private static final int MIN_TANGO_CORE_VERSION = 9377;
|
||||
|
||||
@@ -134,13 +134,30 @@ 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);
|
||||
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 fx, float fy, float cx, float cy,
|
||||
float rgb_fx, float rgb_fy, float rgb_cx, float rgb_cy,
|
||||
float rgbFrameX, float rgbFrameY, float rgbFrameZ, float rgbFrameQX, float rgbFrameQY, float rgbFrameQZ, float rgbFrameQW,
|
||||
double stamp,
|
||||
ByteBuffer yPlane, ByteBuffer uPlane, ByteBuffer vPlane, int yPlaneLen, int rgbWidth, int rgbHeight, int rgbFormat,
|
||||
FloatBuffer points, int pointsLen,
|
||||
float vx, float vy, float vz, float vqx, float vqy, float vqz, float vqw, //view matrix
|
||||
float p00, float p11, float p02, float p12, float p22, float p32, float p23, // projection matrix
|
||||
float t0, float t1, float t2, float t3, float t4, float t5, float t6, float t7); // tex coord
|
||||
public static native void postOdometryEventDepth(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,
|
||||
float depth_fx, float depth_fy, float depth_cx, float depth_cy,
|
||||
float rgbFrameX, float rgbFrameY, float rgbFrameZ, float rgbFrameQX, float rgbFrameQY, float rgbFrameQZ, float rgbFrameQW,
|
||||
float depthFrameX, float depthFrameY, float depthFrameZ, float depthFrameQX, float depthFrameQY, float depthFrameQZ, float depthFrameQW,
|
||||
double rgbStamp,
|
||||
double depthStamp,
|
||||
ByteBuffer yPlane, ByteBuffer uPlane, ByteBuffer vPlane, int yPlaneLen, int rgbWidth, int rgbHeight, int rgbFormat,
|
||||
ByteBuffer depth, int depthLen, int depthWidth, int depthHeight, int depthFormat,
|
||||
FloatBuffer points, int pointsLen);
|
||||
FloatBuffer points, int pointsLen,
|
||||
float vx, float vy, float vz, float vqx, float vqy, float vqz, float vqw, //view matrix
|
||||
float p00, float p11, float p02, float p12, float p22, float p32, float p23, // projection matrix
|
||||
float t0, float t1, float t2, float t3, float t4, float t5, float t6, float t7); // tex coord
|
||||
|
||||
}
|
||||
|
||||
@@ -44,12 +44,14 @@ public class Renderer implements GLSurfaceView.Renderer {
|
||||
private float mTextColor = 1.0f;
|
||||
private int mOffset = 0;
|
||||
private ARCoreSharedCamera mCamera = null;
|
||||
private DisplayRotationHelper mDisplayRotationHelper = null;
|
||||
|
||||
private Vector<TextObject> mTexts;
|
||||
|
||||
private static RTABMapActivity mActivity;
|
||||
public Renderer(RTABMapActivity c) {
|
||||
mActivity = c;
|
||||
mDisplayRotationHelper = new DisplayRotationHelper(/*context=*/ c);
|
||||
}
|
||||
|
||||
private ProgressDialog mProgressDialog = null;
|
||||
@@ -76,6 +78,11 @@ public class Renderer implements GLSurfaceView.Renderer {
|
||||
public void setCamera(ARCoreSharedCamera camera)
|
||||
{
|
||||
mCamera = camera;
|
||||
if(mCamera!=null)
|
||||
{
|
||||
mDisplayRotationHelper.onDisplayChanged(0);
|
||||
mDisplayRotationHelper.updateSessionIfNeeded(mCamera);
|
||||
}
|
||||
}
|
||||
|
||||
// Render loop of the Gl context.
|
||||
@@ -176,6 +183,12 @@ public class Renderer implements GLSurfaceView.Renderer {
|
||||
RTABMapLib.setupGraphic(mActivity.nativeApplication, width, height);
|
||||
}
|
||||
|
||||
mDisplayRotationHelper.onSurfaceChanged(width, height);
|
||||
if(mCamera!=null)
|
||||
{
|
||||
mDisplayRotationHelper.updateSessionIfNeeded(mCamera);
|
||||
}
|
||||
|
||||
mSurfaceHeight = (float)height;
|
||||
|
||||
// Clear our matrices
|
||||
|
||||
@@ -2,18 +2,33 @@ package com.introlab.rtabmap;
|
||||
|
||||
|
||||
import android.graphics.ImageFormat;
|
||||
import android.hardware.camera2.CameraAccessException;
|
||||
import android.hardware.camera2.CameraCaptureSession;
|
||||
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.media.ImageReader;
|
||||
import android.os.Handler;
|
||||
import android.os.HandlerThread;
|
||||
import android.support.annotation.NonNull;
|
||||
import android.util.Log;
|
||||
import android.view.Surface;
|
||||
|
||||
import java.nio.ByteBuffer;
|
||||
import java.util.ArrayList;
|
||||
import java.util.Arrays;
|
||||
import java.util.List;
|
||||
|
||||
public class TOF_ImageReader implements ImageReader.OnImageAvailableListener {
|
||||
|
||||
public int WIDTH;
|
||||
public int HEIGHT;
|
||||
public static final String TAG = TOF_ImageReader.class.getSimpleName();
|
||||
|
||||
public int WIDTH;
|
||||
public int HEIGHT;
|
||||
|
||||
public ImageReader imageReader;
|
||||
public int frameCount = 0;
|
||||
public long timestamp;
|
||||
@@ -21,16 +36,177 @@ public class TOF_ImageReader implements ImageReader.OnImageAvailableListener {
|
||||
// Looper handler thread.
|
||||
private HandlerThread backgroundThread;
|
||||
// Looper handler.
|
||||
private Handler backgroundHandler;
|
||||
public Handler backgroundHandler;
|
||||
|
||||
public ByteBuffer depth16_raw;
|
||||
|
||||
// Camera capture session.
|
||||
private CameraCaptureSession captureSession = null;
|
||||
// Camera device.
|
||||
private CameraDevice cameraDevice = null;
|
||||
|
||||
// Camera preview capture request builder
|
||||
private CaptureRequest.Builder previewCaptureRequestBuilder;
|
||||
|
||||
|
||||
TOF_ImageReader(){
|
||||
}
|
||||
|
||||
public void close()
|
||||
{
|
||||
Log.i(TAG, "close()");
|
||||
if (captureSession != null) {
|
||||
captureSession.close();
|
||||
captureSession = null;
|
||||
}
|
||||
if (cameraDevice != null) {
|
||||
cameraDevice.close();
|
||||
cameraDevice = null;
|
||||
}
|
||||
|
||||
if (imageReader != null) {
|
||||
imageReader.close();
|
||||
imageReader = null;
|
||||
}
|
||||
}
|
||||
|
||||
// Camera device state callback.
|
||||
public final CameraDevice.StateCallback cameraDeviceCallback =
|
||||
new CameraDevice.StateCallback() {
|
||||
@Override
|
||||
public void onOpened(@NonNull CameraDevice cameraDevice) {
|
||||
Log.d(TAG, "Camera depth ID " + cameraDevice.getId() + " opened.");
|
||||
TOF_ImageReader.this.cameraDevice = cameraDevice;
|
||||
createCameraPreviewSession();
|
||||
}
|
||||
|
||||
@Override
|
||||
public void onClosed(@NonNull CameraDevice cameraDevice) {
|
||||
Log.d(TAG, "Camera device ID " + cameraDevice.getId() + " closed.");
|
||||
TOF_ImageReader.this.cameraDevice = null;
|
||||
}
|
||||
|
||||
@Override
|
||||
public void onDisconnected(@NonNull CameraDevice cameraDevice) {
|
||||
Log.w(TAG, "Camera depth ID " + cameraDevice.getId() + " disconnected.");
|
||||
cameraDevice.close();
|
||||
TOF_ImageReader.this.cameraDevice = null;
|
||||
}
|
||||
|
||||
@Override
|
||||
public void onError(@NonNull CameraDevice cameraDevice, int error) {
|
||||
Log.e(TAG, "Camera depth ID " + cameraDevice.getId() + " error " + error);
|
||||
cameraDevice.close();
|
||||
TOF_ImageReader.this.cameraDevice = null;
|
||||
}
|
||||
};
|
||||
|
||||
private CameraCaptureSession.StateCallback captureStateCallback = 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.");
|
||||
}
|
||||
|
||||
@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);
|
||||
}
|
||||
};
|
||||
|
||||
// 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.");
|
||||
frameCount = 0;
|
||||
try {
|
||||
|
||||
// Create an ARCore compatible capture request using `TEMPLATE_RECORD`.
|
||||
previewCaptureRequestBuilder = cameraDevice.createCaptureRequest(CameraDevice.TEMPLATE_RECORD);
|
||||
|
||||
previewCaptureRequestBuilder.addTarget(imageReader.getSurface());
|
||||
|
||||
// Create camera capture session for camera preview using callback.
|
||||
cameraDevice.createCaptureSession(Arrays.asList(imageReader.getSurface()), captureStateCallback, backgroundHandler);
|
||||
|
||||
} catch (CameraAccessException e) {
|
||||
Log.e(TAG, "CameraAccessException", e);
|
||||
}
|
||||
}
|
||||
|
||||
public void createImageReader(int width, int height){
|
||||
this.WIDTH = width;
|
||||
this.HEIGHT = height;
|
||||
Log.w(TAG, String.valueOf(width) + "x" + String.valueOf(height));
|
||||
WIDTH = width;
|
||||
HEIGHT = height;
|
||||
this.imageReader =
|
||||
ImageReader.newInstance(
|
||||
width,
|
||||
@@ -45,18 +221,20 @@ public class TOF_ImageReader implements ImageReader.OnImageAvailableListener {
|
||||
public void onImageAvailable(ImageReader imageReader) {
|
||||
Image image = imageReader.acquireLatestImage();
|
||||
if (image == null) {
|
||||
Log.w("RTABMapActivity", "onImageAvailable: Skipping null image.");
|
||||
Log.w(TAG, "onImageAvailable: Skipping null image.");
|
||||
return;
|
||||
}
|
||||
else{
|
||||
if(image.getFormat() == ImageFormat.DEPTH16){
|
||||
this.timestamp = image.getTimestamp();
|
||||
depth16_raw = image.getPlanes()[0].getBuffer().asReadOnlyBuffer();
|
||||
synchronized (this) {
|
||||
depth16_raw = image.getPlanes()[0].getBuffer().asReadOnlyBuffer();
|
||||
timestamp = image.getTimestamp();
|
||||
}
|
||||
// copy raw undecoded DEPTH16 format depth data to NativeBuffer
|
||||
frameCount++;
|
||||
}
|
||||
else{
|
||||
Log.w("RTABMapActivity", "onImageAvailable: depth image not in DEPTH16 format, skipping image");
|
||||
Log.w(TAG, "onImageAvailable: depth image not in DEPTH16 format, skipping image");
|
||||
}
|
||||
}
|
||||
image.close();
|
||||
@@ -78,7 +256,7 @@ public class TOF_ImageReader implements ImageReader.OnImageAvailableListener {
|
||||
this.backgroundThread = null;
|
||||
this.backgroundHandler = null;
|
||||
} catch (InterruptedException e) {
|
||||
Log.e("RTABMapActivity", "Interrupted while trying to join depth background handler thread", e);
|
||||
Log.e(TAG, "Interrupted while trying to join depth background handler thread", e);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user