In ArSceneView with a custom LocationDataSource the camera orientation is wrong

[simone-vitale-overit]

Hello community,

I'm working with ArSceneView in Esri toolkit for Android and I have the needed to put the user in the exact position getted from GPS on the map visible in the scene.

If I use a class provided by ESRI like ArLocationDataSource I have a wrong altitude and a bad orientation of camera on the map respect the real north.

So I have decided to write my own LocationDataSource and I past the code below:

class ArFusedLocationDataSource(private val context: Context, private val viewModel: GISARActivityViewModel, private val arSceneView : ArcGISArView) : LocationDataSource() {
    // Location Vars
    private lateinit var fusedLocationClient : FusedLocationProviderClient
    private var locationRequest : com.google.android.gms.location.LocationRequest? = null
    private lateinit var locationCallback: LocationCallback
    // The last updated location
    private var lastLocation: Location? = null
    private val ACCURACY_THRESHOLD_FACTOR = 1.0
    // End location vars
    private var currentRotationDegree : Double = -1.0

    // Rotation Vars
    // The sensor manager to detect the device orientation for compass mode
    private val sensorManager: SensorManager? by lazy {
        context.getSystemService(Context.SENSOR_SERVICE) as? SensorManager?
    }

    // The internal listener to update the heading for compass mode
    private var internalHeadingListener: InternalHeadingListener? = null
    // End Rotation vars

    override fun onStart() {

        // Create request
        createLocationRequest()

        // Create Callback
        createCallback()

        // create FusionProviderClient
        fusedLocationClient = LocationServices.getFusedLocationProviderClient(context)

        startLocationUpdates()
        startUpdateHeading()

    }

    override fun onStop() {
        fusedLocationClient.removeLocationUpdates(locationCallback)
        stopUpdateHeading()
    }

    private fun startLocationUpdates() {
        fusedLocationClient.requestLocationUpdates(locationRequest!!, locationCallback, context.mainLooper)
    }

    private fun createCallback()
    {
        locationCallback = object : LocationCallback() {
            override fun onLocationResult(results: LocationResult) {
                if(results == null || results.locations.count() == 0)
                    return

                val  newerLocation = results.locations.last()

                if(newerLocation != null)
                {
                    Toast.makeText(context, "Current GPS!! Accuracy: " + newerLocation.accuracy.toString() + " Time: " + getDate(newerLocation.time, "yyyy-MM-dd HH:mm:ss.SSS").toString(), Toast.LENGTH_LONG).show()
                    updateEsriLocation(newerLocation, true)
                }
                else
                {
                    Toast.makeText(context, "Cannot get location.", Toast.LENGTH_SHORT).show()
                }
            }
        }

    }

    private fun createLocationRequest()
    {
        locationRequest = com.google.android.gms.location.LocationRequest.create()?.apply {
            interval = 5000
            fastestInterval = 2000
            priority = com.google.android.gms.location.LocationRequest.PRIORITY_HIGH_ACCURACY
        }
    }

    /**
     * Updates the LocationDataSource location with the provided [location], indicating if the
     * provided location is the [lastKnown] location.
     *
     * @since 100.6.0
     */
    private fun updateEsriLocation(location: android.location.Location?, lastKnown: Boolean) {
        if (location != null) {

            if(location.accuracy > 10f)
            {
                val message = context.getString(R.string.Alert_GpsSignal_weak) + " " + String.format("%.2f", location.accuracy) + " meters"

                var currentToast = Toast.makeText(context, Html.fromHtml("<font color='#FFFF00'><b>" + message +  "</b></font>"), Toast.LENGTH_SHORT)
                currentToast.setGravity(Gravity.TOP, 50,0)
                currentToast.show()
            }

            // If new location accuracy is two times less than previous one, it will be ignored
            if (lastLocation != null) {
                val accuracyThreshold =
                    lastLocation!!.horizontalAccuracy * ACCURACY_THRESHOLD_FACTOR
                if (location.accuracy > accuracyThreshold) {
                    return
                }
            }

            val currentLocation = location.toEsriLocation(lastKnown)
            updateLocation(currentLocation)
            lastLocation = currentLocation
        }
    }

    private fun startUpdateHeading() {
        if (internalHeadingListener == null) {
            internalHeadingListener = InternalHeadingListener()
        }

        sensorManager?.let {
            // Most devices have one or both hardware-sensors
            it.registerListener(
                internalHeadingListener,
                it.getDefaultSensor(Sensor.TYPE_ACCELEROMETER),
                SensorManager.SENSOR_DELAY_UI
            )
            it.registerListener(
                internalHeadingListener,
                it.getDefaultSensor(Sensor.TYPE_MAGNETIC_FIELD),
                SensorManager.SENSOR_DELAY_UI
            )
        }
    }

