diff --git a/src/android/jni/stream_profile.cpp b/src/android/jni/stream_profile.cpp index 155e972550..774b490d28 100644 --- a/src/android/jni/stream_profile.cpp +++ b/src/android/jni/stream_profile.cpp @@ -2,6 +2,7 @@ // Copyright(c) 2019 Intel Corporation. All Rights Reserved. #include +#include //for memcpy #include "error.h" #include "../../../include/librealsense2/rs.h" @@ -70,3 +71,162 @@ Java_com_intel_realsense_librealsense_StreamProfile_nIsProfileExtendableTo(JNIEn handle_error(env, e); return rv; } + +extern "C" +JNIEXPORT void JNICALL +Java_com_intel_realsense_librealsense_StreamProfile_nGetExtrinsicTo(JNIEnv *env, jclass type, + jlong handle, jlong otherHandle, + jobject extrinsic) { + rs2_error* e = nullptr; + rs2_extrinsics extr; + rs2_get_extrinsics(reinterpret_cast(handle), + reinterpret_cast(otherHandle),&extr, &e); + handle_error(env, e); + + if (e != nullptr) + { + return; + } + + jclass clazz = env->GetObjectClass(extrinsic); + + //retrieving the array members + //mRotation + jfieldID rotation_field = env->GetFieldID(clazz, "mRotation", "[F"); + jfloatArray rotationArray = env->NewFloatArray(9); + if (rotationArray != NULL) + { + env->SetFloatArrayRegion(rotationArray, 0, 9, reinterpret_cast(&extr.rotation)); + } + env->SetObjectField(extrinsic, rotation_field, rotationArray); + + //mTranslation + jfieldID translation_field = env->GetFieldID(clazz, "mTranslation", "[F"); + jfloatArray translationArray = env->NewFloatArray(3); + if (translationArray != NULL) + { + env->SetFloatArrayRegion(translationArray, 0, 3, reinterpret_cast(&extr.translation)); + } + env->SetObjectField(extrinsic, translation_field, translationArray); +} + +extern "C" +JNIEXPORT void JNICALL +Java_com_intel_realsense_librealsense_StreamProfile_nRegisterExtrinsic(JNIEnv *env, jclass type, + jlong fromHandle, jlong toHandle, + jobject extrinsic) +{ + rs2_error* e = nullptr; + rs2_extrinsics extr; + + //fill c++ extrinsic from java extrinsic data + jclass clazz = env->GetObjectClass(extrinsic); + //fill rotation + jfieldID rotation_field = env->GetFieldID(clazz, "mRotation", "[F"); + jobject rotationObject = env->GetObjectField(extrinsic, rotation_field); + jfloatArray* rotationArray = reinterpret_cast(&rotationObject); + jfloat * rotation = env->GetFloatArrayElements(*rotationArray, NULL); + memcpy(extr.rotation, rotation, 9 * sizeof(float)); + env->ReleaseFloatArrayElements(*rotationArray, rotation, 0); + //fill translation + jfieldID translation_field = env->GetFieldID(clazz, "mTranslation", "[F"); + jobject translationObject = env->GetObjectField(extrinsic, translation_field); + jfloatArray* translationArray = reinterpret_cast(&translationObject); + jfloat * translation = env->GetFloatArrayElements(*translationArray, NULL); + memcpy(extr.translation, translation, 9 * sizeof(float)); + env->ReleaseFloatArrayElements(*translationArray, translation, 0); + + //calling the api method + rs2_register_extrinsics(reinterpret_cast(fromHandle), + reinterpret_cast(toHandle), extr, &e); + handle_error(env, e); +} + +extern "C" +JNIEXPORT void JNICALL +Java_com_intel_realsense_librealsense_VideoStreamProfile_nGetIntrinsic(JNIEnv *env, jclass type, + jlong handle, jobject intrinsic) { + rs2_error* e = nullptr; + rs2_intrinsics intr; + rs2_get_video_stream_intrinsics(reinterpret_cast(handle), &intr, &e); + handle_error(env, e); + + if (e != nullptr) + { + return; + } + + jclass clazz = env->GetObjectClass(intrinsic); + + //retrieving all the built-in types members + jfieldID width_field = env->GetFieldID(clazz, "mWidth", "I"); + jfieldID height_field = env->GetFieldID(clazz, "mHeight", "I"); + jfieldID ppx_field = env->GetFieldID(clazz, "mPpx", "F"); + jfieldID ppy_field = env->GetFieldID(clazz, "mPpy", "F"); + jfieldID fx_field = env->GetFieldID(clazz, "mFx", "F"); + jfieldID fy_field = env->GetFieldID(clazz, "mFy", "F"); + jfieldID model_field = env->GetFieldID(clazz, "mModelValue", "I"); + + + env->SetIntField(intrinsic, width_field, intr.width); + env->SetIntField(intrinsic, height_field, intr.height); + env->SetFloatField(intrinsic, ppx_field, intr.ppx); + env->SetFloatField(intrinsic, ppy_field, intr.ppy); + env->SetFloatField(intrinsic, fx_field, intr.fx); + env->SetFloatField(intrinsic, fy_field, intr.fy); + env->SetIntField(intrinsic, model_field, intr.model); + + //retrieving the array member + jfieldID coeff_field = env->GetFieldID(clazz, "mCoeffs", "[F"); + jfloatArray coeffsArray = env->NewFloatArray(5); + if (coeffsArray != NULL) + { + env->SetFloatArrayRegion(coeffsArray, 0, 5, reinterpret_cast(&intr.coeffs)); + } + env->SetObjectField(intrinsic, coeff_field, coeffsArray); +} + +extern "C" +JNIEXPORT void JNICALL +Java_com_intel_realsense_librealsense_MotionStreamProfile_nGetIntrinsic(JNIEnv *env, jclass type, + jlong handle, jobject intrinsic) { + rs2_error* e = nullptr; + rs2_motion_device_intrinsic intr; + rs2_get_motion_intrinsics(reinterpret_cast(handle), &intr, &e); + handle_error(env, e); + + if (e != nullptr) + { + return; + } + + jclass clazz = env->GetObjectClass(intrinsic); + + //retrieving the array members + //mData + jfieldID data_field = env->GetFieldID(clazz, "mData", "[F"); + jfloatArray dataArray = env->NewFloatArray(12); + if (dataArray != NULL) + { + env->SetFloatArrayRegion(dataArray, 0, 12, reinterpret_cast(&intr.data)); + } + env->SetObjectField(intrinsic, data_field, dataArray); + + //mNoiseVariances + jfieldID noise_field = env->GetFieldID(clazz, "mNoiseVariances", "[F"); + jfloatArray noiseArray = env->NewFloatArray(3); + if (noiseArray != NULL) + { + env->SetFloatArrayRegion(noiseArray, 0, 3, reinterpret_cast(&intr.noise_variances)); + } + env->SetObjectField(intrinsic, noise_field, noiseArray); + + //mBiasVariances + jfieldID bias_field = env->GetFieldID(clazz, "mBiasVariances", "[F"); + jfloatArray biasArray = env->NewFloatArray(3); + if (biasArray != NULL) + { + env->SetFloatArrayRegion(biasArray, 0, 3, reinterpret_cast(&intr.bias_variances)); + } + env->SetObjectField(intrinsic, bias_field, biasArray); +} diff --git a/wrappers/android/librealsense/src/main/java/com/intel/realsense/librealsense/DistortionType.java b/wrappers/android/librealsense/src/main/java/com/intel/realsense/librealsense/DistortionType.java new file mode 100644 index 0000000000..d18f399196 --- /dev/null +++ b/wrappers/android/librealsense/src/main/java/com/intel/realsense/librealsense/DistortionType.java @@ -0,0 +1,16 @@ +package com.intel.realsense.librealsense; + +public enum DistortionType { + NONE(0), + MODIFIED_BROWN_CONRADY(1), + INVERSE_BROWN_CONRADY(2), + FTHETA(3), + BROWN_CONRADY(4), + KANNALA_BRANDT4(5), + COUNT(6); + + private final int mValue; + + DistortionType(int value) { mValue = value; } + public int value() { return mValue; } +} diff --git a/wrappers/android/librealsense/src/main/java/com/intel/realsense/librealsense/Extrinsic.java b/wrappers/android/librealsense/src/main/java/com/intel/realsense/librealsense/Extrinsic.java new file mode 100644 index 0000000000..5b771e830e --- /dev/null +++ b/wrappers/android/librealsense/src/main/java/com/intel/realsense/librealsense/Extrinsic.java @@ -0,0 +1,17 @@ +package com.intel.realsense.librealsense; + +public class Extrinsic { + + private float[] mRotation; // Column-major 3x3 rotation matrix + private float[] mTranslation; // Three-element translation vector, in meters + + public Extrinsic(){ + mRotation = new float[9]; + mTranslation = new float[3]; + } + + public Extrinsic(float[] rotation, float[] translation){ + this.mRotation = rotation; + this.mTranslation = translation; + } +} diff --git a/wrappers/android/librealsense/src/main/java/com/intel/realsense/librealsense/Intrinsic.java b/wrappers/android/librealsense/src/main/java/com/intel/realsense/librealsense/Intrinsic.java new file mode 100644 index 0000000000..0b490a5d4c --- /dev/null +++ b/wrappers/android/librealsense/src/main/java/com/intel/realsense/librealsense/Intrinsic.java @@ -0,0 +1,38 @@ +package com.intel.realsense.librealsense; + +public class Intrinsic { + private int mWidth; + private int mHeight; + private float mPpx; + private float mPpy; + private float mFx; + private float mFy; + private DistortionType mModel; + private int mModelValue; + private float[] mCoeffs; + + + public Intrinsic(){ + mCoeffs = new float[5]; + } + + public Intrinsic(int width, int height, + float ppx, float ppy, + float fx, float fy, + int model, + float[] coeffs){ + this.mWidth = width; + this.mHeight = height; + this.mPpx = ppx; + this.mPpy = ppy; + this.mFx = fx; + this.mFy = fy; + this.mModel = DistortionType.values()[model]; + this.mModelValue = model; + this.mCoeffs = coeffs; + } + + public void SetModel(){ + this.mModel = DistortionType.values()[mModelValue]; + } +} diff --git a/wrappers/android/librealsense/src/main/java/com/intel/realsense/librealsense/MotionIntrinsic.java b/wrappers/android/librealsense/src/main/java/com/intel/realsense/librealsense/MotionIntrinsic.java new file mode 100644 index 0000000000..14d0427964 --- /dev/null +++ b/wrappers/android/librealsense/src/main/java/com/intel/realsense/librealsense/MotionIntrinsic.java @@ -0,0 +1,25 @@ +package com.intel.realsense.librealsense; + +public class MotionIntrinsic { + + /* mData matrix description: + * Scale X cross axis cross axis Bias X \n + * cross axis Scale Y cross axis Bias Y \n + * cross axis cross axis Scale Z Bias Z */ + private float[][] mData; + private float[] mNoiseVariances; // Variance of noise for X, Y, and Z axis + private float[] mBiasVariances; // Variance of bias for X, Y, and Z axis + + + public MotionIntrinsic(){ + mData = new float[3][4]; + mNoiseVariances = new float[3]; + mBiasVariances = new float[3]; + } + + public MotionIntrinsic(float[][] data, float[] noiseVariances, float[] biasVariances){ + this.mData = data; + this.mNoiseVariances = noiseVariances; + this.mBiasVariances = biasVariances; + } +} diff --git a/wrappers/android/librealsense/src/main/java/com/intel/realsense/librealsense/MotionStreamProfile.java b/wrappers/android/librealsense/src/main/java/com/intel/realsense/librealsense/MotionStreamProfile.java index 71b4b62c3b..4f45ca50c8 100644 --- a/wrappers/android/librealsense/src/main/java/com/intel/realsense/librealsense/MotionStreamProfile.java +++ b/wrappers/android/librealsense/src/main/java/com/intel/realsense/librealsense/MotionStreamProfile.java @@ -1,8 +1,22 @@ package com.intel.realsense.librealsense; public class MotionStreamProfile extends StreamProfile { + + private MotionIntrinsic mIntrinsic; + MotionStreamProfile(long handle) { super(handle); mOwner = false; + mIntrinsic = null; } + + public MotionIntrinsic getIntrinsic() throws Exception { + if(mIntrinsic == null){ + mIntrinsic = new MotionIntrinsic(); + nGetIntrinsic(mHandle, mIntrinsic); + } + return mIntrinsic; + } + + private static native void nGetIntrinsic(long handle, MotionIntrinsic intrinsic); } diff --git a/wrappers/android/librealsense/src/main/java/com/intel/realsense/librealsense/StreamProfile.java b/wrappers/android/librealsense/src/main/java/com/intel/realsense/librealsense/StreamProfile.java index f23f3731b2..03eda5740c 100644 --- a/wrappers/android/librealsense/src/main/java/com/intel/realsense/librealsense/StreamProfile.java +++ b/wrappers/android/librealsense/src/main/java/com/intel/realsense/librealsense/StreamProfile.java @@ -43,6 +43,17 @@ public int getFrameRate() { return mPp.frameRate; } + public Extrinsic getExtrinsicTo(StreamProfile other) throws Exception { + Extrinsic extrinsic = new Extrinsic(); + nGetExtrinsicTo(mHandle, other.mHandle, extrinsic); + return extrinsic; + } + + public void registerExtrinsic(StreamProfile other, Extrinsic extrinsic) + { + nRegisterExtrinsic(mHandle, other.mHandle, extrinsic); + } + public boolean is(Extension extension) { return nIsProfileExtendableTo(mHandle, extension.value()); } @@ -64,4 +75,6 @@ public void close() { private static native boolean nIsProfileExtendableTo(long handle, int extension); private static native void nGetProfile(long handle, ProfileParams params); private static native void nDelete(long handle); + private static native void nGetExtrinsicTo(long handle, long otherHandle, Extrinsic extrinsic); + private static native void nRegisterExtrinsic(long handle, long otherHandle, Extrinsic extrinsic); } diff --git a/wrappers/android/librealsense/src/main/java/com/intel/realsense/librealsense/VideoStreamProfile.java b/wrappers/android/librealsense/src/main/java/com/intel/realsense/librealsense/VideoStreamProfile.java index b6c2d6cb24..8f5c10f957 100644 --- a/wrappers/android/librealsense/src/main/java/com/intel/realsense/librealsense/VideoStreamProfile.java +++ b/wrappers/android/librealsense/src/main/java/com/intel/realsense/librealsense/VideoStreamProfile.java @@ -2,6 +2,7 @@ public class VideoStreamProfile extends StreamProfile { ResolutionParams mResolutionParams; + private Intrinsic mIntrinsic; private class ResolutionParams { public int width; @@ -13,6 +14,16 @@ private class ResolutionParams { mOwner = false; mResolutionParams = new ResolutionParams(); nGetResolution(mHandle, mResolutionParams); + mIntrinsic = null; + } + + public Intrinsic getIntrinsic() throws Exception { + if(mIntrinsic == null){ + mIntrinsic = new Intrinsic(); + nGetIntrinsic(mHandle, mIntrinsic); + mIntrinsic.SetModel(); + } + return mIntrinsic; } public int getWidth() { @@ -24,4 +35,5 @@ public int getHeight() { } private static native void nGetResolution(long handle, ResolutionParams params); + private static native void nGetIntrinsic(long handle, Intrinsic intrinsic); }