android-intrinsics-extrinsics-utils-functions

src/android/CMakeLists.txt

@@ -25,6 +25,7 @@ target_sources(${LRS_TARGET}
         "${CMAKE_CURRENT_LIST_DIR}/jni/processing.cpp"
         "${CMAKE_CURRENT_LIST_DIR}/jni/options.cpp"
         "${CMAKE_CURRENT_LIST_DIR}/jni/advanced_mode.cpp"
+        "${CMAKE_CURRENT_LIST_DIR}/jni/utils.cpp"
 
         "${CMAKE_CURRENT_LIST_DIR}/fw-logger/rs-fw-logger.cpp"
         "${CMAKE_CURRENT_LIST_DIR}/../../tools/fw-logger/fw-log-data.cpp"

src/android/jni/utils.cpp

@@ -0,0 +1,173 @@
+// License: Apache 2.0. See LICENSE file in root directory.
+// Copyright(c) 2020 Intel Corporation. All Rights Reserved.
+
+#include <jni.h>
+#include "error.h"
+#include "../../../include/librealsense2/rs.hpp"
+#include "../../../include/librealsense2/rsutil.h"
+
+
+extern "C" JNIEXPORT void JNICALL
+Java_com_intel_realsense_librealsense_Utils_nProjectPointToPixel(JNIEnv *env, jclass type,
+                                                              jobject pixel_2D, jobject intrinsic,
+                                                              jobject point_3D) {
+    // retrieving float[3] from point_3D
+    jclass point_3D_class = env->GetObjectClass(point_3D);
+    jfieldID point_x_field = env->GetFieldID(point_3D_class, "mX", "F");
+    jfieldID point_y_field = env->GetFieldID(point_3D_class, "mY", "F");
+    jfieldID point_z_field = env->GetFieldID(point_3D_class, "mZ", "F");
+    float* point = new float[3];
+    point[0] =  env->GetFloatField(point_3D, point_x_field);
+    point[1] =  env->GetFloatField(point_3D, point_y_field);
+    point[2] =  env->GetFloatField(point_3D, point_z_field);
+
+
+    // retrieving rs2_intrinsics from intrinsic object
+    jclass intrinsic_class = env->GetObjectClass(intrinsic);
+    jfieldID width_field = env->GetFieldID(intrinsic_class, "mWidth", "I");
+    jfieldID height_field = env->GetFieldID(intrinsic_class, "mHeight", "I");
+    jfieldID ppx_field = env->GetFieldID(intrinsic_class, "mPpx", "F");
+    jfieldID ppy_field = env->GetFieldID(intrinsic_class, "mPpy", "F");
+    jfieldID fx_field = env->GetFieldID(intrinsic_class, "mFx", "F");
+    jfieldID fy_field = env->GetFieldID(intrinsic_class, "mFy", "F");
+    jfieldID model_field = env->GetFieldID(intrinsic_class, "mModelValue", "I");
+    jfieldID coeff_field = env->GetFieldID(intrinsic_class, "mCoeffs", "[F");
+    rs2_intrinsics intrinsics;
+    intrinsics.width = env->GetIntField(intrinsic, width_field);
+    intrinsics.height = env->GetIntField(intrinsic, height_field);
+    intrinsics.ppx = env->GetFloatField(intrinsic, ppx_field);
+    intrinsics.ppy = env->GetFloatField(intrinsic, ppy_field);
+    intrinsics.fx = env->GetFloatField(intrinsic, fx_field);
+    intrinsics.fy = env->GetFloatField(intrinsic, fy_field);
+    intrinsics.model = (rs2_distortion)env->GetIntField(intrinsic, model_field);
+
+    jobject coeffsObject = env->GetObjectField(intrinsic, coeff_field);
+    jfloatArray* coeffsArray = reinterpret_cast<jfloatArray *>(&coeffsObject);
+    jfloat * coeffs = env->GetFloatArrayElements(*coeffsArray, NULL);
+    memcpy(intrinsics.coeffs, coeffs, 5 * sizeof(float));
+    env->ReleaseFloatArrayElements(*coeffsArray, coeffs, 0);
+
+    // preparing struct to be filled by API function
+    float* pixel = new float[2];
+    pixel[0] = 0.f;
+    pixel[1] = 0.f;
+
+    rs2_project_point_to_pixel(pixel, &intrinsics, point);
+
+    // returning pixel into pixel_2D struct
+    jclass pixel_2D_class = env->GetObjectClass(pixel_2D);
+    jfieldID pixel_x_field = env->GetFieldID(pixel_2D_class, "mX", "I");
+    jfieldID pixel_y_field = env->GetFieldID(pixel_2D_class, "mY", "I");
+    env->SetIntField(pixel_2D, pixel_x_field, pixel[0]);
+    env->SetIntField(pixel_2D, pixel_y_field, pixel[1]);
+}
+
+extern "C" JNIEXPORT void JNICALL
+Java_com_intel_realsense_librealsense_Utils_nDeprojectPixelToPoint(JNIEnv *env, jclass type,
+                                                                 jobject point_3D, jobject intrinsic,
+                                                                 jobject pixel_2D, jfloat depth) {
+
+    // retrieving float[2] from pixel_2D
+    jclass pixel_2D_class = env->GetObjectClass(pixel_2D);
+    jfieldID pixel_x_field = env->GetFieldID(pixel_2D_class, "mX", "I");
+    jfieldID pixel_y_field = env->GetFieldID(pixel_2D_class, "mY", "I");
+    float* pixel = new float[2];
+    pixel[0] = env->GetIntField(pixel_2D, pixel_x_field);
+    pixel[1] = env->GetIntField(pixel_2D, pixel_y_field);
+
+    // retrieving rs2_intrinsics from intrinsic object
+    jclass intrinsic_class = env->GetObjectClass(intrinsic);
+    jfieldID width_field = env->GetFieldID(intrinsic_class, "mWidth", "I");
+    jfieldID height_field = env->GetFieldID(intrinsic_class, "mHeight", "I");
+    jfieldID ppx_field = env->GetFieldID(intrinsic_class, "mPpx", "F");
+    jfieldID ppy_field = env->GetFieldID(intrinsic_class, "mPpy", "F");
+    jfieldID fx_field = env->GetFieldID(intrinsic_class, "mFx", "F");
+    jfieldID fy_field = env->GetFieldID(intrinsic_class, "mFy", "F");
+    jfieldID model_field = env->GetFieldID(intrinsic_class, "mModelValue", "I");
+    jfieldID coeff_field = env->GetFieldID(intrinsic_class, "mCoeffs", "[F");
+    rs2_intrinsics intrinsics;
+    intrinsics.width = env->GetIntField(intrinsic, width_field);
+    intrinsics.height = env->GetIntField(intrinsic, height_field);
+    intrinsics.ppx = env->GetFloatField(intrinsic, ppx_field);
+    intrinsics.ppy = env->GetFloatField(intrinsic, ppy_field);
+    intrinsics.fx = env->GetFloatField(intrinsic, fx_field);
+    intrinsics.fy = env->GetFloatField(intrinsic, fy_field);
+    intrinsics.model = (rs2_distortion)env->GetIntField(intrinsic, model_field);
+
+    jobject coeffsObject = env->GetObjectField(intrinsic, coeff_field);
+    jfloatArray* coeffsArray = reinterpret_cast<jfloatArray *>(&coeffsObject);
+    jfloat * coeffs = env->GetFloatArrayElements(*coeffsArray, NULL);
+    memcpy(intrinsics.coeffs, coeffs, 5 * sizeof(float));
+    env->ReleaseFloatArrayElements(*coeffsArray, coeffs, 0);
+
+    // preparing struct to be filled by API function
+    float* point = new float[3];
+    point[0] = 0.f;
+    point[1] = 0.f;
+    point[2] = 0.f;
+
+    rs2_deproject_pixel_to_point(point, &intrinsics, pixel, depth);
+
+    // returning point into point_3D struct
+    jclass point_3D_class = env->GetObjectClass(point_3D);
+    jfieldID point_x_field = env->GetFieldID(point_3D_class, "mX", "F");
+    jfieldID point_y_field = env->GetFieldID(point_3D_class, "mY", "F");
+    jfieldID point_z_field = env->GetFieldID(point_3D_class, "mZ", "F");
+    env->SetFloatField(point_3D, point_x_field, point[0]);
+    env->SetFloatField(point_3D, point_y_field, point[1]);
+    env->SetFloatField(point_3D, point_z_field, point[2]);
+}
+
+extern "C" JNIEXPORT void JNICALL
+Java_com_intel_realsense_librealsense_Utils_nTransformPointToPoint(JNIEnv *env, jclass type,
+                                                                   jobject to_point_3D, jobject extrinsic,
+                                                                   jobject from_point_3D) {
+    // retrieving float[3] from from_point_3D
+    jclass from_point_3D_class = env->GetObjectClass(from_point_3D);
+    jfieldID from_point_x_field = env->GetFieldID(from_point_3D_class, "mX", "F");
+    jfieldID from_point_y_field = env->GetFieldID(from_point_3D_class, "mY", "F");
+    jfieldID from_point_z_field = env->GetFieldID(from_point_3D_class, "mZ", "F");
+    float* from_point = new float[3];
+    from_point[0] =  env->GetFloatField(from_point_3D, from_point_x_field);
+    from_point[1] =  env->GetFloatField(from_point_3D, from_point_y_field);
+    from_point[2] =  env->GetFloatField(from_point_3D, from_point_z_field);
+
+
+    // retrieving rs2_extrinsics from extrinsic object
+    rs2_extrinsics extrinsics;
+    jclass extrinsic_class = env->GetObjectClass(extrinsic);
+    //fill rotation
+    jfieldID rotation_field = env->GetFieldID(extrinsic_class, "mRotation", "[F");
+    jobject rotationObject = env->GetObjectField(extrinsic, rotation_field);
+    jfloatArray* rotationArray = reinterpret_cast<jfloatArray *>(&rotationObject);
+    jfloat * rotation = env->GetFloatArrayElements(*rotationArray, NULL);
+    memcpy(extrinsics.rotation, rotation, 9 * sizeof(float));
+    env->ReleaseFloatArrayElements(*rotationArray, rotation, 0);
+    //fill translation
+    jfieldID translation_field = env->GetFieldID(extrinsic_class, "mTranslation", "[F");
+    jobject translationObject = env->GetObjectField(extrinsic, translation_field);
+    jfloatArray* translationArray = reinterpret_cast<jfloatArray *>(&translationObject);
+    jfloat * translation = env->GetFloatArrayElements(*translationArray, NULL);
+    memcpy(extrinsics.translation, translation, 9 * sizeof(float));
+    env->ReleaseFloatArrayElements(*translationArray, translation, 0);
+
+
+    // preparing struct to be filled by API function
+    float* to_point = new float[3];
+    to_point[0] = 0.f;
+    to_point[1] = 0.f;
+    to_point[2] = 0.f;
+
+    //api call
+    rs2_transform_point_to_point(to_point, &extrinsics, from_point);
+
+    // returning point into point_3D struct
+    jclass to_point_3D_class = env->GetObjectClass(to_point_3D);
+    jfieldID to_point_x_field = env->GetFieldID(to_point_3D_class, "mX", "F");
+    jfieldID to_point_y_field = env->GetFieldID(to_point_3D_class, "mY", "F");
+    jfieldID to_point_z_field = env->GetFieldID(to_point_3D_class, "mZ", "F");
+    env->SetFloatField(to_point_3D, to_point_x_field, to_point[0]);
+    env->SetFloatField(to_point_3D, to_point_y_field, to_point[1]);
+    env->SetFloatField(to_point_3D, to_point_z_field, to_point[2]);
+}
+

wrappers/android/librealsense/src/main/java/com/intel/realsense/librealsense/Intrinsic.java

@@ -32,6 +32,11 @@ public Intrinsic(int width, int height,
         this.mCoeffs = coeffs;
     }
 
+    public int getWidth() {return mWidth;}
+    public int getHeight() {return mHeight;}
+    public DistortionType getModel() {return mModel;}
+
+
     public void SetModel(){
         this.mModel = DistortionType.values()[mModelValue];
     }

wrappers/android/librealsense/src/main/java/com/intel/realsense/librealsense/Pixel.java

@@ -0,0 +1,16 @@
+package com.intel.realsense.librealsense;
+
+public class Pixel {
+    public int mX;
+    public int mY;
+
+    public Pixel(){
+        mX = 0;
+        mY = 0;
+    }
+
+    public Pixel(int x, int y){
+        mX = x;
+        mY = y;
+    }
+}

wrappers/android/librealsense/src/main/java/com/intel/realsense/librealsense/Point_3D.java

@@ -0,0 +1,19 @@
+package com.intel.realsense.librealsense;
+
+public class Point_3D {
+    public float mX;
+    public float mY;
+    public float mZ;
+
+    public Point_3D() {
+        mX = 0.f;
+        mY = 0.f;
+        mZ = 0.f;
+    }
+
+    public Point_3D(float x, float y, float z) {
+        mX = x;
+        mY = y;
+        mZ = z;
+    }
+}

wrappers/android/librealsense/src/main/java/com/intel/realsense/librealsense/Utils.java

@@ -0,0 +1,33 @@
+package com.intel.realsense.librealsense;
+
+
+public class Utils {
+
+     // Given a point in 3D space, compute the corresponding pixel coordinates in an image
+    // with no distortion or forward distortion coefficients produced by the same camera
+    public static Pixel projectPointToPixel(final Intrinsic intrinsic, final Point_3D point) {
+        Pixel rv = new Pixel();
+        nProjectPointToPixel(rv, intrinsic, point);
+        return rv;
+    }
+
+    // Given pixel coordinates and depth in an image with no distortion or inverse distortion coefficients,
+    // compute the corresponding point in 3D space relative to the same camera
+    public static Point_3D deprojectPixelToPoint(final Intrinsic intrinsic, final Pixel pixel, final float depth) {
+        Point_3D rv = new Point_3D();
+        nDeprojectPixelToPoint(rv, intrinsic, pixel, depth);
+        return rv;
+    }
+
+    // Transform 3D coordinates relative to one sensor to 3D coordinates relative to another viewpoint
+    public static Point_3D transformPointToPoint(final Extrinsic extrinsic, final Point_3D from_point) {
+        Point_3D rv = new Point_3D();
+        nTransformPointToPoint(rv, extrinsic, from_point);
+        return rv;
+    }
+
+
+    private native static void nProjectPointToPixel(Pixel pixel, Intrinsic intrinsic, Point_3D point_3D);
+    private native static void nDeprojectPixelToPoint(Point_3D point_3D, Intrinsic intrinsic, Pixel pixel, float depth);
+    private native static void nTransformPointToPoint(Point_3D to_point_3D, Extrinsic extrinsic, Point_3D from_point_3D);
+}