    /**
     * Stops the sensor manager if it is started.
     *
     * @since 100.6.0
     */
    private fun stopUpdateHeading() {
        sensorManager?.unregisterListener(internalHeadingListener)
        if (internalHeadingListener != null) {
            internalHeadingListener = null
        }
        // Reset the heading to NaN when the heading is not available
        updateHeading(java.lang.Double.NaN)
    }

    fun getCurrentRotationInDegree() : Double
    {
        return currentRotationDegree
    }


    /**
     * Internal implementation for [SensorEventListener] to listen to changes in orientation of the device.
     *
     * @since 100.6.0
     */
    private inner class InternalHeadingListener : SensorEventListener {
        private var gravity = FloatArray(3)

        private var geomagnetic = FloatArray(3)

        override fun onSensorChanged(event: SensorEvent) {

            val type = event.sensor.type
            if (type == Sensor.TYPE_ACCELEROMETER) {
                gravity = lowPassFilter(event.values.clone(), gravity)
            }

            if (type == Sensor.TYPE_MAGNETIC_FIELD) {
                geomagnetic = lowPassFilter(event.values.clone(), geomagnetic)
            }

            if(type == Sensor.TYPE_ROTATION_VECTOR)
            {
                val currentRotation = event.values
                val rotationMatrix = FloatArray(9)
                SensorManager.getRotationMatrixFromVector(rotationMatrix, currentRotation)
            }

            if(gravity != null && geomagnetic != null)
            {
                val R = FloatArray(9)
                val outR = FloatArray(9)
                val I = FloatArray(9)

                val success = SensorManager.getRotationMatrix(R,I, gravity, geomagnetic)
                if(success)
                {
                    val orientation = FloatArray(3)
                    SensorManager.remapCoordinateSystem(R, SensorManager.AXIS_X, SensorManager.AXIS_Z, outR)
                    SensorManager.getOrientation(outR, orientation)
                    val azimut = orientation[0]
                    var azimutInDegree = (Math.toDegrees(azimut.toDouble()))%360
                    if(azimutInDegree < 0)
                        azimutInDegree += 360.0
                    Toast.makeText(context, "Rotation computed: " + azimutInDegree.toString(), Toast.LENGTH_SHORT).show()
                    updateHeading(azimutInDegree)
                }
            }
        }

        /**
         * Function to apply low pass filter to smooth out sensor readings. Based upon implementation here:
         * https://www.built.io/blog/applying-low-pass-filter-to-android-sensor-s-readings
         *
         * @since 100.6.0
         */
        private fun lowPassFilter(input: FloatArray, output: FloatArray?): FloatArray {
            if (output == null) {
                return input
            }
            for (i in input.indices) {
                output[i] = output[i] + 0.1f * (input[i] - output[i])
            }
            return output
        }

        override fun onAccuracyChanged(sensor: Sensor, accuracy: Int) {}

    }

    fun getDate(milliSeconds: Long, dateFormat: String? = "yyyy-MM-dd HH:mm:ss.SSS"): String? {
        // Create a DateFormatter object for displaying date in specified format.
        val formatter: DateFormat = SimpleDateFormat(dateFormat)

        // Create a calendar object that will convert the date and time value in milliseconds to date.
        val calendar = Calendar.getInstance()
        calendar.timeInMillis = milliSeconds
        return formatter.format(calendar.time)
    }
}

// Utility methods

/**
 * Creates an instance of LocationDataSource.Location from an instance of android.location.Location.
 *
 * @param lastKnown true if the location is last one, otherwise it should be false
 * @since 100.6.0
 */
private fun android.location.Location.toEsriLocation(lastKnown: Boolean): LocationDataSource.Location {
    val position = if (hasAltitude()) Point(
        longitude,
        latitude,
        1.5,
        SpatialReferences.getWgs84()
    ) else {
        // Else provide a position without Z value
        Point(longitude, latitude, SpatialReferences.getWgs84())
    }

    val timeStamp = createCalendarFromTimeInMillis(time)

    val verticalAccuracy =
        if (Build.VERSION.SDK_INT >= Build.VERSION_CODES.O) verticalAccuracyMeters.toDouble() else Double.NaN

    return LocationDataSource.Location(
        position,
        accuracy.toDouble(),
        verticalAccuracy,
        speed.toDouble(),
        bearing.toDouble(),
        lastKnown,
        timeStamp
    )
}

/**
 * Creates a Calendar with standard TimeZone and Locale from a [timeInMillis].
 *
 * @since 100.6.0
 */
private fun createCalendarFromTimeInMillis(timeInMillis: Long): Calendar {
    val ret = GregorianCalendar(TimeZone.getTimeZone("UTC"), Locale.ENGLISH)
    ret.timeInMillis = timeInMillis
    return ret
}

Some parts of this class I have getted from the original ArLocationDataSource present on ESRI's Github.

The problem with my custom location datasource is that the device orientation is completely lost and the camera is setting to north of the map and not in the direction where the camera point in the reality respect the map.

Why I have this strange behaviour with my custom LocationDataSource?

Thanks for the support.