Merge pull request #6594 from remibettan/android-intrisics-extrinsics…

…-added-to-wrapper

Android intrisics extrinsics added to wrapper

src/android/jni/stream_profile.cpp

@@ -2,6 +2,7 @@
 // Copyright(c) 2019 Intel Corporation. All Rights Reserved.
 
 #include <jni.h>
+#include <cstring>      //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<const rs2_stream_profile *>(handle),
+            reinterpret_cast<const rs2_stream_profile *>(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<jfloat*>(&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<jfloat*>(&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<jfloatArray *>(&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<jfloatArray *>(&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<const rs2_stream_profile *>(fromHandle),
+                       reinterpret_cast<const rs2_stream_profile *>(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<const rs2_stream_profile *>(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<jfloat*>(&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<const rs2_stream_profile *>(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<jfloat*>(&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<jfloat*>(&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<jfloat*>(&intr.bias_variances));
+    }
+    env->SetObjectField(intrinsic, bias_field, biasArray);
+}

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; }
+}

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;
+    }
+}

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];
+    }
+}

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;
+    }
+}

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);
 }

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);
 }

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);
 }