diff --git a/android/.gitignore b/android/.gitignore index fb05626..ef1e81c 100644 --- a/android/.gitignore +++ b/android/.gitignore @@ -7,3 +7,7 @@ /captures .externalNativeBuild .cxx + +# Local AAR drop-ins (e.g. INMO air3_core) — keep the README, ignore the binaries +/app/libs/*.aar +!/app/libs/README.md diff --git a/android/app/build.gradle.kts b/android/app/build.gradle.kts index 560e630..43c621f 100644 --- a/android/app/build.gradle.kts +++ b/android/app/build.gradle.kts @@ -13,6 +13,10 @@ android { targetSdk = 34 versionCode = 1 versionName = "0.1.0" + + // OpenCV native is large; restrict to arm64 (modern phones, incl. the Pixel 9a). + // Add "armeabi-v7a" here if you need to run on an older 32-bit device. + ndk { abiFilters += "arm64-v8a" } } buildTypes { @@ -40,6 +44,12 @@ android { } dependencies { + // INMO Air3 native fusion (decision 011). Local .aar dropped into app/libs/. + // Absent by default — the app builds and falls back to Game Rotation Vector until + // the .aar is present. See app/libs/README.md. fileTree is used so this bypasses + // the FAIL_ON_PROJECT_REPOS restriction in settings.gradle. + implementation(fileTree(mapOf("dir" to "libs", "include" to listOf("*.aar")))) + // Compose val composeBom = platform("androidx.compose:compose-bom:2024.01.00") implementation(composeBom) @@ -60,6 +70,9 @@ dependencies { // ARCore implementation("com.google.ar:core:1.41.0") + // OpenCV (objdetect/ArucoDetector) for fiducial alignment to the hub frame. + implementation("org.opencv:opencv:4.11.0") + // Core implementation("androidx.core:core-ktx:1.12.0") implementation("androidx.lifecycle:lifecycle-runtime-ktx:2.7.0") diff --git a/android/app/libs/README.md b/android/app/libs/README.md new file mode 100644 index 0000000..df89139 --- /dev/null +++ b/android/app/libs/README.md @@ -0,0 +1,39 @@ +# Local AAR drop-in — INMO Air3 native fusion + +This directory holds local `.aar` dependencies that are **not** committed to git +(see `.gitignore`). The build picks up any `*.aar` here automatically via the +`fileTree(... "libs" ...)` dependency in `app/build.gradle.kts`. + +## What to drop here + +`air3_core-debug.aar` (or `air3_core-release.aar`) from the INMO Air3 SDK. This is +the library that provides `com.inmo.air3_core.atw.GyroRotation`, whose `vQuat` field +is the pre-calibrated head-orientation quaternion we consume in +[`InmoFusionTracker.kt`](../src/main/java/com/pri4l/hub/InmoFusionTracker.kt). + +### How to obtain it + +1. INMO Air3 Unity SDK: + The `air3_core` AAR ships inside the Unity package + (`Assets/Plugins/Android/` or the `.unitypackage`). Extract it. +2. Alternatively pull it off-device from an installed INMO app: + `adb shell pm path ` → `adb pull` the APK → unzip and look for the + bundled native libs / classes. (The AAR repackages `libinmoair3.so` + the + `com.inmo.air3_core.*` classes.) +3. Community wiki for pointers: + +## After dropping it in + +- Rebuild. `InmoFusionTracker.isAvailable()` flips to `true` and + `HeadTrackerFactory` selects the INMO path automatically. +- Launch `GlassesTestActivity` (standalone cubes) and check logcat for: + `head tracker source: INMO GyroRotation.vQuat (native fusion)`. +- Verify on-device and adjust the assumptions flagged in `InmoFusionTracker.kt`: + - vQuat component order ([x,y,z,w] vs [w,x,y,z]) — `copyQuat()` + - field type (float[] vs Quaternion object) — `readQuat()` + - lifecycle (getInstance / constructor / start signature) — `tryGetInstance` / `tryInvokeStart` + - eye-axis handedness — `axisFix` + +If the glasses still spin wrong after the quaternion reads correctly, fix `axisFix` +(a constant eye-frame rotation) rather than re-introducing `remapCoordinateSystem` +on the raw sensor path — that path cannot represent the mounting offset (decision 011). diff --git a/android/app/src/main/AndroidManifest.xml b/android/app/src/main/AndroidManifest.xml index cd12fd0..943ecaa 100644 --- a/android/app/src/main/AndroidManifest.xml +++ b/android/app/src/main/AndroidManifest.xml @@ -1,5 +1,14 @@ - + + + + diff --git a/android/app/src/main/java/com/pri4l/hub/FiducialAligner.kt b/android/app/src/main/java/com/pri4l/hub/FiducialAligner.kt new file mode 100644 index 0000000..68b5fcf --- /dev/null +++ b/android/app/src/main/java/com/pri4l/hub/FiducialAligner.kt @@ -0,0 +1,173 @@ +package com.pri4l.hub + +import android.media.Image +import android.opengl.Matrix +import android.util.Log +import com.google.ar.core.Frame +import com.google.ar.core.TrackingState +import com.google.ar.core.exceptions.NotYetAvailableException +import org.opencv.calib3d.Calib3d +import org.opencv.core.CvType +import org.opencv.core.Mat +import org.opencv.core.MatOfDouble +import org.opencv.core.MatOfPoint2f +import org.opencv.core.MatOfPoint3f +import org.opencv.core.Point +import org.opencv.core.Point3 +import org.opencv.objdetect.ArucoDetector +import org.opencv.objdetect.Objdetect + +/** + * Detects an ArUco marker (DICT_4X4_50, id [TARGET_ID]) in the ARCore camera image and returns + * the marker's pose in the ARCore world frame. Used to align ARCore to the hub map frame: the + * tag is placed at the hub origin, so feeding the tag's AR-world pose to + * [FrameAlignment.align] (with hub pose = origin) links the two frames automatically — no need + * to physically stand the phone at the D435 (decision 009's manual flow). + * + * Coordinate handling: solvePnP yields the tag pose in the OpenCV camera frame (x-right, y-down, + * z-forward). ARCore's camera frame is x-right, y-up, z-backward, so we flip Y and Z + * (C = diag(1,-1,-1)) before composing with the ARCore camera pose. + */ +class FiducialAligner { + + private val detector = ArucoDetector(Objdetect.getPredefinedDictionary(Objdetect.DICT_4X4_50)) + + private val camMatrix = Mat(3, 3, CvType.CV_64F) + private val distCoeffs = MatOfDouble(0.0, 0.0, 0.0, 0.0, 0.0) + private val objPoints = MatOfPoint3f() + private val rvec = Mat() + private val tvec = Mat() + private val rMat = Mat() + + private val tArcamTag = FloatArray(16) + private val camPose = FloatArray(16) + private val tagWorld = FloatArray(16) + + @Volatile var lastRangeM = 0f + private set + + /** + * @return the tag pose in ARCore world as ([x,y,z], [qx,qy,qz,qw]), or null if no marker / + * not tracking / image unavailable. + */ + fun detectTagInArWorld(frame: Frame): Pair? { + val cam = frame.camera + if (cam.trackingState != TrackingState.TRACKING) return null + + val image: Image = try { + frame.acquireCameraImage() + } catch (e: NotYetAvailableException) { + return null + } + try { + val gray = grayFromY(image) ?: return null + + val corners = ArrayList() + val ids = Mat() + try { + detector.detectMarkers(gray, corners, ids) + if (ids.empty()) return null + + var idx = -1 + for (i in 0 until ids.rows()) { + if (ids.get(i, 0)[0].toInt() == TARGET_ID) { idx = i; break } + } + if (idx < 0) return null + + // Intrinsics from ARCore, matching the sensor-native acquired image. + val intr = cam.imageIntrinsics + val fl = intr.focalLength + val pp = intr.principalPoint + camMatrix.put(0, 0, fl[0].toDouble(), 0.0, pp[0].toDouble(), + 0.0, fl[1].toDouble(), pp[1].toDouble(), + 0.0, 0.0, 1.0) + + val L = MARKER_SIZE_M.toDouble() + objPoints.fromArray( + Point3(-L / 2, L / 2, 0.0), Point3(L / 2, L / 2, 0.0), + Point3(L / 2, -L / 2, 0.0), Point3(-L / 2, -L / 2, 0.0) + ) + val img = MatOfPoint2f() + val pts = ArrayList(4) + val c = corners[idx] + for (k in 0 until 4) { val v = c.get(0, k); pts.add(Point(v[0], v[1])) } + img.fromList(pts) + + val ok = Calib3d.solvePnP(objPoints, img, camMatrix, distCoeffs, rvec, tvec) + img.release() + if (!ok) return null + + Calib3d.Rodrigues(rvec, rMat) + val r = DoubleArray(9); rMat.get(0, 0, r) // row-major tag->cv-cam + val t = DoubleArray(3); tvec.get(0, 0, t) + lastRangeM = Math.sqrt(t[0] * t[0] + t[1] * t[1] + t[2] * t[2]).toFloat() + + // C = diag(1,-1,-1): OpenCV cam -> ARCore cam (negate rows 1,2 of R and t1,t2). + // T_arcam_tag column-major. + tArcamTag[0] = r[0].toFloat(); tArcamTag[1] = (-r[3]).toFloat(); tArcamTag[2] = (-r[6]).toFloat(); tArcamTag[3] = 0f + tArcamTag[4] = r[1].toFloat(); tArcamTag[5] = (-r[4]).toFloat(); tArcamTag[6] = (-r[7]).toFloat(); tArcamTag[7] = 0f + tArcamTag[8] = r[2].toFloat(); tArcamTag[9] = (-r[5]).toFloat(); tArcamTag[10] = (-r[8]).toFloat(); tArcamTag[11] = 0f + tArcamTag[12] = t[0].toFloat(); tArcamTag[13] = (-t[1]).toFloat(); tArcamTag[14] = (-t[2]).toFloat(); tArcamTag[15] = 1f + + // tagWorld = cameraPose(world<-arcam) * T_arcam_tag + cam.pose.toMatrix(camPose, 0) + Matrix.multiplyMM(tagWorld, 0, camPose, 0, tArcamTag, 0) + + val pos = floatArrayOf(tagWorld[12], tagWorld[13], tagWorld[14]) + val rot = matrixToQuaternion(tagWorld) + Log.w("Pri4L", "FIDUCIAL: range=%.2fm tagWorld=[%.2f, %.2f, %.2f]" + .format(lastRangeM, pos[0], pos[1], pos[2])) + return Pair(pos, rot) + } finally { + gray.release(); ids.release(); corners.forEach { it.release() } + } + } catch (e: Throwable) { + Log.e("Pri4L", "fiducial detect failed", e) + return null + } finally { + image.close() + } + } + + private fun grayFromY(image: Image): Mat? { + if (image.planes.isEmpty()) return null + val y = image.planes[0] + val buf = y.buffer + val w = image.width + val h = image.height + val gray = Mat(h, w, CvType.CV_8UC1) + val stride = y.rowStride + if (stride == w) { + val data = ByteArray(w * h); buf.get(data); gray.put(0, 0, data) + } else { + val row = ByteArray(w) + for (r in 0 until h) { buf.position(r * stride); buf.get(row, 0, w); gray.put(r, 0, row) } + } + return gray + } + + private fun matrixToQuaternion(m: FloatArray): FloatArray { + val trace = m[0] + m[5] + m[10] + val q = FloatArray(4) + if (trace > 0) { + val s = 0.5f / Math.sqrt((trace + 1.0).toDouble()).toFloat() + q[3] = 0.25f / s; q[0] = (m[6] - m[9]) * s; q[1] = (m[8] - m[2]) * s; q[2] = (m[1] - m[4]) * s + } else if (m[0] > m[5] && m[0] > m[10]) { + val s = 2f * Math.sqrt((1.0 + m[0] - m[5] - m[10]).toDouble()).toFloat() + q[3] = (m[6] - m[9]) / s; q[0] = 0.25f * s; q[1] = (m[4] + m[1]) / s; q[2] = (m[8] + m[2]) / s + } else if (m[5] > m[10]) { + val s = 2f * Math.sqrt((1.0 + m[5] - m[0] - m[10]).toDouble()).toFloat() + q[3] = (m[8] - m[2]) / s; q[0] = (m[4] + m[1]) / s; q[1] = 0.25f * s; q[2] = (m[9] + m[6]) / s + } else { + val s = 2f * Math.sqrt((1.0 + m[10] - m[0] - m[5]).toDouble()).toFloat() + q[3] = (m[1] - m[4]) / s; q[0] = (m[8] + m[2]) / s; q[1] = (m[9] + m[6]) / s; q[2] = 0.25f * s + } + return q + } + + companion object { + private const val TARGET_ID = 0 + /** Printed black-square width in metres. MUST match the physical print. */ + const val MARKER_SIZE_M = 0.15f + } +} diff --git a/android/app/src/main/java/com/pri4l/hub/GlassesRenderer.kt b/android/app/src/main/java/com/pri4l/hub/GlassesRenderer.kt index 3e24df8..788c3dc 100644 --- a/android/app/src/main/java/com/pri4l/hub/GlassesRenderer.kt +++ b/android/app/src/main/java/com/pri4l/hub/GlassesRenderer.kt @@ -12,7 +12,7 @@ import javax.microedition.khronos.opengles.GL10 * No camera background — renders on transparent/black background. */ class GlassesRenderer( - private val tracker: GlassesTracker, + private val tracker: HeadTracker, private val getHubAnchors: () -> List, private val getPhoneAnchors: () -> List, private val getAlignmentMatrix: () -> FloatArray? diff --git a/android/app/src/main/java/com/pri4l/hub/GlassesTestActivity.kt b/android/app/src/main/java/com/pri4l/hub/GlassesTestActivity.kt index 1b99fa0..9e34955 100644 --- a/android/app/src/main/java/com/pri4l/hub/GlassesTestActivity.kt +++ b/android/app/src/main/java/com/pri4l/hub/GlassesTestActivity.kt @@ -5,6 +5,7 @@ import android.opengl.GLES20 import android.opengl.GLSurfaceView import android.opengl.Matrix import android.os.Bundle +import android.view.KeyEvent import android.view.WindowManager import javax.microedition.khronos.egl.EGLConfig import javax.microedition.khronos.opengles.GL10 @@ -16,7 +17,7 @@ import javax.microedition.khronos.opengles.GL10 class GlassesTestActivity : Activity() { private lateinit var glView: GLSurfaceView - private lateinit var tracker: GlassesTracker + private lateinit var tracker: HeadTracker override fun onCreate(savedInstanceState: Bundle?) { super.onCreate(savedInstanceState) @@ -25,8 +26,10 @@ class GlassesTestActivity : Activity() { window.addFlags(WindowManager.LayoutParams.FLAG_FULLSCREEN) window.addFlags(WindowManager.LayoutParams.FLAG_KEEP_SCREEN_ON) - tracker = GlassesTracker(this) + // Picks INMO native fusion if the air3_core AAR is present, else Game Rotation Vector. + tracker = HeadTrackerFactory.create(this) tracker.start() + android.util.Log.w("Pri4L", "GlassesTest head tracker: ${tracker.sourceName}") glView = GLSurfaceView(this).apply { setEGLContextClientVersion(2) @@ -48,9 +51,18 @@ class GlassesTestActivity : Activity() { glView.onPause() tracker.stop() } + + // Look straight ahead, then tap the temple touchpad (any key) to redefine "forward". + // The auto-recenter on first frame can capture a stale/pitched reference, leaving + // content offset (e.g. cubes sitting below the gaze); this lets the wearer fix it. + override fun onKeyDown(keyCode: Int, event: KeyEvent?): Boolean { + tracker.recenter() + android.util.Log.w("Pri4L", "recenter requested (keyCode=$keyCode)") + return true + } } -class HeadTrackedCubeRenderer(private val tracker: GlassesTracker) : GLSurfaceView.Renderer { +class HeadTrackedCubeRenderer(private val tracker: HeadTracker) : GLSurfaceView.Renderer { private var cubeRenderer = CubeRenderer() private val mvpMatrix = FloatArray(16) diff --git a/android/app/src/main/java/com/pri4l/hub/GlassesTracker.kt b/android/app/src/main/java/com/pri4l/hub/GlassesTracker.kt index 22b5afc..298edba 100644 --- a/android/app/src/main/java/com/pri4l/hub/GlassesTracker.kt +++ b/android/app/src/main/java/com/pri4l/hub/GlassesTracker.kt @@ -12,7 +12,9 @@ import android.opengl.Matrix * Provides a view matrix suitable for rendering anchors relative to head orientation. * No positional tracking — anchors will rotate correctly but no parallax. */ -class GlassesTracker(context: Context) : SensorEventListener { +class GlassesTracker(context: Context) : SensorEventListener, HeadTracker { + + override val sourceName: String = "Android Game Rotation Vector (WIP/broken axes)" private val sensorManager = context.getSystemService(Context.SENSOR_SERVICE) as SensorManager private val rotationSensor = sensorManager.getDefaultSensor(Sensor.TYPE_GAME_ROTATION_VECTOR) @@ -21,7 +23,7 @@ class GlassesTracker(context: Context) : SensorEventListener { private val viewMatrix = FloatArray(16) @Volatile - var isTracking = false + override var isTracking = false private set init { @@ -29,26 +31,29 @@ class GlassesTracker(context: Context) : SensorEventListener { Matrix.setIdentityM(viewMatrix, 0) } - fun start() { + override fun start() { if (rotationSensor != null) { sensorManager.registerListener(this, rotationSensor, SensorManager.SENSOR_DELAY_GAME) isTracking = true } } - fun stop() { + override fun stop() { sensorManager.unregisterListener(this) isTracking = false } private val remappedMatrix = FloatArray(16) - fun getViewMatrix(dest: FloatArray) { + override fun getViewMatrix(dest: FloatArray) { synchronized(rotationMatrix) { - // Glasses are landscape with screen facing outward. - // Use AXIS_Y, AXIS_MINUS_X: equivalent to 90° CW rotation of device, - // which maps landscape-native sensor data to GL correctly. - // head yaw → GL yaw, head pitch → GL pitch, head roll → GL roll + // WIP / KNOWN-BROKEN: head yaw currently shows up as roll. See decision 011. + // remapCoordinateSystem can only express axis-aligned 90° permutations and + // cannot represent the real (non-axis-aligned) IMU-to-eye mounting offset of + // the INMO chassis, so NO axis pair fixes all three of yaw/pitch/roll. + // Do not keep permuting these axes — replace this path with INMO's calibrated + // fusion quaternion (GyroRotation.vQuat) or an empirically-solved correction + // quaternion C applied as view = C · Rᵀ. This call is a placeholder. SensorManager.remapCoordinateSystem( rotationMatrix, SensorManager.AXIS_Y, diff --git a/android/app/src/main/java/com/pri4l/hub/HeadTracker.kt b/android/app/src/main/java/com/pri4l/hub/HeadTracker.kt new file mode 100644 index 0000000..159aa73 --- /dev/null +++ b/android/app/src/main/java/com/pri4l/hub/HeadTracker.kt @@ -0,0 +1,38 @@ +package com.pri4l.hub + +/** + * Common interface for 3DoF head-orientation sources. + * + * Two implementations exist: + * - [GlassesTracker] — Android Game Rotation Vector. KNOWN-BROKEN axis mapping + * on the INMO (see decision 011); kept as fallback. + * - [InmoFusionTracker] — INMO's native sensor fusion (`GyroRotation.vQuat`), which + * bakes in the true IMU-to-eye mounting offset. Preferred. + * + * Use [HeadTrackerFactory.create] to obtain the best available source. + */ +interface HeadTracker { + /** True once a valid orientation stream is being received. */ + val isTracking: Boolean + + /** Begin listening to the underlying sensor/fusion source. */ + fun start() + + /** Stop listening and release resources. */ + fun stop() + + /** + * Writes a column-major 4x4 GL view matrix (world -> eye) into [dest]. + * Caller must pass a FloatArray of length 16. + */ + fun getViewMatrix(dest: FloatArray) + + /** + * Re-defines "forward" as the current head orientation. Anchors placed straight + * ahead will appear centered after this call. Optional; no-op if unsupported. + */ + fun recenter() {} + + /** Short human-readable name of the active source, for logging/diagnostics. */ + val sourceName: String +} diff --git a/android/app/src/main/java/com/pri4l/hub/HeadTrackerFactory.kt b/android/app/src/main/java/com/pri4l/hub/HeadTrackerFactory.kt new file mode 100644 index 0000000..996123c --- /dev/null +++ b/android/app/src/main/java/com/pri4l/hub/HeadTrackerFactory.kt @@ -0,0 +1,32 @@ +package com.pri4l.hub + +import android.app.Activity +import android.content.Context +import android.util.Log + +/** + * Selects the best available [HeadTracker]. + * + * Preference order: + * 1. [InmoFusionTracker] — INMO native fusion (correct axes; see decision 011). + * 2. [GlassesTracker] — Android Game Rotation Vector (WIP/broken axes) fallback. + * + * The INMO path only wins once the `air3_core` AAR is present (see + * `android/app/libs/README.md`); otherwise we transparently fall back. + */ +object HeadTrackerFactory { + private const val TAG = "Pri4L" + + fun create(activity: Activity): HeadTracker { + if (InmoFusionTracker.isAvailable()) { + Log.w(TAG, "HeadTracker: using INMO native fusion") + return InmoFusionTracker(activity) + } + Log.w(TAG, "HeadTracker: INMO AAR absent, falling back to Game Rotation Vector") + return GlassesTracker(activity) + } + + /** True if any head-tracking source is usable on this device. */ + fun isAvailable(context: Context): Boolean = + InmoFusionTracker.isAvailable() || GlassesTracker.isAvailable(context) +} diff --git a/android/app/src/main/java/com/pri4l/hub/InmoFusionTracker.kt b/android/app/src/main/java/com/pri4l/hub/InmoFusionTracker.kt new file mode 100644 index 0000000..01b41aa --- /dev/null +++ b/android/app/src/main/java/com/pri4l/hub/InmoFusionTracker.kt @@ -0,0 +1,239 @@ +package com.pri4l.hub + +import android.app.Activity +import android.content.Context +import android.opengl.Matrix +import android.util.Log +import com.unity3d.player.UnityPlayer + +/** + * Head tracker backed by INMO's native sensor fusion (`com.inmo.air3_core.atw.GyroRotation`). + * + * WHY REFLECTION: the `air3_core` AAR is integrated as a local file dependency (see + * `android/app/libs/README.md`). Until that .aar is actually dropped in, the INMO classes + * are absent at compile time, so we bind to them reflectively. This keeps the project + * building today and lets the INMO path activate automatically once the AAR is present. + * Once the API is confirmed on-device, this can be swapped for direct typed calls. + * + * The vendor quaternion (`GyroRotation.vQuat`) already accounts for the glasses' physical + * IMU mounting — that is exactly the offset `remapCoordinateSystem` could not represent + * (see decision 011). So this path should not exhibit the "yaw -> roll" bug. + * + * UNVERIFIED ON HARDWARE (confirm and adjust when the AAR lands): + * - vQuat component order (assumed [x, y, z, w]; INMO may use [w, x, y, z]). + * - vQuat field type (assumed float[4]; may be a Quaternion object). + * - GyroRotation lifecycle (getInstance vs constructor; start() arg shape). + * - eye-axis handedness vs GL (see [axisFix]). + * + * REQUIRES AN ACTIVITY: `GyroRotation.start()` reads `UnityPlayer.currentActivity` + * (not any passed-in Context) to get the Activity it calls `getSystemService("sensor")` + * on — the AAR is compiled against Unity. We set that field from [activity] in [start] + * via our local [UnityPlayer] stub (decision 011). Without it, `start()` throws + * NoClassDefFoundError / leaves the SensorManager null and no `vQuat` ever arrives. + */ +class InmoFusionTracker(private val activity: Activity) : HeadTracker { + + override val sourceName: String = "INMO GyroRotation.vQuat (native fusion)" + + @Volatile + override var isTracking = false + private set + + // Reflection handles, resolved in start(). + private var gyroInstance: Any? = null + private var quatField: java.lang.reflect.Field? = null + + private val quat = FloatArray(4) // working [x, y, z, w] + private val rot = FloatArray(16) // head -> world + private val refRot = FloatArray(16) // head -> world at recenter + private val tmp = FloatArray(16) + @Volatile private var haveRef = false + @Volatile private var recenterRequested = true // auto-recenter on first frame + + /** + * Eye-frame correction applied as `view = axisFix * viewRaw`. Identity by default. + * If, on hardware, forward/up/right come out swapped or mirrored even after the + * quaternion is read correctly, set this to the appropriate constant 90deg rotation + * instead of touching the quaternion math. Recentering cancels world-frame offsets + * but NOT a body/eye-axis mismatch — that is what this matrix is for. + */ + private val axisFix = FloatArray(16).also { Matrix.setIdentityM(it, 0) } + + override fun start() { + try { + // The AAR's GyroRotation.start() reads UnityPlayer.currentActivity (not the + // Context we hold) to obtain the Activity for getSystemService("sensor"). + // Populate our stub before invoking it, or no sensors register. + UnityPlayer.currentActivity = activity + + // The native fusion symbol (AtwCore.nativeGyroFusion) lives in libinmoair3.so, + // normally loaded by Air3Core's static initializer during Unity/Air3Core init — + // which we bypass. Load it ourselves, or the first sensor callback aborts with + // UnsatisfiedLinkError. Idempotent; transitively pulls in the bundled deps. + try { + System.loadLibrary(NATIVE_LIB) + } catch (e: UnsatisfiedLinkError) { + Log.e(TAG, "Failed to load lib$NATIVE_LIB.so; INMO native fusion unavailable", e) + isTracking = false + return + } + + val cls = Class.forName(GYRO_CLASS) + + // Lifecycle: try singleton accessor, then Context constructor, then no-arg. + gyroInstance = tryGetInstance(cls) + + // Locate the vQuat field (instance or static). + quatField = cls.declaredFields.firstOrNull { it.name == "vQuat" } + ?.apply { isAccessible = true } + ?: cls.fields.firstOrNull { it.name == "vQuat" }?.apply { isAccessible = true } + + // Kick the fusion loop. Tolerate either start() or start(Context). + tryInvokeStart(cls, gyroInstance) + + if (quatField == null) { + Log.e(TAG, "GyroRotation found but no vQuat field; cannot track") + isTracking = false + return + } + isTracking = true + Log.w(TAG, "INMO fusion tracker started ($sourceName)") + } catch (e: ClassNotFoundException) { + Log.w(TAG, "INMO air3_core AAR not present; INMO fusion unavailable") + isTracking = false + } catch (e: Throwable) { + Log.e(TAG, "Failed to start INMO fusion tracker", e) + isTracking = false + } + } + + override fun stop() { + isTracking = false + val inst = gyroInstance ?: return + try { + inst.javaClass.methods.firstOrNull { it.name == "stop" && it.parameterTypes.isEmpty() } + ?.invoke(inst) + } catch (e: Throwable) { + Log.w(TAG, "stop() on GyroRotation failed (non-fatal)", e) + } + } + + override fun recenter() { + recenterRequested = true + } + + override fun getViewMatrix(dest: FloatArray) { + if (!isTracking || !readQuat()) { + Matrix.setIdentityM(dest, 0) + return + } + + quatToMatrix(quat, rot) // rot = head -> world + + if (recenterRequested || !haveRef) { + System.arraycopy(rot, 0, refRot, 0, 16) + haveRef = true + recenterRequested = false + } + + // view = axisFix * (rot^T * refRot) — relative to the recenter orientation. + Matrix.transposeM(tmp, 0, rot, 0) + Matrix.multiplyMM(dest, 0, tmp, 0, refRot, 0) + Matrix.multiplyMM(tmp, 0, axisFix, 0, dest, 0) + System.arraycopy(tmp, 0, dest, 0, 16) + } + + // --- reflection helpers -------------------------------------------------- + + private fun tryGetInstance(cls: Class<*>): Any? { + // 1) static getInstance() + cls.methods.firstOrNull { it.name == "getInstance" && it.parameterTypes.isEmpty() } + ?.let { return it.invoke(null) } + // 2) constructor(Context) + runCatching { cls.getConstructor(Context::class.java).newInstance(activity) } + .getOrNull()?.let { return it } + // 3) no-arg constructor + runCatching { cls.getConstructor().newInstance() }.getOrNull()?.let { return it } + // 4) fully static API — no instance needed + return null + } + + private fun tryInvokeStart(cls: Class<*>, instance: Any?) { + cls.methods.firstOrNull { it.name == "start" && it.parameterTypes.size == 1 && + it.parameterTypes[0] == Context::class.java } + ?.let { it.invoke(instance, activity); return } + cls.methods.firstOrNull { it.name == "start" && it.parameterTypes.isEmpty() } + ?.let { it.invoke(instance); return } + Log.w(TAG, "No start() method on GyroRotation; assuming auto-started") + } + + /** Reads vQuat into [quat] as [x,y,z,w]. Returns false if unreadable. */ + private fun readQuat(): Boolean { + val f = quatField ?: return false + return try { + when (val v = f.get(gyroInstance)) { + is FloatArray -> if (v.size >= 4) { copyQuat(v[0], v[1], v[2], v[3]); true } else false + is DoubleArray -> if (v.size >= 4) { + copyQuat(v[0].toFloat(), v[1].toFloat(), v[2].toFloat(), v[3].toFloat()); true + } else false + null -> false + else -> readQuatObject(v) // custom Quaternion type + } + } catch (e: Throwable) { + false + } + } + + /** Best-effort read of an object exposing x/y/z/w as fields or getters. */ + private fun readQuatObject(obj: Any): Boolean { + fun comp(name: String): Float? { + obj.javaClass.fields.firstOrNull { it.name == name }?.let { + return (it.get(obj) as? Number)?.toFloat() + } + val getter = "get" + name.uppercase() + obj.javaClass.methods.firstOrNull { it.name == getter && it.parameterTypes.isEmpty() } + ?.let { return (it.invoke(obj) as? Number)?.toFloat() } + return null + } + val x = comp("x") ?: return false + val y = comp("y") ?: return false + val z = comp("z") ?: return false + val w = comp("w") ?: return false + copyQuat(x, y, z, w) + return true + } + + /** ASSUMPTION: vendor order is [x,y,z,w]. If glasses spin wrong, try [w,x,y,z] here. */ + private fun copyQuat(a: Float, b: Float, c: Float, d: Float) { + quat[0] = a; quat[1] = b; quat[2] = c; quat[3] = d + } + + /** Column-major head->world rotation matrix from a unit quaternion [x,y,z,w]. */ + private fun quatToMatrix(q: FloatArray, m: FloatArray) { + val x = q[0]; val y = q[1]; val z = q[2]; val w = q[3] + val n = x * x + y * y + z * z + w * w + val s = if (n > 0f) 2f / n else 0f + val xs = x * s; val ys = y * s; val zs = z * s + val wx = w * xs; val wy = w * ys; val wz = w * zs + val xx = x * xs; val xy = x * ys; val xz = x * zs + val yy = y * ys; val yz = y * zs; val zz = z * zs + + m[0] = 1f - (yy + zz); m[1] = xy + wz; m[2] = xz - wy; m[3] = 0f + m[4] = xy - wz; m[5] = 1f - (xx + zz); m[6] = yz + wx; m[7] = 0f + m[8] = xz + wy; m[9] = yz - wx; m[10] = 1f - (xx + yy); m[11] = 0f + m[12] = 0f; m[13] = 0f; m[14] = 0f; m[15] = 1f + } + + companion object { + private const val TAG = "Pri4L" + private const val GYRO_CLASS = "com.inmo.air3_core.atw.GyroRotation" + private const val NATIVE_LIB = "inmoair3" // libinmoair3.so — provides nativeGyroFusion + + /** True if the INMO fusion classes are on the classpath (AAR present). */ + fun isAvailable(): Boolean = try { + Class.forName(GYRO_CLASS); true + } catch (e: Throwable) { + false + } + } +} diff --git a/android/app/src/main/java/com/pri4l/hub/MainActivity.kt b/android/app/src/main/java/com/pri4l/hub/MainActivity.kt index 0282715..6776e92 100644 --- a/android/app/src/main/java/com/pri4l/hub/MainActivity.kt +++ b/android/app/src/main/java/com/pri4l/hub/MainActivity.kt @@ -27,6 +27,7 @@ class MainActivity : ComponentActivity() { private val frameAlignment = FrameAlignment() private var arCameraBridge: ArCameraBridge? = null + private var fiducialAligner: FiducialAligner? = null private var arSession: Session? = null private val glSurfaceViewState = mutableStateOf(null) @@ -40,7 +41,7 @@ class MainActivity : ComponentActivity() { private val aligned = mutableStateOf(false) private val pose = mutableStateOf(PoseData()) - private var glassesTracker: GlassesTracker? = null + private var glassesTracker: HeadTracker? = null // Flag set by UI thread, consumed by GL thread @Volatile private var alignRequested = false @@ -72,6 +73,14 @@ class MainActivity : ComponentActivity() { imuSender = ImuSender(this, rosbridge) checkArAvailability() + // Fiducial alignment: detect an ArUco tag at the hub origin to align ARCore <-> hub. + if (org.opencv.android.OpenCVLoader.initLocal()) { + fiducialAligner = FiducialAligner() + android.util.Log.w("Pri4L", "OpenCV loaded; fiducial aligner ready (point at the tag, then Align)") + } else { + android.util.Log.e("Pri4L", "OpenCV failed to load; fiducial align unavailable (manual align still works)") + } + setContent { MaterialTheme { Surface { @@ -119,7 +128,7 @@ class MainActivity : ComponentActivity() { arAvailable.value = arSupported // If no ARCore, check if we have sensors for glasses mode if (!arAvailable.value) { - glassesAvailable.value = GlassesTracker.isAvailable(this) + glassesAvailable.value = HeadTrackerFactory.isAvailable(this) } android.util.Log.w("Pri4L", "arAvailable=$arSupported glassesAvailable=${glassesAvailable.value}") } @@ -277,19 +286,29 @@ class MainActivity : ComponentActivity() { if (alignRequested) { alignRequested = false if (cam.trackingState == TrackingState.TRACKING) { - val arPose = cam.pose - val arPos = floatArrayOf(arPose.tx(), arPose.ty(), arPose.tz()) - val arRot = floatArrayOf(arPose.qx(), arPose.qy(), arPose.qz(), arPose.qw()) - - // D435 is at RTAB-Map origin + // Tag at hub origin (D435), identity orientation in the hub frame. val hubPos = floatArrayOf(0f, 0f, 0f) val hubRot = floatArrayOf(0f, 0f, 0f, 1f) - frameAlignment.align(hubPos, hubRot, arPos, arRot) - runOnUiThread { aligned.value = true } - - android.util.Log.w("Pri4L", - "ALIGNED: arPos=[%.3f, %.3f, %.3f]".format(arPos[0], arPos[1], arPos[2])) + // Prefer fiducial alignment: detect the ArUco tag and use its AR-world pose. + // Falls back to phone-at-origin (decision 009) if no tag is in view. + val tag = fiducialAligner?.detectTagInArWorld(frame) + if (tag != null) { + frameAlignment.align(hubPos, hubRot, tag.first, tag.second) + runOnUiThread { aligned.value = true } + android.util.Log.w("Pri4L", + "ALIGNED via FIDUCIAL: tagWorld=[%.3f, %.3f, %.3f]" + .format(tag.first[0], tag.first[1], tag.first[2])) + } else { + val arPose = cam.pose + val arPos = floatArrayOf(arPose.tx(), arPose.ty(), arPose.tz()) + val arRot = floatArrayOf(arPose.qx(), arPose.qy(), arPose.qz(), arPose.qw()) + frameAlignment.align(hubPos, hubRot, arPos, arRot) + runOnUiThread { aligned.value = true } + android.util.Log.w("Pri4L", + "ALIGNED via phone-at-origin (no tag in view): arPos=[%.3f, %.3f, %.3f]" + .format(arPos[0], arPos[1], arPos[2])) + } } } @@ -332,9 +351,10 @@ class MainActivity : ComponentActivity() { private fun startGlasses() { android.util.Log.w("Pri4L", "startGlasses() called") - val tracker = GlassesTracker(this) + val tracker = HeadTrackerFactory.create(this) tracker.start() glassesTracker = tracker + android.util.Log.w("Pri4L", "head tracker source: ${tracker.sourceName}") val renderer = GlassesRenderer( tracker = tracker, diff --git a/android/app/src/main/java/com/unity3d/player/UnityPlayer.java b/android/app/src/main/java/com/unity3d/player/UnityPlayer.java new file mode 100644 index 0000000..9688702 --- /dev/null +++ b/android/app/src/main/java/com/unity3d/player/UnityPlayer.java @@ -0,0 +1,23 @@ +package com.unity3d.player; + +import android.app.Activity; + +/** + * Minimal stand-in for Unity's UnityPlayer. + * + * The INMO {@code air3_core} AAR is compiled against the Unity runtime: + * {@code GyroRotation.start()} (and most of the SDK) reads + * {@code UnityPlayer.currentActivity} to obtain an Activity for + * {@code getSystemService("sensor")}. The real Unity class is NOT bundled in the + * AAR — it is provided by the Unity engine at runtime — so in this native (non-Unity) + * app we supply our own class exposing the one static field the AAR's bytecode + * references via {@code getstatic com/unity3d/player/UnityPlayer.currentActivity}. + * + * Set {@link #currentActivity} before starting any INMO tracker; see + * {@code InmoFusionTracker.start()}. This is the standard pattern for reusing + * Unity Android plugins outside of Unity. + */ +public class UnityPlayer { + /** Read by air3_core via {@code getstatic UnityPlayer.currentActivity}. */ + public static Activity currentActivity; +} diff --git a/android/glasses/.gitignore b/android/glasses/.gitignore new file mode 100644 index 0000000..4853510 --- /dev/null +++ b/android/glasses/.gitignore @@ -0,0 +1,5 @@ +/build + +# Local AAR drop-ins (INMO air3_core) — keep the README, ignore the binaries +/libs/*.aar +!/libs/README.md diff --git a/android/glasses/build.gradle.kts b/android/glasses/build.gradle.kts new file mode 100644 index 0000000..1c1bff9 --- /dev/null +++ b/android/glasses/build.gradle.kts @@ -0,0 +1,62 @@ +plugins { + id("com.android.application") + id("org.jetbrains.kotlin.android") +} + +android { + namespace = "com.pri4l.glasses" + compileSdk = 34 + + defaultConfig { + applicationId = "com.pri4l.glasses" + // INMO Air3 runs Android 14. Targeting 34 lets us consume air3_core.aar + // (which declares minSdk 34) directly, with no tools:overrideLibrary hack. + minSdk = 34 + targetSdk = 34 + versionCode = 1 + versionName = "0.1.0" + + // INMO is arm64 only — restrict native libs (air3_core, OpenCV) to one ABI so the + // OpenCV native blob doesn't bloat the APK across ABIs we'll never run on. + ndk { abiFilters += "arm64-v8a" } + } + + buildTypes { + release { + isMinifyEnabled = false + } + } + + compileOptions { + sourceCompatibility = JavaVersion.VERSION_1_8 + targetCompatibility = JavaVersion.VERSION_1_8 + } + + kotlinOptions { + jvmTarget = "1.8" + } +} + +dependencies { + // INMO Air3 native fusion (decision 011). Local .aar dropped into glasses/libs/. + // Provides com.inmo.air3_core.* + libinmoair3.so. See glasses/libs/README.md. + implementation(fileTree(mapOf("dir" to "libs", "include" to listOf("*.aar")))) + + // CameraX — world-camera streaming to the hub + val cameraVersion = "1.3.1" + implementation("androidx.camera:camera-core:$cameraVersion") + implementation("androidx.camera:camera-camera2:$cameraVersion") + implementation("androidx.camera:camera-lifecycle:$cameraVersion") + + // WebSocket to the hub (rosbridge) + implementation("com.squareup.okhttp3:okhttp:4.12.0") + + // OpenCV (objdetect includes ArucoDetector) for on-device fiducial alignment (decision 010). + // Official Maven AAR with bundled Android native libs since 4.9.0. + implementation("org.opencv:opencv:4.11.0") + + // Core — ComponentActivity is our LifecycleOwner for CameraX (no Compose) + implementation("androidx.core:core-ktx:1.12.0") + implementation("androidx.activity:activity-ktx:1.8.2") + implementation("androidx.lifecycle:lifecycle-runtime-ktx:2.7.0") +} diff --git a/android/glasses/libs/README.md b/android/glasses/libs/README.md new file mode 100644 index 0000000..c618a3d --- /dev/null +++ b/android/glasses/libs/README.md @@ -0,0 +1,24 @@ +# Local AAR drop-in — INMO Air3 native fusion (glasses module) + +Holds the local `air3_core` `.aar` (not committed; see `.gitignore`). The build picks +up any `*.aar` here via the `fileTree(... "libs" ...)` dependency in `build.gradle.kts`. + +## What to drop here + +`air3_core-debug.aar` from the INMO Air3 Unity SDK +(, `Assets/Plugins/Android/`). It provides +`com.inmo.air3_core.atw.GyroRotation` (whose static `vQuat` is the pre-calibrated +head-orientation quaternion) and bundles `libinmoair3.so`. + +## Integration notes (validated 2026-06-23 on IMA301, Android 14) + +The AAR is compiled against the Unity runtime. Running it in this native app needs: + +1. A `com.unity3d.player.UnityPlayer` stub (in `src/main/java/com/unity3d/player/`) — + `GyroRotation.start()` reads `UnityPlayer.currentActivity` for + `getSystemService("sensor")`. `InmoFusionTracker` sets it before `start()`. +2. `System.loadLibrary("inmoair3")` before `start()` — the native fusion symbol + (`AtwCore.nativeGyroFusion`) lives in `libinmoair3.so`, normally loaded by + `Air3Core`'s static initializer (which we bypass). `InmoFusionTracker` does this. + +`vQuat` order `[x,y,z,w]` is correct as-is. See `decisions/011-inmo-air3-track-a.md`. diff --git a/android/glasses/src/main/AndroidManifest.xml b/android/glasses/src/main/AndroidManifest.xml new file mode 100644 index 0000000..ba6e2eb --- /dev/null +++ b/android/glasses/src/main/AndroidManifest.xml @@ -0,0 +1,32 @@ + + + + + + + + + + + + + + + + + + + + + + diff --git a/android/glasses/src/main/java/com/pri4l/glasses/AnchorRenderer.kt b/android/glasses/src/main/java/com/pri4l/glasses/AnchorRenderer.kt new file mode 100644 index 0000000..de38abc --- /dev/null +++ b/android/glasses/src/main/java/com/pri4l/glasses/AnchorRenderer.kt @@ -0,0 +1,95 @@ +package com.pri4l.glasses + +import android.opengl.GLES20 +import android.opengl.GLSurfaceView +import android.opengl.Matrix +import javax.microedition.khronos.egl.EGLConfig +import javax.microedition.khronos.opengles.GL10 + +/** + * Renders hub/phone anchors as labeled cubes, oriented by the 3DoF head tracker. + * + * The INMO is orientation-only (no position), so anchors cannot be placed at their true + * room coordinates — instead the hub map origin is pinned a fixed distance in front of the + * wearer and anchors are laid out relative to it, body-stabilized: as you look around they + * stay put in space (rotating opposite head motion), which reads as "anchored in the room" + * for a head-locked viewer. Use the tracker's recenter to align "forward" with the hub. + * + * Coordinate mapping ROS map frame (x fwd, y left, z up) -> GL eye (x right, y up, z back): + * GL.x = -ROS.y ; GL.y = ROS.z ; GL.z = -ROS.x + * plus [originForward] metres pushed ahead so anchors near the hub origin are visible. + */ +class AnchorRenderer( + private val tracker: HeadTracker, + private val getHubAnchors: () -> List, + private val getPhoneAnchors: () -> List +) : GLSurfaceView.Renderer { + + private val cube = CubeRenderer() + private val label = TextLabelRenderer() + + private val viewMatrix = FloatArray(16) + private val projectionMatrix = FloatArray(16) + private val mvpMatrix = FloatArray(16) + private val modelMatrix = FloatArray(16) + private val tempMatrix = FloatArray(16) + private val labelModel = FloatArray(16) + private val labelMvp = FloatArray(16) + + /** Metres the hub map origin sits ahead of the wearer (so origin-adjacent anchors show). */ + private val originForward = 2.0f + + override fun onSurfaceCreated(gl: GL10?, config: EGLConfig?) { + GLES20.glClearColor(0f, 0f, 0f, 0f) // transparent — waveguide shows the world through + cube.create() + label.create() + } + + override fun onSurfaceChanged(gl: GL10?, width: Int, height: Int) { + GLES20.glViewport(0, 0, width, height) + val aspect = width.toFloat() / height.toFloat() + // INMO Air3 waveguide: ~38° vertical FOV. + Matrix.perspectiveM(projectionMatrix, 0, 38f, aspect, 0.05f, 50f) + } + + override fun onDrawFrame(gl: GL10?) { + GLES20.glClear(GLES20.GL_COLOR_BUFFER_BIT or GLES20.GL_DEPTH_BUFFER_BIT) + if (!tracker.isTracking) return + + tracker.getViewMatrix(viewMatrix) + GLES20.glEnable(GLES20.GL_DEPTH_TEST) + + for (a in getHubAnchors()) drawAnchor(a, 0.2f, 0.4f, 1.0f) // hub = blue + for (a in getPhoneAnchors()) drawAnchor(a, 0.2f, 1.0f, 0.4f) // phone = green + } + + private fun drawAnchor(rosPos: FloatArray, r: Float, g: Float, b: Float) { + val gx = -rosPos[1] + val gy = rosPos[2] + val gz = -rosPos[0] - originForward + + Matrix.setIdentityM(modelMatrix, 0) + Matrix.translateM(modelMatrix, 0, gx, gy, gz) + Matrix.scaleM(modelMatrix, 0, 0.07f, 0.07f, 0.07f) + + Matrix.multiplyMM(tempMatrix, 0, viewMatrix, 0, modelMatrix, 0) + Matrix.multiplyMM(mvpMatrix, 0, projectionMatrix, 0, tempMatrix, 0) + cube.draw(mvpMatrix, r, g, b) + + // Billboarded coordinate label above the cube. + val text = "x:%.2f y:%.2f z:%.2f".format(rosPos[0], rosPos[1], rosPos[2]) + val texId = label.getOrCreateTexture(text) + + Matrix.setIdentityM(labelModel, 0) + // Billboard: copy the view rotation transpose into the model's upper 3x3. + labelModel[0] = viewMatrix[0]; labelModel[1] = viewMatrix[4]; labelModel[2] = viewMatrix[8] + labelModel[4] = viewMatrix[1]; labelModel[5] = viewMatrix[5]; labelModel[6] = viewMatrix[9] + labelModel[8] = viewMatrix[2]; labelModel[9] = viewMatrix[6]; labelModel[10] = viewMatrix[10] + labelModel[12] = gx; labelModel[13] = gy + 0.08f; labelModel[14] = gz + Matrix.scaleM(labelModel, 0, 0.12f, 0.03f, 1f) + + Matrix.multiplyMM(tempMatrix, 0, viewMatrix, 0, labelModel, 0) + Matrix.multiplyMM(labelMvp, 0, projectionMatrix, 0, tempMatrix, 0) + label.draw(labelMvp, texId) + } +} diff --git a/android/glasses/src/main/java/com/pri4l/glasses/ArucoAligner.kt b/android/glasses/src/main/java/com/pri4l/glasses/ArucoAligner.kt new file mode 100644 index 0000000..2c1c3f0 --- /dev/null +++ b/android/glasses/src/main/java/com/pri4l/glasses/ArucoAligner.kt @@ -0,0 +1,175 @@ +package com.pri4l.glasses + +import android.util.Log +import androidx.camera.core.ImageProxy +import org.opencv.calib3d.Calib3d +import org.opencv.core.CvType +import org.opencv.core.Mat +import org.opencv.core.MatOfDouble +import org.opencv.core.MatOfPoint2f +import org.opencv.core.MatOfPoint3f +import org.opencv.core.Point +import org.opencv.core.Point3 +import org.opencv.objdetect.ArucoDetector +import org.opencv.objdetect.Objdetect + +/** + * On-device fiducial alignment (decision 010). Detects a single ArUco marker (DICT_4X4_50, + * id [TARGET_ID]) placed at the hub map origin and solves the glasses' 6DoF pose in the hub + * frame. Because the tag IS the hub origin, glasses-in-hub = inverse of the detected + * tag-in-camera pose. + * + * 3DoF caveat: this is a one-shot 6DoF fix valid at the detection instant. With no positional + * tracking afterward, the translation only stays correct while the wearer rotates in place. + * + * Feed frames via [process] from the camera analyzer thread; read [hubFromCam] (column-major + * GL 4x4, world=hub -> ... actually camera-in-hub) and [aligned] from the render thread. + */ +class ArucoAligner { + + private val detector = ArucoDetector(Objdetect.getPredefinedDictionary(Objdetect.DICT_4X4_50)) + + @Volatile var aligned = false + private set + @Volatile var lastSeenMs = 0L + private set + @Volatile var rangeM = 0f + private set + /** Camera (glasses) position in the hub frame, metres. */ + val camPosHub = FloatArray(3) + /** Camera-in-hub pose, column-major GL 4x4 (rotation R^T, translation -R^T t). */ + val hubFromCam = FloatArray(16) + + private var lastProcMs = 0L + private var diagThrottle = 0 + + // Reused OpenCV scratch. + private val objPoints = MatOfPoint3f() + private val camMatrix = Mat(3, 3, CvType.CV_64F) + private val distCoeffs = MatOfDouble(0.0, 0.0, 0.0, 0.0, 0.0) + private val rvec = Mat() + private val tvec = Mat() + private val rMat = Mat() + + /** + * Detect + solve. [fx320]/[fy320]/[cx320]/[cy320] are intrinsics at the 320x240 stream + * target; we rescale them to the actual frame resolution ArUco sees. + * Returns true if the marker was found this frame. + */ + fun process(image: ImageProxy, fx320: Double, fy320: Double, cx320: Double, cy320: Double): Boolean { + val now = System.currentTimeMillis() + if (now - lastProcMs < MIN_INTERVAL_MS) return false + lastProcMs = now + + val w = image.width + val h = image.height + val gray = grayFromY(image) ?: return false + + val corners = ArrayList() + val ids = Mat() + try { + detector.detectMarkers(gray, corners, ids) + val n = if (ids.empty()) 0 else ids.rows() + if (++diagThrottle % 8 == 0) { + val seen = if (n == 0) "none" else (0 until n).map { ids.get(it, 0)[0].toInt() }.joinToString(",") + val meanY = org.opencv.core.Core.mean(gray).`val`[0] + Log.w(TAG, "aruco: frame ${w}x${h} meanY=%.1f markers=%d ids=[%s]".format(meanY, n, seen)) + } + if (ids.empty()) { aligned = false; return false } + + var found = false + for (i in 0 until ids.rows()) { + if (ids.get(i, 0)[0].toInt() != TARGET_ID) continue + found = solveForCorner(corners[i], w, h, fx320, fy320, cx320, cy320) + break + } + return found + } catch (e: Throwable) { + Log.e(TAG, "aruco detect/solve failed", e) + return false + } finally { + gray.release(); ids.release(); corners.forEach { it.release() } + } + } + + private fun solveForCorner( + corner: Mat, w: Int, h: Int, + fx320: Double, fy320: Double, cx320: Double, cy320: Double + ): Boolean { + // Rescale 320x240-based intrinsics to the actual frame size. + val sx = w / 320.0 + val sy = h / 240.0 + camMatrix.put(0, 0, fx320 * sx, 0.0, w / 2.0, 0.0, fy320 * sy, h / 2.0, 0.0, 0.0, 1.0) + + val L = MARKER_SIZE_M.toDouble() + objPoints.fromArray( + Point3(-L / 2, L / 2, 0.0), Point3(L / 2, L / 2, 0.0), + Point3(L / 2, -L / 2, 0.0), Point3(-L / 2, -L / 2, 0.0) + ) + val img = MatOfPoint2f() + val pts = ArrayList(4) + for (k in 0 until 4) { val v = corner.get(0, k); pts.add(Point(v[0], v[1])) } + img.fromList(pts) + + val ok = Calib3d.solvePnP(objPoints, img, camMatrix, distCoeffs, rvec, tvec) + img.release() + if (!ok) return false + + // tvec = tag origin in camera coords; R (Rodrigues) = tag->camera rotation. + Calib3d.Rodrigues(rvec, rMat) + val r = DoubleArray(9); rMat.get(0, 0, r) // row-major 3x3 + val t = DoubleArray(3); tvec.get(0, 0, t) + + // Camera-in-hub (tag=hub): R_hub_cam = R^T ; pos = -R^T t. + val px = -(r[0] * t[0] + r[3] * t[1] + r[6] * t[2]) + val py = -(r[1] * t[0] + r[4] * t[1] + r[7] * t[2]) + val pz = -(r[2] * t[0] + r[5] * t[1] + r[8] * t[2]) + + synchronized(hubFromCam) { + // Column-major GL: columns are R^T rows; translation = pos. + hubFromCam[0] = r[0].toFloat(); hubFromCam[1] = r[1].toFloat(); hubFromCam[2] = r[2].toFloat(); hubFromCam[3] = 0f + hubFromCam[4] = r[3].toFloat(); hubFromCam[5] = r[4].toFloat(); hubFromCam[6] = r[5].toFloat(); hubFromCam[7] = 0f + hubFromCam[8] = r[6].toFloat(); hubFromCam[9] = r[7].toFloat(); hubFromCam[10] = r[8].toFloat(); hubFromCam[11] = 0f + hubFromCam[12] = px.toFloat(); hubFromCam[13] = py.toFloat(); hubFromCam[14] = pz.toFloat(); hubFromCam[15] = 1f + } + camPosHub[0] = px.toFloat(); camPosHub[1] = py.toFloat(); camPosHub[2] = pz.toFloat() + rangeM = Math.sqrt(t[0] * t[0] + t[1] * t[1] + t[2] * t[2]).toFloat() + lastSeenMs = System.currentTimeMillis() + aligned = true + + Log.w(TAG, "ALIGNED: range=%.2fm camPosHub=[%.2f, %.2f, %.2f]".format(rangeM, px, py, pz)) + return true + } + + /** Build a single-channel grayscale Mat from the ImageProxy's Y plane. */ + private fun grayFromY(image: ImageProxy): Mat? { + if (image.planes.isEmpty()) return null + val y = image.planes[0] + val buf = y.buffer + val w = image.width + val h = image.height + val rowStride = y.rowStride + val gray = Mat(h, w, CvType.CV_8UC1) + if (rowStride == w) { + val data = ByteArray(w * h) + buf.get(data) + gray.put(0, 0, data) + } else { + val row = ByteArray(w) + for (r in 0 until h) { + buf.position(r * rowStride) + buf.get(row, 0, w) + gray.put(r, 0, row) + } + } + return gray + } + + companion object { + private const val TAG = "Pri4L" + private const val TARGET_ID = 0 + private const val MIN_INTERVAL_MS = 150L // ~6.6 Hz detection cap + /** Printed black-square width in metres. MUST match the physical print. */ + const val MARKER_SIZE_M = 0.15f + } +} diff --git a/android/glasses/src/main/java/com/pri4l/glasses/CameraSender.kt b/android/glasses/src/main/java/com/pri4l/glasses/CameraSender.kt new file mode 100644 index 0000000..77277aa --- /dev/null +++ b/android/glasses/src/main/java/com/pri4l/glasses/CameraSender.kt @@ -0,0 +1,269 @@ +package com.pri4l.glasses + +import android.content.Context +import android.graphics.ImageFormat +import android.util.Base64 +import androidx.camera.core.Camera +import androidx.camera.core.CameraSelector +import androidx.camera.core.ImageAnalysis +import androidx.camera.core.ImageProxy +import androidx.camera.lifecycle.ProcessCameraProvider +import androidx.core.content.ContextCompat +import androidx.lifecycle.LifecycleOwner +import org.json.JSONObject +import java.util.concurrent.Executors + +/** + * Captures the glasses' world camera via CameraX ImageAnalysis and publishes raw RGB to + * /glasses/image at ~1.3fps, plus camera_info (real intrinsics) and dummy odom for RTAB-Map + * sync. 320x240. Ported from the phone app's CameraSender. + * + * NOTE: uses DEFAULT_BACK_CAMERA — on the INMO the world-facing camera should bind here; + * verify on-device and switch the selector if it grabs the wrong one. + */ +class CameraSender( + private val lifecycleOwner: LifecycleOwner, + private val client: RosbridgeClient +) { + private var cameraProvider: ProcessCameraProvider? = null + private var camera: Camera? = null + private val executor = Executors.newSingleThreadExecutor() + private var lastSendMs = 0L + private val minIntervalMs = 750L // ~1.3fps + private var lastCameraInfoMs = 0L + + private val targetW = 320 + private val targetH = 240 + + private var fxPx = 0.0 + private var fyPx = 0.0 + private var cxPx = 0.0 + private var cyPx = 0.0 + private var intrinsicsReady = false + + /** + * Optional per-frame tap (with current intrinsics) for on-device processing such as + * fiducial detection. Runs on the analyzer thread, before the frame is closed, on every + * analyzed frame (not gated by the ~1.3fps send throttle). Keep it fast. + */ + var onFrame: ((ImageProxy, Double, Double, Double, Double) -> Unit)? = null + + fun start(context: Context) { + val future = ProcessCameraProvider.getInstance(context) + future.addListener({ + cameraProvider = future.get() + bindAnalysis() + }, ContextCompat.getMainExecutor(context)) + } + + fun stop() { + cameraProvider?.unbindAll() + cameraProvider = null + camera = null + intrinsicsReady = false + } + + private fun bindAnalysis() { + val provider = cameraProvider ?: return + + // 640x480 analyzer frames so the ArUco detector has enough marker pixels; streaming + // still downscales to 320x240 in imageProxyToRgbScaled (the aligner scales intrinsics). + // 640x480 analyzer frames so ArUco has enough marker pixels; default auto-exposure. + val analysis = ImageAnalysis.Builder() + .setTargetResolution(android.util.Size(640, 480)) + .setBackpressureStrategy(ImageAnalysis.STRATEGY_KEEP_ONLY_LATEST) + .build() + + analysis.setAnalyzer(executor) { imageProxy -> + if (!intrinsicsReady) computeIntrinsics(imageProxy) + + // Per-frame tap (fiducial detection) — runs every frame, independent of streaming. + if (intrinsicsReady) onFrame?.invoke(imageProxy, fxPx, fyPx, cxPx, cyPx) + + val now = System.currentTimeMillis() + if (now - lastSendMs >= minIntervalMs && client.state.value == ConnectionState.CONNECTED && intrinsicsReady) { + lastSendMs = now + if (now - lastCameraInfoMs >= 2000) { + publishCameraInfo(imageProxy) + lastCameraInfoMs = now + } + publishFrame(imageProxy) + publishDummyOdom() + } + imageProxy.close() + } + + provider.unbindAll() + camera = provider.bindToLifecycle( + lifecycleOwner, + CameraSelector.DEFAULT_BACK_CAMERA, + analysis + ) + } + + private fun computeIntrinsics(imageProxy: ImageProxy) { + try { + computeIntrinsicsCamera2(imageProxy) + } catch (e: Exception) { + android.util.Log.e("Pri4L", "Camera2 intrinsics failed: ${e.message}", e) + computeIntrinsicsFallback(imageProxy) + } + } + + @androidx.annotation.OptIn(androidx.camera.camera2.interop.ExperimentalCamera2Interop::class) + private fun computeIntrinsicsCamera2(imageProxy: ImageProxy) { + val cam = camera ?: throw IllegalStateException("Camera not bound yet") + val cameraInfo = cam.cameraInfo + val camera2Info = androidx.camera.camera2.interop.Camera2CameraInfo.from(cameraInfo) + val focalLengths = camera2Info.getCameraCharacteristic( + android.hardware.camera2.CameraCharacteristics.LENS_INFO_AVAILABLE_FOCAL_LENGTHS + ) + val sensorSize = camera2Info.getCameraCharacteristic( + android.hardware.camera2.CameraCharacteristics.SENSOR_INFO_PHYSICAL_SIZE + ) + + if (focalLengths != null && focalLengths.isNotEmpty() && sensorSize != null) { + val focalMm = focalLengths[0] + val srcW = imageProxy.width.toDouble() + val srcH = imageProxy.height.toDouble() + + val srcFx = focalMm * srcW / sensorSize.width + val srcFy = focalMm * srcH / sensorSize.height + + val scaleX = targetW.toDouble() / srcW + val scaleY = targetH.toDouble() / srcH + fxPx = srcFx * scaleX + fyPx = srcFy * scaleY + cxPx = targetW / 2.0 + cyPx = targetH / 2.0 + intrinsicsReady = true + + android.util.Log.w("Pri4L", + "INTRINSICS OK: focal=${focalMm}mm, sensor=${sensorSize.width}x${sensorSize.height}mm, " + + "src=${srcW.toInt()}x${srcH.toInt()}, target=${targetW}x${targetH}, fx=$fxPx, fy=$fyPx") + } else { + throw IllegalStateException("Focal lengths or sensor size not available") + } + } + + private fun computeIntrinsicsFallback(imageProxy: ImageProxy) { + // Generic ~70° HFOV guess, refined on-device once Camera2 intrinsics are confirmed. + val srcW = imageProxy.width.toDouble() + val srcH = imageProxy.height.toDouble() + val approxFx = srcW * 0.8 + val scaleX = targetW.toDouble() / srcW + val scaleY = targetH.toDouble() / srcH + fxPx = approxFx * scaleX + fyPx = approxFx * scaleY + cxPx = targetW / 2.0 + cyPx = targetH / 2.0 + intrinsicsReady = true + android.util.Log.w("Pri4L", "INTRINSICS FALLBACK (approx): fx=$fxPx, fy=$fyPx") + } + + private fun publishCameraInfo(imageProxy: ImageProxy) { + val now = System.currentTimeMillis() + val msg = JSONObject().apply { + put("header", JSONObject().apply { + put("stamp", JSONObject().apply { + put("sec", now / 1000) + put("nanosec", (now % 1000) * 1_000_000) + }) + put("frame_id", "glasses_camera") + }) + put("width", targetW) + put("height", targetH) + put("distortion_model", "plumb_bob") + put("d", org.json.JSONArray(doubleArrayOf(0.0, 0.0, 0.0, 0.0, 0.0).toList())) + put("k", org.json.JSONArray(listOf(fxPx, 0.0, cxPx, 0.0, fyPx, cyPx, 0.0, 0.0, 1.0))) + put("r", org.json.JSONArray(listOf(1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0))) + put("p", org.json.JSONArray(listOf(fxPx, 0.0, cxPx, 0.0, 0.0, fyPx, cyPx, 0.0, 0.0, 0.0, 1.0, 0.0))) + } + client.publish("/glasses/camera_info", "sensor_msgs/msg/CameraInfo", msg) + } + + private fun publishFrame(imageProxy: ImageProxy) { + val rgb = imageProxyToRgbScaled(imageProxy) ?: return + val b64 = Base64.encodeToString(rgb, Base64.NO_WRAP) + + val now = System.currentTimeMillis() + val msg = JSONObject().apply { + put("header", JSONObject().apply { + put("stamp", JSONObject().apply { + put("sec", now / 1000) + put("nanosec", (now % 1000) * 1_000_000) + }) + put("frame_id", "glasses_camera") + }) + put("height", targetH) + put("width", targetW) + put("encoding", "rgb8") + put("is_bigendian", 0) + put("step", targetW * 3) + put("data", b64) + } + client.publish("/glasses/image", "sensor_msgs/msg/Image", msg) + } + + private fun publishDummyOdom() { + val now = System.currentTimeMillis() + val msg = JSONObject().apply { + put("header", JSONObject().apply { + put("stamp", JSONObject().apply { + put("sec", now / 1000) + put("nanosec", (now % 1000) * 1_000_000) + }) + put("frame_id", "odom") + }) + put("child_frame_id", "glasses_camera") + put("pose", JSONObject().apply { + put("pose", JSONObject().apply { + put("position", JSONObject().apply { + put("x", 0.0); put("y", 0.0); put("z", 0.0) + }) + put("orientation", JSONObject().apply { + put("x", 0.0); put("y", 0.0); put("z", 0.0); put("w", 1.0) + }) + }) + }) + } + client.publish("/glasses/odom", "nav_msgs/msg/Odometry", msg) + } + + private fun imageProxyToRgbScaled(imageProxy: ImageProxy): ByteArray? { + if (imageProxy.format != ImageFormat.YUV_420_888) return null + + val srcW = imageProxy.width + val srcH = imageProxy.height + val yPlane = imageProxy.planes[0] + val uPlane = imageProxy.planes[1] + val vPlane = imageProxy.planes[2] + + val yBuf = yPlane.buffer + val uBuf = uPlane.buffer + val vBuf = vPlane.buffer + + val yRowStride = yPlane.rowStride + val uvRowStride = uPlane.rowStride + val uvPixelStride = uPlane.pixelStride + + val rgb = ByteArray(targetW * targetH * 3) + var idx = 0 + for (dstRow in 0 until targetH) { + val srcRow = dstRow * srcH / targetH + for (dstCol in 0 until targetW) { + val srcCol = dstCol * srcW / targetW + val y = (yBuf.get(srcRow * yRowStride + srcCol).toInt() and 0xFF) + val uvRow = srcRow / 2 + val uvCol = srcCol / 2 + val u = (uBuf.get(uvRow * uvRowStride + uvCol * uvPixelStride).toInt() and 0xFF) - 128 + val v = (vBuf.get(uvRow * uvRowStride + uvCol * uvPixelStride).toInt() and 0xFF) - 128 + + rgb[idx++] = (y + 1.370705 * v).toInt().coerceIn(0, 255).toByte() + rgb[idx++] = (y - 0.337633 * u - 0.698001 * v).toInt().coerceIn(0, 255).toByte() + rgb[idx++] = (y + 1.732446 * u).toInt().coerceIn(0, 255).toByte() + } + } + return rgb + } +} diff --git a/android/glasses/src/main/java/com/pri4l/glasses/GlRenderers.kt b/android/glasses/src/main/java/com/pri4l/glasses/GlRenderers.kt new file mode 100644 index 0000000..bf9b9e3 --- /dev/null +++ b/android/glasses/src/main/java/com/pri4l/glasses/GlRenderers.kt @@ -0,0 +1,204 @@ +package com.pri4l.glasses + +import android.graphics.Bitmap +import android.graphics.Canvas +import android.graphics.Color +import android.graphics.Paint +import android.graphics.Typeface +import android.opengl.GLES20 +import android.opengl.GLUtils + +/** Solid-color cube (unit half-extent). Ported from the phone app's ArRenderer. */ +class CubeRenderer { + private var program = 0 + private var mvpUniform = 0 + private var colorUniform = 0 + private var positionAttrib = 0 + + private val vertexShader = """ + uniform mat4 u_MVP; + attribute vec4 a_Position; + void main() { + gl_Position = u_MVP * a_Position; + } + """.trimIndent() + + private val fragmentShader = """ + precision mediump float; + uniform vec4 u_Color; + void main() { + gl_FragColor = u_Color; + } + """.trimIndent() + + private val cubeVertices = floatArrayOf( + -1f, -1f, 1f, 1f, -1f, 1f, 1f, 1f, 1f, + -1f, -1f, 1f, 1f, 1f, 1f, -1f, 1f, 1f, + -1f, -1f, -1f, -1f, 1f, -1f, 1f, 1f, -1f, + -1f, -1f, -1f, 1f, 1f, -1f, 1f, -1f, -1f, + -1f, -1f, -1f, -1f, -1f, 1f, -1f, 1f, 1f, + -1f, -1f, -1f, -1f, 1f, 1f, -1f, 1f, -1f, + 1f, -1f, -1f, 1f, 1f, -1f, 1f, 1f, 1f, + 1f, -1f, -1f, 1f, 1f, 1f, 1f, -1f, 1f, + -1f, 1f, -1f, -1f, 1f, 1f, 1f, 1f, 1f, + -1f, 1f, -1f, 1f, 1f, 1f, 1f, 1f, -1f, + -1f, -1f, -1f, 1f, -1f, -1f, 1f, -1f, 1f, + -1f, -1f, -1f, 1f, -1f, 1f, -1f, -1f, 1f, + ) + + private var vertexBuffer: java.nio.FloatBuffer? = null + + fun create() { + program = createProgram(vertexShader, fragmentShader) + mvpUniform = GLES20.glGetUniformLocation(program, "u_MVP") + colorUniform = GLES20.glGetUniformLocation(program, "u_Color") + positionAttrib = GLES20.glGetAttribLocation(program, "a_Position") + + vertexBuffer = java.nio.ByteBuffer.allocateDirect(cubeVertices.size * 4) + .order(java.nio.ByteOrder.nativeOrder()).asFloatBuffer().apply { + put(cubeVertices) + position(0) + } + } + + fun draw(mvpMatrix: FloatArray, r: Float = 0.2f, g: Float = 0.8f, b: Float = 0.4f) { + GLES20.glUseProgram(program) + GLES20.glUniformMatrix4fv(mvpUniform, 1, false, mvpMatrix, 0) + GLES20.glUniform4f(colorUniform, r, g, b, 1f) + GLES20.glVertexAttribPointer(positionAttrib, 3, GLES20.GL_FLOAT, false, 0, vertexBuffer) + GLES20.glEnableVertexAttribArray(positionAttrib) + GLES20.glDrawArrays(GLES20.GL_TRIANGLES, 0, 36) + } +} + +/** Renders text as a textured quad (billboard). Ported from the phone app's ArRenderer. */ +class TextLabelRenderer { + private var program = 0 + private var mvpUniform = 0 + private var textureUniform = 0 + private var positionAttrib = 0 + private var texCoordAttrib = 0 + + private val textureCache = HashMap() + + private val vertexShader = """ + uniform mat4 u_MVP; + attribute vec4 a_Position; + attribute vec2 a_TexCoord; + varying vec2 v_TexCoord; + void main() { + gl_Position = u_MVP * a_Position; + v_TexCoord = a_TexCoord; + } + """.trimIndent() + + private val fragmentShader = """ + precision mediump float; + varying vec2 v_TexCoord; + uniform sampler2D u_Texture; + void main() { + gl_FragColor = texture2D(u_Texture, v_TexCoord); + } + """.trimIndent() + + private val quadVertices = floatArrayOf( + -1f, -1f, 0f, 1f, -1f, 0f, 1f, 1f, 0f, + -1f, -1f, 0f, 1f, 1f, 0f, -1f, 1f, 0f, + ) + private val quadTexCoords = floatArrayOf( + 0f, 1f, 1f, 1f, 1f, 0f, + 0f, 1f, 1f, 0f, 0f, 0f, + ) + + private var vertexBuffer: java.nio.FloatBuffer? = null + private var texCoordBuffer: java.nio.FloatBuffer? = null + + fun create() { + program = createProgram(vertexShader, fragmentShader) + mvpUniform = GLES20.glGetUniformLocation(program, "u_MVP") + textureUniform = GLES20.glGetUniformLocation(program, "u_Texture") + positionAttrib = GLES20.glGetAttribLocation(program, "a_Position") + texCoordAttrib = GLES20.glGetAttribLocation(program, "a_TexCoord") + + vertexBuffer = java.nio.ByteBuffer.allocateDirect(quadVertices.size * 4) + .order(java.nio.ByteOrder.nativeOrder()).asFloatBuffer().apply { + put(quadVertices); position(0) + } + texCoordBuffer = java.nio.ByteBuffer.allocateDirect(quadTexCoords.size * 4) + .order(java.nio.ByteOrder.nativeOrder()).asFloatBuffer().apply { + put(quadTexCoords); position(0) + } + } + + fun getOrCreateTexture(text: String): Int { + textureCache[text]?.let { return it } + val texId = generateLabelTexture(text) + textureCache[text] = texId + return texId + } + + private fun generateLabelTexture(text: String): Int { + val paint = Paint(Paint.ANTI_ALIAS_FLAG).apply { + color = Color.WHITE + textSize = 32f + typeface = Typeface.MONOSPACE + } + val textWidth = paint.measureText(text).toInt() + 16 + val textHeight = 48 + + val bitmap = Bitmap.createBitmap(textWidth, textHeight, Bitmap.Config.ARGB_8888) + val canvas = Canvas(bitmap) + canvas.drawColor(0xCC000000.toInt()) + canvas.drawText(text, 8f, 34f, paint) + + val textures = IntArray(1) + GLES20.glGenTextures(1, textures, 0) + GLES20.glBindTexture(GLES20.GL_TEXTURE_2D, textures[0]) + GLES20.glTexParameteri(GLES20.GL_TEXTURE_2D, GLES20.GL_TEXTURE_MIN_FILTER, GLES20.GL_LINEAR) + GLES20.glTexParameteri(GLES20.GL_TEXTURE_2D, GLES20.GL_TEXTURE_MAG_FILTER, GLES20.GL_LINEAR) + GLES20.glTexParameteri(GLES20.GL_TEXTURE_2D, GLES20.GL_TEXTURE_WRAP_S, GLES20.GL_CLAMP_TO_EDGE) + GLES20.glTexParameteri(GLES20.GL_TEXTURE_2D, GLES20.GL_TEXTURE_WRAP_T, GLES20.GL_CLAMP_TO_EDGE) + GLUtils.texImage2D(GLES20.GL_TEXTURE_2D, 0, bitmap, 0) + bitmap.recycle() + + return textures[0] + } + + fun draw(mvpMatrix: FloatArray, textureId: Int) { + GLES20.glEnable(GLES20.GL_BLEND) + GLES20.glBlendFunc(GLES20.GL_SRC_ALPHA, GLES20.GL_ONE_MINUS_SRC_ALPHA) + + GLES20.glUseProgram(program) + GLES20.glUniformMatrix4fv(mvpUniform, 1, false, mvpMatrix, 0) + + GLES20.glActiveTexture(GLES20.GL_TEXTURE0) + GLES20.glBindTexture(GLES20.GL_TEXTURE_2D, textureId) + GLES20.glUniform1i(textureUniform, 0) + + GLES20.glVertexAttribPointer(positionAttrib, 3, GLES20.GL_FLOAT, false, 0, vertexBuffer) + GLES20.glEnableVertexAttribArray(positionAttrib) + GLES20.glVertexAttribPointer(texCoordAttrib, 2, GLES20.GL_FLOAT, false, 0, texCoordBuffer) + GLES20.glEnableVertexAttribArray(texCoordAttrib) + + GLES20.glDrawArrays(GLES20.GL_TRIANGLES, 0, 6) + + GLES20.glDisable(GLES20.GL_BLEND) + } +} + +fun createProgram(vertexSource: String, fragmentSource: String): Int { + val vertexShader = loadShader(GLES20.GL_VERTEX_SHADER, vertexSource) + val fragmentShader = loadShader(GLES20.GL_FRAGMENT_SHADER, fragmentSource) + val program = GLES20.glCreateProgram() + GLES20.glAttachShader(program, vertexShader) + GLES20.glAttachShader(program, fragmentShader) + GLES20.glLinkProgram(program) + return program +} + +fun loadShader(type: Int, source: String): Int { + val shader = GLES20.glCreateShader(type) + GLES20.glShaderSource(shader, source) + GLES20.glCompileShader(shader) + return shader +} diff --git a/android/glasses/src/main/java/com/pri4l/glasses/GlassesTracker.kt b/android/glasses/src/main/java/com/pri4l/glasses/GlassesTracker.kt new file mode 100644 index 0000000..c222ab9 --- /dev/null +++ b/android/glasses/src/main/java/com/pri4l/glasses/GlassesTracker.kt @@ -0,0 +1,76 @@ +package com.pri4l.glasses + +import android.content.Context +import android.hardware.Sensor +import android.hardware.SensorEvent +import android.hardware.SensorEventListener +import android.hardware.SensorManager +import android.opengl.Matrix + +/** + * Head tracker using Game Rotation Vector (3DoF). Fallback only. + * + * KNOWN-BROKEN on the INMO: head yaw shows up as roll. `remapCoordinateSystem` cannot + * represent the non-axis-aligned IMU-to-eye mounting offset of the INMO chassis (decision + * 011), so this never tracks correctly there — it exists only so the app still runs if the + * air3_core AAR is absent. Prefer [InmoFusionTracker]. + */ +class GlassesTracker(context: Context) : SensorEventListener, HeadTracker { + + override val sourceName: String = "Android Game Rotation Vector (WIP/broken axes)" + + private val sensorManager = context.getSystemService(Context.SENSOR_SERVICE) as SensorManager + private val rotationSensor = sensorManager.getDefaultSensor(Sensor.TYPE_GAME_ROTATION_VECTOR) + + private val rotationMatrix = FloatArray(16) + private val remappedMatrix = FloatArray(16) + + @Volatile + override var isTracking = false + private set + + init { + Matrix.setIdentityM(rotationMatrix, 0) + } + + override fun start() { + if (rotationSensor != null) { + sensorManager.registerListener(this, rotationSensor, SensorManager.SENSOR_DELAY_GAME) + isTracking = true + } + } + + override fun stop() { + sensorManager.unregisterListener(this) + isTracking = false + } + + override fun getViewMatrix(dest: FloatArray) { + synchronized(rotationMatrix) { + SensorManager.remapCoordinateSystem( + rotationMatrix, + SensorManager.AXIS_Y, + SensorManager.AXIS_MINUS_X, + remappedMatrix + ) + Matrix.transposeM(dest, 0, remappedMatrix, 0) + } + } + + override fun onSensorChanged(event: SensorEvent) { + if (event.sensor.type == Sensor.TYPE_GAME_ROTATION_VECTOR) { + synchronized(rotationMatrix) { + SensorManager.getRotationMatrixFromVector(rotationMatrix, event.values) + } + } + } + + override fun onAccuracyChanged(sensor: Sensor?, accuracy: Int) {} + + companion object { + fun isAvailable(context: Context): Boolean { + val sm = context.getSystemService(Context.SENSOR_SERVICE) as SensorManager + return sm.getDefaultSensor(Sensor.TYPE_GAME_ROTATION_VECTOR) != null + } + } +} diff --git a/android/glasses/src/main/java/com/pri4l/glasses/HeadTracker.kt b/android/glasses/src/main/java/com/pri4l/glasses/HeadTracker.kt new file mode 100644 index 0000000..aa57b40 --- /dev/null +++ b/android/glasses/src/main/java/com/pri4l/glasses/HeadTracker.kt @@ -0,0 +1,46 @@ +package com.pri4l.glasses + +/** + * Common interface for 3DoF head-orientation sources. + * + * - [GlassesTracker] — Android Game Rotation Vector. KNOWN-BROKEN axis mapping on + * the INMO (decision 011); kept only as a fallback. + * - [InmoFusionTracker] — INMO's native sensor fusion (`GyroRotation.vQuat`), which + * bakes in the true IMU-to-eye mounting offset. Preferred. + * + * Use [HeadTrackerFactory.create] to obtain the best available source. + */ +interface HeadTracker { + /** True once a valid orientation stream is being received. */ + val isTracking: Boolean + + /** Begin listening to the underlying sensor/fusion source. */ + fun start() + + /** Stop listening and release resources. */ + fun stop() + + /** + * Writes a column-major 4x4 GL view matrix (world -> eye) into [dest]. + * Caller must pass a FloatArray of length 16. + */ + fun getViewMatrix(dest: FloatArray) + + /** + * Re-defines "forward" as the current head orientation. Content placed straight + * ahead appears centered after this call. Optional; no-op if unsupported. + */ + fun recenter() {} + + /** Short human-readable name of the active source, for logging/diagnostics. */ + val sourceName: String + + /** Toggle experimental magnetometer yaw-drift correction. Returns new on/off state. */ + fun toggleYawCorrection(): Boolean = false + + /** Flip the sign of the yaw-drift correction (for on-device tuning). */ + fun flipYawSign() {} + + /** HUD label for the yaw-correction state ("off", "on(+)", "on(-)", or "n/a"). */ + val yawCorrectionLabel: String get() = "n/a" +} diff --git a/android/glasses/src/main/java/com/pri4l/glasses/HeadTrackerFactory.kt b/android/glasses/src/main/java/com/pri4l/glasses/HeadTrackerFactory.kt new file mode 100644 index 0000000..7a85a20 --- /dev/null +++ b/android/glasses/src/main/java/com/pri4l/glasses/HeadTrackerFactory.kt @@ -0,0 +1,31 @@ +package com.pri4l.glasses + +import android.app.Activity +import android.content.Context +import android.util.Log + +/** + * Selects the best available [HeadTracker]. + * + * Preference order: + * 1. [InmoFusionTracker] — INMO native fusion (correct axes; decision 011). + * 2. [GlassesTracker] — Android Game Rotation Vector (WIP/broken axes) fallback. + * + * The INMO path wins once the `air3_core` AAR is present (see `libs/README.md`). + */ +object HeadTrackerFactory { + private const val TAG = "Pri4L" + + fun create(activity: Activity): HeadTracker { + if (InmoFusionTracker.isAvailable()) { + Log.w(TAG, "HeadTracker: using INMO native fusion") + return InmoFusionTracker(activity) + } + Log.w(TAG, "HeadTracker: INMO AAR absent, falling back to Game Rotation Vector") + return GlassesTracker(activity) + } + + /** True if any head-tracking source is usable on this device. */ + fun isAvailable(context: Context): Boolean = + InmoFusionTracker.isAvailable() || GlassesTracker.isAvailable(context) +} diff --git a/android/glasses/src/main/java/com/pri4l/glasses/HubConfig.kt b/android/glasses/src/main/java/com/pri4l/glasses/HubConfig.kt new file mode 100644 index 0000000..0f44aea --- /dev/null +++ b/android/glasses/src/main/java/com/pri4l/glasses/HubConfig.kt @@ -0,0 +1,32 @@ +package com.pri4l.glasses + +import android.content.Context + +/** + * Hub connection config, persisted in SharedPreferences ("pri4l_glasses"). + * + * Entering an IP via the temple touchpad is painful, so the host/port are stored and + * default to the lab hub. Override without a UI via adb on a debug build: + * adb shell run-as com.pri4l.glasses sh -c \ + * "echo \"X.X.X.X\" > shared_prefs/pri4l_glasses.xml" + */ +object HubConfig { + private const val PREFS = "pri4l_glasses" + private const val KEY_HOST = "host" + private const val KEY_PORT = "port" + private const val DEFAULT_HOST = "192.168.68.129" + private const val DEFAULT_PORT = 9090 + + fun host(context: Context): String = + context.getSharedPreferences(PREFS, Context.MODE_PRIVATE) + .getString(KEY_HOST, DEFAULT_HOST) ?: DEFAULT_HOST + + fun port(context: Context): Int = + context.getSharedPreferences(PREFS, Context.MODE_PRIVATE) + .getInt(KEY_PORT, DEFAULT_PORT) + + fun setHost(context: Context, host: String) { + context.getSharedPreferences(PREFS, Context.MODE_PRIVATE) + .edit().putString(KEY_HOST, host).apply() + } +} diff --git a/android/glasses/src/main/java/com/pri4l/glasses/ImuSender.kt b/android/glasses/src/main/java/com/pri4l/glasses/ImuSender.kt new file mode 100644 index 0000000..37dd12f --- /dev/null +++ b/android/glasses/src/main/java/com/pri4l/glasses/ImuSender.kt @@ -0,0 +1,105 @@ +package com.pri4l.glasses + +import android.content.Context +import android.hardware.Sensor +import android.hardware.SensorEvent +import android.hardware.SensorEventListener +import android.hardware.SensorManager +import android.os.Handler +import android.os.HandlerThread +import org.json.JSONArray +import org.json.JSONObject + +/** + * Publishes the glasses' IMU to /glasses/imu at ~10Hz. + * + * Coordinate frame: Android x-right, y-up, z-out-of-screen — no transform applied; the hub + * converts when consuming. frame_id is "glasses_imu" to distinguish from the phone client. + */ +class ImuSender( + private val context: Context, + private val client: RosbridgeClient +) { + private val sensorManager = context.getSystemService(Context.SENSOR_SERVICE) as SensorManager + + private var accel = FloatArray(3) + private var gyro = FloatArray(3) + private var running = false + + private var publishThread: HandlerThread? = null + private var publishHandler: Handler? = null + + private val accelListener = object : SensorEventListener { + override fun onSensorChanged(event: SensorEvent) { accel = event.values.clone() } + override fun onAccuracyChanged(sensor: Sensor?, accuracy: Int) {} + } + + private val gyroListener = object : SensorEventListener { + override fun onSensorChanged(event: SensorEvent) { gyro = event.values.clone() } + override fun onAccuracyChanged(sensor: Sensor?, accuracy: Int) {} + } + + fun start() { + if (running) return + running = true + + sensorManager.getDefaultSensor(Sensor.TYPE_ACCELEROMETER)?.let { + sensorManager.registerListener(accelListener, it, SensorManager.SENSOR_DELAY_GAME) + } + sensorManager.getDefaultSensor(Sensor.TYPE_GYROSCOPE)?.let { + sensorManager.registerListener(gyroListener, it, SensorManager.SENSOR_DELAY_GAME) + } + + publishThread = HandlerThread("imu-publish").also { it.start() } + publishHandler = Handler(publishThread!!.looper) + publishHandler?.post(publishRunnable) + } + + fun stop() { + running = false + sensorManager.unregisterListener(accelListener) + sensorManager.unregisterListener(gyroListener) + publishHandler?.removeCallbacksAndMessages(null) + publishThread?.quitSafely() + publishThread = null + publishHandler = null + } + + private val publishRunnable = object : Runnable { + override fun run() { + if (!running || client.state.value != ConnectionState.CONNECTED) { + if (running) publishHandler?.postDelayed(this, 100) + return + } + + val now = System.currentTimeMillis() + val sec = now / 1000 + val nanosec = (now % 1000) * 1_000_000 + + val msg = JSONObject().apply { + put("header", JSONObject().apply { + put("stamp", JSONObject().apply { + put("sec", sec) + put("nanosec", nanosec) + }) + put("frame_id", "glasses_imu") + }) + put("linear_acceleration", JSONObject().apply { + put("x", accel[0].toDouble()); put("y", accel[1].toDouble()); put("z", accel[2].toDouble()) + }) + put("angular_velocity", JSONObject().apply { + put("x", gyro[0].toDouble()); put("y", gyro[1].toDouble()); put("z", gyro[2].toDouble()) + }) + put("orientation", JSONObject().apply { + put("x", 0.0); put("y", 0.0); put("z", 0.0); put("w", 0.0) + }) + put("orientation_covariance", JSONArray().apply { put(-1.0); repeat(8) { put(0.0) } }) + put("angular_velocity_covariance", JSONArray().apply { repeat(9) { put(0.0) } }) + put("linear_acceleration_covariance", JSONArray().apply { repeat(9) { put(0.0) } }) + } + + client.publish("/glasses/imu", "sensor_msgs/msg/Imu", msg) + publishHandler?.postDelayed(this, 100) // ~10Hz + } + } +} diff --git a/android/glasses/src/main/java/com/pri4l/glasses/InmoArActivity.kt b/android/glasses/src/main/java/com/pri4l/glasses/InmoArActivity.kt new file mode 100644 index 0000000..47bea98 --- /dev/null +++ b/android/glasses/src/main/java/com/pri4l/glasses/InmoArActivity.kt @@ -0,0 +1,218 @@ +package com.pri4l.glasses + +import android.Manifest +import android.content.pm.PackageManager +import android.graphics.Color +import android.opengl.GLSurfaceView +import android.os.Bundle +import android.util.Log +import android.util.TypedValue +import android.view.Gravity +import android.view.KeyEvent +import android.view.WindowManager +import android.widget.FrameLayout +import android.widget.TextView +import androidx.activity.ComponentActivity +import androidx.activity.result.contract.ActivityResultContracts +import androidx.core.content.ContextCompat +import org.opencv.android.OpenCVLoader + +/** + * Purpose-built INMO Air3 client (Track A). Native landscape/fullscreen at the waveguide's + * 1920x1080 — no Compose, no phone UI. Renders hub/phone anchors in 3D with INMO native head + * tracking (decision 011), streams the glasses' camera + IMU to the hub, and shows a minimal + * HUD. The phone app (:app) stays the Track B / ARCore client; this is its own APK. + * + * Touchpad: tap = recenter "forward"; D-pad down = toggle streaming; D-pad up = reconnect. + */ +class InmoArActivity : ComponentActivity() { + + private val TAG = "Pri4L" + + private val client = RosbridgeClient() + private lateinit var tracker: HeadTracker + private lateinit var glView: GLSurfaceView + private lateinit var hud: TextView + + private var imuSender: ImuSender? = null + private var cameraSender: CameraSender? = null + private var aligner: ArucoAligner? = null + private var streaming = false + private var cameraGranted = false + + @Volatile private var hubAnchors: List = emptyList() + @Volatile private var phoneAnchors: List = emptyList() + + private val requestCamera = + registerForActivityResult(ActivityResultContracts.RequestPermission()) { granted -> + cameraGranted = granted + Log.w(TAG, "camera permission granted=$granted") + // Streaming may have auto-started (IMU only) before this dialog was answered. + // If so, bring the camera up now that we're allowed. + if (granted && streaming) cameraSender?.start(this) + updateHud() + } + + override fun onCreate(savedInstanceState: Bundle?) { + super.onCreate(savedInstanceState) + window.addFlags(WindowManager.LayoutParams.FLAG_FULLSCREEN) + window.addFlags(WindowManager.LayoutParams.FLAG_KEEP_SCREEN_ON) + + // Head tracking — INMO native fusion if the AAR is present, else the broken fallback. + tracker = HeadTrackerFactory.create(this) + tracker.start() + Log.w(TAG, "head tracker source: ${tracker.sourceName}") + + glView = GLSurfaceView(this).apply { + setEGLContextClientVersion(2) + setRenderer(AnchorRenderer(tracker, { hubAnchors }, { phoneAnchors })) + renderMode = GLSurfaceView.RENDERMODE_CONTINUOUSLY + } + + hud = TextView(this).apply { + setTextColor(Color.WHITE) + setShadowLayer(3f, 0f, 0f, Color.BLACK) + typeface = android.graphics.Typeface.MONOSPACE + setTextSize(TypedValue.COMPLEX_UNIT_SP, 14f) + setPadding(24, 24, 24, 24) + } + + setContentView(FrameLayout(this).apply { + addView(glView, FrameLayout.LayoutParams( + FrameLayout.LayoutParams.MATCH_PARENT, FrameLayout.LayoutParams.MATCH_PARENT)) + addView(hud, FrameLayout.LayoutParams( + FrameLayout.LayoutParams.WRAP_CONTENT, FrameLayout.LayoutParams.WRAP_CONTENT, + Gravity.TOP or Gravity.START)) + }) + + imuSender = ImuSender(this, client) + cameraSender = CameraSender(this, client) + + // On-device fiducial alignment (decision 010). Detect the ArUco tag at the hub origin + // and tap each camera frame into the aligner. + if (OpenCVLoader.initLocal()) { + aligner = ArucoAligner() + cameraSender?.onFrame = { proxy, fx, fy, cx, cy -> + aligner?.process(proxy, fx, fy, cx, cy) + runOnUiThread { updateHud() } + } + Log.w(TAG, "OpenCV loaded; ArUco aligner ready (marker ${ArucoAligner.MARKER_SIZE_M}m @ hub origin)") + } else { + Log.e(TAG, "OpenCV failed to load; fiducial alignment disabled") + } + + cameraGranted = ContextCompat.checkSelfPermission(this, Manifest.permission.CAMERA) == + PackageManager.PERMISSION_GRANTED + if (!cameraGranted) requestCamera.launch(Manifest.permission.CAMERA) + + // HUD updates whenever connection state or message count changes. + client.state.listener = { runOnUiThread { onStateChanged(); updateHud() } } + client.messageCount.listener = { runOnUiThread { updateHud() } } + + client.subscribe("/hub/anchors/hub", "geometry_msgs/msg/PoseArray", throttleMs = 1000) { + hubAnchors = parsePoseArray(it); runOnUiThread { updateHud() } + } + client.subscribe("/hub/anchors/phone", "geometry_msgs/msg/PoseArray", throttleMs = 1000) { + phoneAnchors = parsePoseArray(it); runOnUiThread { updateHud() } + } + + connect() + updateHud() + } + + private fun connect() { + Log.w(TAG, "connecting to ${HubConfig.host(this)}:${HubConfig.port(this)}") + client.connect(HubConfig.host(this), HubConfig.port(this)) + } + + /** Auto-start streaming once connected (full-client behavior). */ + private fun onStateChanged() { + if (client.state.value == ConnectionState.CONNECTED && !streaming) { + startStreaming() + } + } + + private fun startStreaming() { + imuSender?.start() + if (cameraGranted) cameraSender?.start(this) + streaming = true + Log.w(TAG, "streaming started (imu + ${if (cameraGranted) "camera" else "no-camera"})") + updateHud() + } + + private fun stopStreaming() { + imuSender?.stop() + cameraSender?.stop() + streaming = false + Log.w(TAG, "streaming stopped") + updateHud() + } + + private fun updateHud() { + val s = client.state.value + val cam = if (cameraGranted) "cam" else "cam(no-perm)" + hud.text = buildString { + append("Pri4L Glasses\n") + append("Hub ${HubConfig.host(this@InmoArActivity)}:${HubConfig.port(this@InmoArActivity)} [$s]\n") + append("Tracker ${if (tracker.isTracking) "OK" else "--"}: ${tracker.sourceName}\n") + append("Anchors hub=${hubAnchors.size} phone=${phoneAnchors.size} msgs=${client.messageCount.value}\n") + append("Stream ${if (streaming) "ON ($cam+imu)" else "off"} yawCorr=${tracker.yawCorrectionLabel}\n") + val a = aligner + when { + a == null -> append("Align: opencv off\n") + a.aligned -> append("Align: tag OK range=%.2fm pos=[%.2f,%.2f,%.2f]\n" + .format(a.rangeM, a.camPosHub[0], a.camPosHub[1], a.camPosHub[2])) + else -> append("Align: searching for tag (id 0)\n") + } + append("tap=recenter down=stream up=reconnect L=yawcorr R=flipsign") + } + } + + override fun onKeyDown(keyCode: Int, event: KeyEvent?): Boolean { + when (keyCode) { + KeyEvent.KEYCODE_ENTER, KeyEvent.KEYCODE_NUMPAD_ENTER, + KeyEvent.KEYCODE_DPAD_CENTER, KeyEvent.KEYCODE_SPACE -> { + tracker.recenter() + Log.w(TAG, "recenter (keyCode=$keyCode)") + } + KeyEvent.KEYCODE_DPAD_DOWN -> if (streaming) stopStreaming() else startStreaming() + KeyEvent.KEYCODE_DPAD_UP -> connect() + KeyEvent.KEYCODE_DPAD_LEFT -> { tracker.toggleYawCorrection(); updateHud() } + KeyEvent.KEYCODE_DPAD_RIGHT -> { tracker.flipYawSign(); updateHud() } + else -> { Log.w(TAG, "unhandled keyCode=$keyCode"); return super.onKeyDown(keyCode, event) } + } + return true + } + + private fun parsePoseArray(msg: org.json.JSONObject): List { + val poses = msg.optJSONArray("poses") ?: return emptyList() + val result = mutableListOf() + for (i in 0 until poses.length()) { + val ap = poses.getJSONObject(i).optJSONObject("position") ?: continue + result.add(floatArrayOf( + ap.optDouble("x", 0.0).toFloat(), + ap.optDouble("y", 0.0).toFloat(), + ap.optDouble("z", 0.0).toFloat() + )) + } + return result + } + + override fun onResume() { + super.onResume() + glView.onResume() + tracker.start() + } + + override fun onPause() { + super.onPause() + glView.onPause() + if (streaming) stopStreaming() + tracker.stop() + } + + override fun onDestroy() { + super.onDestroy() + client.disconnect() + } +} diff --git a/android/glasses/src/main/java/com/pri4l/glasses/InmoFusionTracker.kt b/android/glasses/src/main/java/com/pri4l/glasses/InmoFusionTracker.kt new file mode 100644 index 0000000..a10c665 --- /dev/null +++ b/android/glasses/src/main/java/com/pri4l/glasses/InmoFusionTracker.kt @@ -0,0 +1,329 @@ +package com.pri4l.glasses + +import android.app.Activity +import android.content.Context +import android.hardware.Sensor +import android.hardware.SensorEvent +import android.hardware.SensorEventListener +import android.hardware.SensorManager +import android.opengl.Matrix +import android.util.Log +import com.unity3d.player.UnityPlayer + +/** + * Head tracker backed by INMO's native sensor fusion (`com.inmo.air3_core.atw.GyroRotation`). + * + * The vendor quaternion (`GyroRotation.vQuat`) already accounts for the glasses' physical + * IMU mounting — the offset `remapCoordinateSystem` could not represent (decision 011). + * Validated 2026-06-23 on IMA301 (Android 14): yaw/pitch/roll all track correctly, + * vQuat order [x,y,z,w], axisFix identity. + * + * Bound reflectively so the module still builds if the AAR is absent. + * + * REQUIRES AN ACTIVITY: `GyroRotation.start()` reads `UnityPlayer.currentActivity` (not any + * passed-in Context) for `getSystemService("sensor")`. We set it from [activity] in [start] + * via our [UnityPlayer] stub, and `System.loadLibrary("inmoair3")` so the native fusion + * symbol resolves (normally loaded by Air3Core's static init, which we bypass). + */ +class InmoFusionTracker(private val activity: Activity) : HeadTracker { + + override val sourceName: String = "INMO GyroRotation.vQuat (native fusion)" + + @Volatile + override var isTracking = false + private set + + private var gyroInstance: Any? = null + private var quatField: java.lang.reflect.Field? = null + + private val quat = FloatArray(4) // working [x, y, z, w] + private val rot = FloatArray(16) // head -> world + private val refRot = FloatArray(16) // head -> world at recenter + private val tmp = FloatArray(16) + @Volatile private var haveRef = false + @Volatile private var recenterRequested = true // auto-recenter on first frame + + /** Eye-frame correction `view = axisFix * viewRaw`. Identity (validated correct). */ + private val axisFix = FloatArray(16).also { Matrix.setIdentityM(it, 0) } + + // --- magnetometer yaw-drift correction ------------------------------------------- + // INMO's GyroRotation is a 6-axis fusion (accel+gyro, no magnetometer — confirmed via + // sensorservice), so yaw drifts ~0.4°/s with no heading reference. We read the + // magnetometer ourselves and slowly pull the rendered yaw toward magnetic heading. + // Pitch/roll are gravity-stabilized by the fusion and left untouched. + // + // Tunables (expect on-device iteration): [DRIFT_ALPHA] slow-LP rate on the fusion-vs-mag + // yaw difference; [CORRECTION_SIGN] flip if drift is corrected the wrong way; [UP_AXIS] + // the world up-axis to rotate about (assume Z-up/ENU; flip to Y if wrong). + private val sensorManager = + activity.getSystemService(Context.SENSOR_SERVICE) as SensorManager + private val accelData = FloatArray(3) + private val magData = FloatArray(3) + @Volatile private var haveAccel = false + @Volatile private var haveMag = false + private val rMag = FloatArray(9) + private val orient = FloatArray(3) + @Volatile private var magYaw = 0f + @Volatile private var haveMagYaw = false + + private val r9 = FloatArray(9) + private var driftLP = 0f + private var haveDrift = false + private var driftRef = 0f + private val rCorr = FloatArray(16) + private val refCorrected = FloatArray(16) + private var logThrottle = 0 + + // Yaw correction is OFF by default — on-device it needs the right sign/up-axis, and the + // desk is magnetically noisy. Toggle/flip at runtime to tune (see toggleYawCorrection / + // flipYawSign). When off, behavior is the validated 6-axis fusion (slow drift + recenter). + @Volatile var yawCorrectionOn = false + private set + @Volatile private var corrSign = 1f + + private val magListener = object : SensorEventListener { + override fun onSensorChanged(e: SensorEvent) { + when (e.sensor.type) { + Sensor.TYPE_ACCELEROMETER -> { System.arraycopy(e.values, 0, accelData, 0, 3); haveAccel = true } + Sensor.TYPE_MAGNETIC_FIELD -> { System.arraycopy(e.values, 0, magData, 0, 3); haveMag = true } + } + if (haveAccel && haveMag && SensorManager.getRotationMatrix(rMag, null, accelData, magData)) { + SensorManager.getOrientation(rMag, orient) + magYaw = orient[0] // azimuth (radians), absolute magnetic heading + haveMagYaw = true + } + } + override fun onAccuracyChanged(s: Sensor?, a: Int) {} + } + + override fun start() { + try { + // GyroRotation.start() reads UnityPlayer.currentActivity for getSystemService("sensor"). + UnityPlayer.currentActivity = activity + + // Native fusion symbol (AtwCore.nativeGyroFusion) lives in libinmoair3.so, + // normally loaded by Air3Core's static init which we bypass. Load it or the + // first sensor callback aborts with UnsatisfiedLinkError. Idempotent. + try { + System.loadLibrary(NATIVE_LIB) + } catch (e: UnsatisfiedLinkError) { + Log.e(TAG, "Failed to load lib$NATIVE_LIB.so; INMO native fusion unavailable", e) + isTracking = false + return + } + + val cls = Class.forName(GYRO_CLASS) + gyroInstance = tryGetInstance(cls) + + quatField = cls.declaredFields.firstOrNull { it.name == "vQuat" } + ?.apply { isAccessible = true } + ?: cls.fields.firstOrNull { it.name == "vQuat" }?.apply { isAccessible = true } + + tryInvokeStart(cls, gyroInstance) + + if (quatField == null) { + Log.e(TAG, "GyroRotation found but no vQuat field; cannot track") + isTracking = false + return + } + // Our own magnetometer + accel feed for yaw-drift correction (INMO's fusion omits mag). + sensorManager.getDefaultSensor(Sensor.TYPE_ACCELEROMETER)?.let { + sensorManager.registerListener(magListener, it, SensorManager.SENSOR_DELAY_GAME) + } + sensorManager.getDefaultSensor(Sensor.TYPE_MAGNETIC_FIELD)?.let { + sensorManager.registerListener(magListener, it, SensorManager.SENSOR_DELAY_UI) + } + + isTracking = true + Log.w(TAG, "INMO fusion tracker started ($sourceName)") + } catch (e: ClassNotFoundException) { + Log.w(TAG, "INMO air3_core AAR not present; INMO fusion unavailable") + isTracking = false + } catch (e: Throwable) { + Log.e(TAG, "Failed to start INMO fusion tracker", e) + isTracking = false + } + } + + override fun stop() { + isTracking = false + sensorManager.unregisterListener(magListener) + val inst = gyroInstance ?: return + try { + inst.javaClass.methods.firstOrNull { it.name == "stop" && it.parameterTypes.isEmpty() } + ?.invoke(inst) + } catch (e: Throwable) { + Log.w(TAG, "stop() on GyroRotation failed (non-fatal)", e) + } + } + + override fun recenter() { + recenterRequested = true + } + + override fun toggleYawCorrection(): Boolean { + yawCorrectionOn = !yawCorrectionOn + Log.w(TAG, "yaw correction ${if (yawCorrectionOn) "ON sign=$corrSign" else "OFF"}") + return yawCorrectionOn + } + + override fun flipYawSign() { + corrSign = -corrSign + Log.w(TAG, "yaw correction sign flipped to $corrSign") + } + + override val yawCorrectionLabel: String + get() = if (!yawCorrectionOn) "off" else "on(${if (corrSign > 0) "+" else "-"})" + + override fun getViewMatrix(dest: FloatArray) { + if (!isTracking || !readQuat()) { + Matrix.setIdentityM(dest, 0) + return + } + + quatToMatrix(quat, rot) // rot = head -> world + + // Fusion azimuth from rot (column-major head->world) via the same getOrientation + // convention the magnetometer uses, so their difference isolates the yaw drift. + r9[0] = rot[0]; r9[1] = rot[4]; r9[2] = rot[8] + r9[3] = rot[1]; r9[4] = rot[5]; r9[5] = rot[9] + r9[6] = rot[2]; r9[7] = rot[6]; r9[8] = rot[10] + SensorManager.getOrientation(r9, orient) + val fusYaw = orient[0] + + if (haveMagYaw) { + val d = wrap(fusYaw - magYaw) // offset + accumulated drift + if (!haveDrift) { driftLP = d; haveDrift = true } + else driftLP += DRIFT_ALPHA * wrap(d - driftLP) + } + + if (recenterRequested || !haveRef) { + System.arraycopy(rot, 0, refRot, 0, 16) + driftRef = driftLP // freeze the constant offset at recenter + haveRef = true + recenterRequested = false + } + + // Pre-rotate the reference about world-up by the drift accrued since recenter, so a + // stationary head holds a fixed magnetic heading instead of drifting. OFF by default. + val corrDeg = if (yawCorrectionOn && haveMagYaw) + Math.toDegrees((corrSign * wrap(driftLP - driftRef)).toDouble()).toFloat() else 0f + Matrix.setRotateM(rCorr, 0, corrDeg, 0f, 0f, 1f) // Z = world up (ENU); flip to (0,1,0) if wrong + Matrix.multiplyMM(refCorrected, 0, rCorr, 0, refRot, 0) + + // view = axisFix * (rot^T * refCorrected) — relative to the recenter orientation. + Matrix.transposeM(tmp, 0, rot, 0) + Matrix.multiplyMM(dest, 0, tmp, 0, refCorrected, 0) + Matrix.multiplyMM(tmp, 0, axisFix, 0, dest, 0) + System.arraycopy(tmp, 0, dest, 0, 16) + + if (++logThrottle % 120 == 0 && haveMagYaw) { + Log.w(TAG, "yaw fus=%.0f mag=%.0f driftLP=%.0f corr=%.0f".format( + Math.toDegrees(fusYaw.toDouble()), Math.toDegrees(magYaw.toDouble()), + Math.toDegrees(driftLP.toDouble()), corrDeg)) + } + } + + /** Wrap an angle (radians) to (-pi, pi]. */ + private fun wrap(a: Float): Float { + var x = a + val twoPi = (2.0 * Math.PI).toFloat() + while (x > Math.PI) x -= twoPi + while (x < -Math.PI) x += twoPi + return x + } + + // --- reflection helpers -------------------------------------------------- + + private fun tryGetInstance(cls: Class<*>): Any? { + cls.methods.firstOrNull { it.name == "getInstance" && it.parameterTypes.isEmpty() } + ?.let { return it.invoke(null) } + runCatching { cls.getConstructor(Context::class.java).newInstance(activity) } + .getOrNull()?.let { return it } + runCatching { cls.getConstructor().newInstance() }.getOrNull()?.let { return it } + return null + } + + private fun tryInvokeStart(cls: Class<*>, instance: Any?) { + cls.methods.firstOrNull { it.name == "start" && it.parameterTypes.size == 1 && + it.parameterTypes[0] == Context::class.java } + ?.let { it.invoke(instance, activity); return } + cls.methods.firstOrNull { it.name == "start" && it.parameterTypes.isEmpty() } + ?.let { it.invoke(instance); return } + Log.w(TAG, "No start() method on GyroRotation; assuming auto-started") + } + + /** Reads vQuat into [quat] as [x,y,z,w]. Returns false if unreadable. */ + private fun readQuat(): Boolean { + val f = quatField ?: return false + return try { + when (val v = f.get(gyroInstance)) { + is FloatArray -> if (v.size >= 4) { copyQuat(v[0], v[1], v[2], v[3]); true } else false + is DoubleArray -> if (v.size >= 4) { + copyQuat(v[0].toFloat(), v[1].toFloat(), v[2].toFloat(), v[3].toFloat()); true + } else false + null -> false + else -> readQuatObject(v) + } + } catch (e: Throwable) { + false + } + } + + private fun readQuatObject(obj: Any): Boolean { + fun comp(name: String): Float? { + obj.javaClass.fields.firstOrNull { it.name == name }?.let { + return (it.get(obj) as? Number)?.toFloat() + } + val getter = "get" + name.uppercase() + obj.javaClass.methods.firstOrNull { it.name == getter && it.parameterTypes.isEmpty() } + ?.let { return (it.invoke(obj) as? Number)?.toFloat() } + return null + } + val x = comp("x") ?: return false + val y = comp("y") ?: return false + val z = comp("z") ?: return false + val w = comp("w") ?: return false + copyQuat(x, y, z, w) + return true + } + + /** Vendor order is [x,y,z,w] (validated). */ + private fun copyQuat(a: Float, b: Float, c: Float, d: Float) { + quat[0] = a; quat[1] = b; quat[2] = c; quat[3] = d + } + + /** Column-major head->world rotation matrix from a unit quaternion [x,y,z,w]. */ + private fun quatToMatrix(q: FloatArray, m: FloatArray) { + val x = q[0]; val y = q[1]; val z = q[2]; val w = q[3] + val n = x * x + y * y + z * z + w * w + val s = if (n > 0f) 2f / n else 0f + val xs = x * s; val ys = y * s; val zs = z * s + val wx = w * xs; val wy = w * ys; val wz = w * zs + val xx = x * xs; val xy = x * ys; val xz = x * zs + val yy = y * ys; val yz = y * zs; val zz = z * zs + + m[0] = 1f - (yy + zz); m[1] = xy + wz; m[2] = xz - wy; m[3] = 0f + m[4] = xy - wz; m[5] = 1f - (xx + zz); m[6] = yz + wx; m[7] = 0f + m[8] = xz + wy; m[9] = yz - wx; m[10] = 1f - (xx + yy); m[11] = 0f + m[12] = 0f; m[13] = 0f; m[14] = 0f; m[15] = 1f + } + + companion object { + private const val TAG = "Pri4L" + private const val GYRO_CLASS = "com.inmo.air3_core.atw.GyroRotation" + private const val NATIVE_LIB = "inmoair3" // libinmoair3.so — provides nativeGyroFusion + + // Yaw-drift correction tunables (getViewMatrix runs at GL frame rate ~60Hz). + // DRIFT_ALPHA ~0.005 → ~3s time constant: rejects magnetometer jitter while still + // removing the slow gyro drift. Lower = smoother but slower correction. + private const val DRIFT_ALPHA = 0.005f + + /** True if the INMO fusion classes are on the classpath (AAR present). */ + fun isAvailable(): Boolean = try { + Class.forName(GYRO_CLASS); true + } catch (e: Throwable) { + false + } + } +} diff --git a/android/glasses/src/main/java/com/pri4l/glasses/RosbridgeClient.kt b/android/glasses/src/main/java/com/pri4l/glasses/RosbridgeClient.kt new file mode 100644 index 0000000..bddf30d --- /dev/null +++ b/android/glasses/src/main/java/com/pri4l/glasses/RosbridgeClient.kt @@ -0,0 +1,177 @@ +package com.pri4l.glasses + +import android.os.Handler +import android.os.Looper +import okhttp3.OkHttpClient +import okhttp3.Request +import okhttp3.Response +import okhttp3.WebSocket +import okhttp3.WebSocketListener +import org.json.JSONObject +import java.util.concurrent.ConcurrentHashMap +import java.util.concurrent.TimeUnit + +enum class ConnectionState { DISCONNECTED, CONNECTING, CONNECTED, ERROR } + +/** + * Minimal observable holder — replaces Compose's mutableStateOf so this module has no + * Compose dependency. `.value` reads/writes work as before; [listener] (if set) fires on + * the thread that performed the write (callers here write on the main Looper). + */ +class Observable(initial: T) { + @Volatile + var value: T = initial + set(v) { + field = v + listener?.invoke(v) + } + var listener: ((T) -> Unit)? = null +} + +class RosbridgeClient { + + val state = Observable(ConnectionState.DISCONNECTED) + val messageCount = Observable(0) + + private val client = OkHttpClient.Builder() + .readTimeout(0, TimeUnit.MILLISECONDS) // no timeout for WebSocket + .pingInterval(10, TimeUnit.SECONDS) + .build() + + private var ws: WebSocket? = null + private val subscriptions = ConcurrentHashMap Unit>() + private val mainHandler = Handler(Looper.getMainLooper()) + + private var reconnectHost: String? = null + private var reconnectPort: Int? = null + private var reconnectAttempt = 0 + + fun connect(host: String, port: Int = 9090) { + mainHandler.removeCallbacksAndMessages(null) + ws?.cancel() + reconnectHost = host + reconnectPort = port + reconnectAttempt = 0 + doConnect(host, port) + } + + private fun doConnect(host: String, port: Int) { + if (state.value == ConnectionState.CONNECTED) return + ws?.cancel() + state.value = ConnectionState.CONNECTING + + val request = Request.Builder() + .url("ws://$host:$port") + .build() + + ws = client.newWebSocket(request, object : WebSocketListener() { + override fun onOpen(webSocket: WebSocket, response: Response) { + mainHandler.post { + state.value = ConnectionState.CONNECTED + reconnectAttempt = 0 + advertisedTopics.clear() + // Re-issue subscriptions after a (re)connect. + resubscribe() + } + } + + override fun onMessage(webSocket: WebSocket, text: String) { + val json = JSONObject(text) + if (json.optString("op") == "publish") { + mainHandler.post { messageCount.value++ } + val topic = json.optString("topic") + val msg = json.optJSONObject("msg") + if (msg != null) { + subscriptions[topic]?.let { callback -> + mainHandler.post { callback(msg) } + } + } + } else if (json.optString("op") == "service_response") { + val id = json.optString("id", "") + subscriptions["__service__$id"]?.let { callback -> + mainHandler.post { callback(json) } + } + } + } + + override fun onFailure(webSocket: WebSocket, t: Throwable, response: Response?) { + mainHandler.post { + state.value = ConnectionState.ERROR + scheduleReconnect() + } + } + + override fun onClosed(webSocket: WebSocket, code: Int, reason: String) { + mainHandler.post { + state.value = ConnectionState.DISCONNECTED + } + } + }) + } + + private fun scheduleReconnect() { + val host = reconnectHost ?: return + val port = reconnectPort ?: return + reconnectAttempt++ + val delayMs = minOf(1000L * (1 shl minOf(reconnectAttempt, 3)), 10_000L) + mainHandler.postDelayed({ doConnect(host, port) }, delayMs) + } + + fun disconnect() { + reconnectHost = null + reconnectPort = null + mainHandler.removeCallbacksAndMessages(null) + ws?.close(1000, "user disconnect") + ws = null + advertisedTopics.clear() + state.value = ConnectionState.DISCONNECTED + } + + private val advertisedTopics = mutableSetOf() + private val subscribeTypes = ConcurrentHashMap>() + + fun publish(topic: String, type: String, msg: JSONObject) { + if (topic !in advertisedTopics) { + val adJson = JSONObject().apply { + put("op", "advertise") + put("topic", topic) + put("type", type) + } + ws?.send(adJson.toString()) + advertisedTopics.add(topic) + } + + val json = JSONObject().apply { + put("op", "publish") + put("topic", topic) + put("type", type) + put("msg", msg) + } + try { + ws?.send(json.toString()) + } catch (_: Exception) { + // Drop frame silently — reconnect will recover. + } + } + + fun subscribe(topic: String, type: String, throttleMs: Int = 0, callback: (JSONObject) -> Unit) { + subscriptions[topic] = callback + subscribeTypes[topic] = type to throttleMs + sendSubscribe(topic, type, throttleMs) + } + + private fun sendSubscribe(topic: String, type: String, throttleMs: Int) { + val json = JSONObject().apply { + put("op", "subscribe") + put("topic", topic) + put("type", type) + if (throttleMs > 0) put("throttle_rate", throttleMs) + } + ws?.send(json.toString()) + } + + /** Re-send all subscriptions (called on (re)connect so they survive a dropped socket). */ + private fun resubscribe() { + subscribeTypes.forEach { (topic, tt) -> sendSubscribe(topic, tt.first, tt.second) } + } +} diff --git a/android/glasses/src/main/java/com/unity3d/player/UnityPlayer.java b/android/glasses/src/main/java/com/unity3d/player/UnityPlayer.java new file mode 100644 index 0000000..91c71f2 --- /dev/null +++ b/android/glasses/src/main/java/com/unity3d/player/UnityPlayer.java @@ -0,0 +1,20 @@ +package com.unity3d.player; + +import android.app.Activity; + +/** + * Minimal stand-in for Unity's UnityPlayer. + * + * The INMO {@code air3_core} AAR is compiled against the Unity runtime: + * {@code GyroRotation.start()} reads {@code UnityPlayer.currentActivity} to obtain an + * Activity for {@code getSystemService("sensor")}. The real Unity class is NOT bundled + * in the AAR, so this native app supplies the one static field the AAR's bytecode + * references via {@code getstatic com/unity3d/player/UnityPlayer.currentActivity}. + * + * Set {@link #currentActivity} before starting any INMO tracker (see + * {@code InmoFusionTracker.start()}). See decision 011. + */ +public class UnityPlayer { + /** Read by air3_core via {@code getstatic UnityPlayer.currentActivity}. */ + public static Activity currentActivity; +} diff --git a/android/settings.gradle.kts b/android/settings.gradle.kts index 1801ac5..aa45a88 100644 --- a/android/settings.gradle.kts +++ b/android/settings.gradle.kts @@ -16,3 +16,4 @@ dependencyResolutionManagement { rootProject.name = "Pri4LHub" include(":app") +include(":glasses") diff --git a/archive/README.md b/archive/README.md new file mode 100644 index 0000000..ec7cff2 --- /dev/null +++ b/archive/README.md @@ -0,0 +1,20 @@ +# Archive + +Experiments and one-off artifacts kept for reference but not part of the active system. + +## `launch_glasses_localizer.sh` + +A second RTAB-Map instance in RGB-only localization mode that tried to relocalize the INMO +glasses' world camera against the hub's RealSense-built map (cross-sensor visual matching). + +**Outcome (2026-06-23): dead end — archived, do not revive.** It reproduced the failure +decision 009 already documented for the phone: cross-sensor monocular relocalization can't +register frames to the RGB-D map (0 successful localizations, 84 `Transform cannot be +estimated`). The working path for client→hub alignment is **fiducial alignment** (decision +010): see `fiducials/aruco_DICT4X4_50_id0.png` and the `FiducialAligner` in the apps. + +## `publish_random_anchors.py` + +Dev utility that publishes random anchors within the D435 depth frustum to `/hub/anchors` +at 1 Hz (for exercising the AR overlay). Not referenced by any launch script; archived as a +standalone test tool. Run directly with `python3 archive/publish_random_anchors.py` if needed. diff --git a/archive/launch_glasses_localizer.sh b/archive/launch_glasses_localizer.sh new file mode 100644 index 0000000..b9f684d --- /dev/null +++ b/archive/launch_glasses_localizer.sh @@ -0,0 +1,63 @@ +#!/bin/bash +# AR Spatial Hub — Glasses Localizer (EXPERIMENT) +# Second RTAB-Map instance in localization-only mode that tries to relocalize the INMO +# glasses' world camera against the hub's saved map (cross-sensor visual matching). +# +# This revisits the approach decision 009 abandoned for the phone (3-5/6 inliers). We're +# running it to see the glasses-cam numbers firsthand before committing to fiducial align. +# +# Prereqs: hub running (rosbridge + map at ~/.ros/rtabmap.db), glasses app streaming +# /glasses/image + /glasses/camera_info. +# Output: /glasses/pose (if it ever localizes). Watch the log for loop-closure inliers. + +set -e +source /opt/ros/humble/setup.bash + +DB_PATH="${HOME}/.ros/rtabmap.db" +LOG="${HOME}/.ros/logs/hub/glasses_localizer.log" +mkdir -p "$(dirname "$LOG")" + +if [ ! -f "$DB_PATH" ]; then + echo "ERROR: No map database at $DB_PATH — run launch_hub.sh first." + exit 1 +fi + +cleanup() { echo "Shutting down glasses localizer..."; kill $PID 2>/dev/null; wait 2>/dev/null; } +trap cleanup EXIT INT TERM + +echo "=== Glasses localizer (RTAB-Map localization mode) ===" +echo " rgb=/glasses/image info=/glasses/camera_info db=$DB_PATH log=$LOG" + +# Localization-only, monocular (no depth). Mirrors the removed phone localizer (decision 009) +# but pointed at /glasses/* and with looser visual-matching gates so we can SEE inlier counts: +# Vis/MinInliers low + RGBD/OptimizeMaxError 0 so matches aren't silently rejected on odom. +# Run the rtabmap node DIRECTLY in RGB-only mode (the rtabmap_launch wrapper always wires an +# RGB-D sync and hangs waiting on a depth topic the monocular glasses cam can't provide). +# subscribe_rgb + subscribe_depth=false -> RGB + camera_info only +# Mem/IncrementalMemory=false -> localization (don't grow the map) +# Vis/EstimationType=1 (PnP) -> 2D(query)->3D(map) pose, the only option w/o depth +# Kp/DetectorStrategy / Vis/FeatureType -> SIFT (best cross-sensor descriptor per decision 009) +# Vis/MinInliers 6 -> the 6-inlier gate 009 couldn't clear; we LOG attempts +# odom:=/glasses/odom (dummy identity) -> gives rtabmap a pose source so it processes frames +# NB: RTAB-Map's "Foo/Bar" params must be passed as STRINGS (the node rejects int/bool/float). +# The :="'value'" quoting makes ros2/YAML treat them as strings. Node-level params +# (subscribe_*, approx_sync, frame_id, database_path) are real typed params. +ros2 run rtabmap_slam rtabmap --ros-args \ + -p subscribe_depth:=false \ + -p subscribe_rgb:=true \ + -p approx_sync:=true \ + -p frame_id:=glasses_camera \ + -p database_path:="$DB_PATH" \ + -p Mem/IncrementalMemory:="'false'" \ + -p Mem/InitWMWithAllNodes:="'true'" \ + -p Vis/EstimationType:="'1'" \ + -p Vis/MinInliers:="'6'" \ + -p RGBD/OptimizeMaxError:="'0.0'" \ + -p Rtabmap/LoopThr:="'0.05'" \ + -r rgb/image:=/glasses/image \ + -r rgb/camera_info:=/glasses/camera_info \ + -r odom:=/glasses/odom 2>&1 | tee "$LOG" & +PID=$! + +echo " PID=$PID — Ctrl+C to stop. Tail: tail -f $LOG" +wait $PID diff --git a/publish_random_anchors.py b/archive/publish_random_anchors.py similarity index 100% rename from publish_random_anchors.py rename to archive/publish_random_anchors.py diff --git a/decisions/011-inmo-air3-track-a.md b/decisions/011-inmo-air3-track-a.md index 5229462..9ebd333 100644 --- a/decisions/011-inmo-air3-track-a.md +++ b/decisions/011-inmo-air3-track-a.md @@ -1,7 +1,7 @@ # 011 — INMO Air3: Track A Glasses Platform -**Status:** In Progress — sensor orientation blocked -**Date:** 2026-05-31 +**Status:** 3DoF orientation RESOLVED on-device 2026-06-23 (IMA301, Android 14) via INMO native fusion (`GyroRotation.vQuat`). Required: a `com.unity3d.player.UnityPlayer` stub (the AAR reads `currentActivity`), `System.loadLibrary("inmoair3")` (the SDK normally loads it in `Air3Core`'s static init, which we bypass), and `tools:overrideLibrary` for the AAR's minSdk 34. `vQuat` order `[x,y,z,w]` correct as-is; tap-to-recenter handles content centering. Next: full `MainActivity` + hub test; VIO/6DoF still future. (Earlier: blocked on `remapCoordinateSystem` — structurally insufficient, see below.) +**Date:** 2026-05-31 (updated 2026-06-23) **Context:** Track A requires AR glasses that can connect to the hub as a thin client. INMO Air3 arrived — this doc captures platform capabilities, integration progress, and current blockers. --- @@ -47,6 +47,18 @@ **Root cause:** The Game Rotation Vector's reference frame assumes the device's "natural" orientation is portrait (phone). The INMO is landscape-native AND worn perpendicular to a phone's typical orientation. The sensor fusion already accounts for gravity, but the mapping from device axes to GL axes has a rotational ambiguity that `remapCoordinateSystem` can't resolve with a single call. +#### Investigation 2026-06-01 — refined diagnosis + +Re-read `GlassesTracker.getViewMatrix()` (`android/.../GlassesTracker.kt:46-60`). Two findings sharpen the root cause: + +1. **The remaps are phone-derived and double-rotate.** `AXIS_X, AXIS_Z` (the earlier attempt) is the *standard "phone held vertically, back camera pointing forward"* remap — it rotates the frame ~90° about pitch so a vertically-held phone's screen-up becomes forward. **Glasses are already worn in the looking-forward orientation**, so the IMU frame is already ~aligned with the eye frame. Applying the phone-camera remap adds a spurious ~90° pitch rotation, which swaps the yaw and roll axes — exactly the observed "yaw → roll" symptom. The current `AXIS_Y, AXIS_MINUS_X` is a different guess at the same dead end. + +2. **`remapCoordinateSystem` is structurally too weak.** It can only express the 24 axis-aligned 90° permutations (proper + improper). The true IMU→eye transform depends on how the IMU is physically soldered inside the INMO chassis — almost certainly **not** a clean 90° multiple. No single `remapCoordinateSystem` call can represent a non-axis-aligned mounting offset. This is the structural reason every axis combination fails on at least one axis, and it means the fix is **not** "find the right remap pair." + +3. **No recentering / Earth-referenced frame.** `getRotationMatrixFromVector` yields a device→world matrix in Android ENU (X=East, Y=North, Z=Up). With no captured reference orientation, "forward" is magnetic north, not where the wearer is looking. Even with axes fixed, a "look straight ahead" reference capture is required for cubes to sit in front of the user. (Game Rotation Vector drops the magnetometer, so absolute yaw is unavailable anyway — relative tracking from a captured reference is the only correct model.) + +**Implication:** Continuing to permute `remapCoordinateSystem` axes cannot succeed. The correct fix is either (a) consume INMO's pre-calibrated fusion quaternion (`GyroRotation.vQuat`), which already bakes in the true mounting offset, or (b) solve for a single constant correction quaternion `C` empirically (`view = C · Rᵀ`, with `C` from a Unity reference capture or a guided look-forward calibration). See refined plan below. + ### 2. VIO/6DoF Not Yet Accessed **Problem:** The INMO has full VIO (Visual Inertial Odometry) via `com.inmo.air3_core.vio.ArPoseCore` and `com.arglasses.arservice.IServiceInterface`, but this service only starts when their Unity SDK app runs. We haven't tried binding to it from our app. @@ -80,6 +92,8 @@ Native library: `libinmoair3.so` (4.2MB, arm64) — contains the actual SLAM/VIO ### Phase 1: Fix 3DoF Orientation (immediate next session) +> **Priority after 2026-06-01 investigation:** Do **not** spend more time permuting `remapCoordinateSystem` axes — it cannot represent the true (non-axis-aligned) IMU mounting. Pursue Option A first; if the AAR can't be integrated quickly, Option B (empirical correction quaternion via a guided look-forward capture) is the fastest self-contained fallback. Option C (Unity) is the reference-of-last-resort for deriving the exact `C`. + **Option A — Use INMO's native fusion (preferred):** 1. Include `air3_core-debug.aar` in our project as a dependency 2. Instantiate `GyroRotation`, call `start()` @@ -89,6 +103,14 @@ Native library: `libinmoair3.so` (4.2MB, arm64) — contains the actual SLAM/VIO **Why this should work:** INMO's fusion is calibrated for their hardware orientation. It outputs a quaternion that their ATW uses for reprojection — it's designed for exactly this use case. +**Scaffolding landed 2026-06-01 (compiles, AAR not yet present):** +- `HeadTracker` interface abstracts the orientation source; `GlassesTracker` (Game Rotation Vector, fallback) and `InmoFusionTracker` (INMO fusion) both implement it. +- `InmoFusionTracker` binds to `com.inmo.air3_core.atw.GyroRotation` **reflectively** so the project builds before the AAR is in-hand and the INMO path activates automatically once it is. Includes quaternion→view conversion + a recenter ("look forward") reference and an `axisFix` hook for eye-axis handedness. +- `HeadTrackerFactory.create()` selects INMO fusion when `air3_core` is on the classpath, else falls back. Wired into `MainActivity.startGlasses()` and `GlassesTestActivity`. +- AAR integration: drop `air3_core-*.aar` into `android/app/libs/` (gitignored); picked up via `fileTree` in `app/build.gradle.kts`. See `android/app/libs/README.md` for how to obtain it. +- **Remaining (needs hardware + the AAR):** obtain the AAR; confirm on-device the assumptions flagged in `InmoFusionTracker.kt` — vQuat component order, field type, `GyroRotation` lifecycle, and `axisFix`. Validate via `GlassesTestActivity` (logcat should report `head tracker source: INMO ...`). +- Build note: AGP requires JDK 17+; the only JDK on the bench machine is 11. Use Android Studio's bundled JBR — `JAVA_HOME=~/android-studio/jbr ./gradlew ...`. + **Option B — Manual calibration approach:** 1. On app launch, render a fixed crosshair and ask user to look straight ahead 2. Capture the raw rotation matrix at that moment as "reference forward" diff --git a/fiducials/aruco_DICT4X4_50_id0.png b/fiducials/aruco_DICT4X4_50_id0.png new file mode 100644 index 0000000..79cf423 Binary files /dev/null and b/fiducials/aruco_DICT4X4_50_id0.png differ