no.kartverket.positionorientation

Class TangoPositionOrientationProvider

java.lang.Object

no.kartverket.positionorientation.PositionOrientationProvider

no.kartverket.positionorientation.TangoPositionOrientationProvider

public final class TangoPositionOrientationProvider
extends PositionOrientationProvider

Provides position and orientation information for the application. The the Tango system uses various device sensors (camera, distance sensors, acceleration/rotation sensors etc...) to find the device position in relation to the environment, and build a geometrical model of the environment. The device and environment positions are determined in a local coordinate system with the x and y axis in the horizontal plane, and the z axis towards zenith. The position of the origin and the horizontal orientation of the coordinate system is determined my the device position and orientation when Tango si initialized.

By using data from external sensors the local Tango coordiante system can be placed and oriented in relation to a global (geographic) coordinate system. For an initial determination of the position and orientation parameters data from the devices compass and GPS may be added through the handleRotationVectorObservation(float[]) and handleLocationObservation(Location) methods.

For further refinement manual observations may be added by pointing in a map or using the Tango point cloud sensor for matching against a terrain model

Created by runaas on 20.04.2017.

Nested Class Summary

Nested Classes

Modifier and Type	Class and Description
private class	TangoPositionOrientationProvider.PrivPredictionMatrixUpdater
private class	TangoPositionOrientationProvider.TransformWorker Worker run in the background, recomputing the transform parameters when new data is present

Field Summary

Fields

Modifier and Type	Field and Description
private float	compass_az
private float	compass_dir
private float	declination
(package private) static double	EarthRadius
private double	geoidH_0
private org.ejml.data.DMatrixRMaj	H_tango
private KalmanFilter	kalmanFilter Simple Kalman filter for smoothing Tango position information and computing speed, travel direction and predicted positions
private android.location.Location	lastLocation
private static int	numParameters
private java.util.concurrent.CopyOnWriteArrayList<Transform.Observation>	observations List of observations used in determining the transformation between TAngo and external coordiantes
private OriginData	origin
private TangoPositionOrientationProvider.PrivPredictionMatrixUpdater	predictionMatrixUpdater
private long	prevCompTime Timestamp of previous kalman filter prediction
private org.ejml.data.DMatrixRMaj	R_tango
(package private) static java.lang.String	TAG
private float	tango_dir
private double	tangoVariance Apriori variance of Tango device position
private float[]	tmpRotA
private float[]	tmpRotMatrix
private float[]	transformMatrix
private TangoPositionOrientationProvider.TransformWorker	transformWorker
private java.lang.Thread	transformWorkerThread
private Transform	xform
private boolean	xformOk
private org.ejml.data.DMatrixRMaj	z_tango

Fields inherited from class no.kartverket.positionorientation.PositionOrientationProvider

originUpdateListeners

Constructor Summary

Constructors

Constructor and Description
TangoPositionOrientationProvider() Create and initialize

Method Summary

Modifier and Type	Method and Description
android.location.Location	getLocation() Get the current position of the device
OriginData	getOrigin()
float[]	getTransformationMatrix() Get the current rotation of the device in the rotation matrix format
Transform.Observation	handleLatLngHObservation(double latitude, double longitude, double height, float horizontal_accuracy, float vertical_accuracy) Handle an observation of the current 3D device position.
Transform.Observation	handleLatLngObservation(double latitude, double longitude, float accuracy) Handle an observation of the current 2D device position.
Transform.Observation	handleLocationObservation(android.location.Location location) Create a Transform.PositionObservation3D object from a GPS (or similar observation).
java.util.ArrayList<Transform.Observation>	handlePointCloudObservation(int numPoints, java.nio.FloatBuffer points, DTMGrid grid, float cloud_accuracy) Create a set of Transform.PointInTerrainObservation from a point cloud observation and a gridded terrain model.
void	handlePoseObservation(com.google.atap.tangoservice.TangoPoseData pose) Update the provider with the most recent pose (position and orientation) in the Tango coordinate system Typically called from Tango.TangoUpdateCallback.onPoseAvailable(TangoPoseData)
Transform.Observation	handleRotationVectorObservation(float[] rotationVector) Create a Transform.OrientationObservation from a rotationVector as given from a Rotation Vector Sensor.
void	onDestroy()
boolean	removeObservation(Transform.Observation obs) Remove an observation and initialize recompute of parameters
boolean	removeObservations(java.util.Collection<Transform.Observation> obs_set) Remove a set of observations and initialize recompute of parameters
void	reset() Initialize everything (when Tango initializes with a new origin)

Methods inherited from class no.kartverket.positionorientation.PositionOrientationProvider

addOriginUpdateListener, callOriginUpdateListeners, removeOriginUpdateListener

Methods inherited from class java.lang.Object

clone, equals, finalize, getClass, hashCode, notify, notifyAll, toString, wait, wait, wait

Field Detail

TAG

static final java.lang.String TAG

EarthRadius

static final double EarthRadius

See Also:

Constant Field Values

transformMatrix

private volatile float[] transformMatrix

numParameters

private static final int numParameters

See Also:

Constant Field Values

tangoVariance

private double tangoVariance

Apriori variance of Tango device position

prevCompTime

private long prevCompTime

Timestamp of previous kalman filter prediction

origin

private OriginData origin

geoidH_0

private double geoidH_0

z_tango

private org.ejml.data.DMatrixRMaj z_tango

R_tango

private org.ejml.data.DMatrixRMaj R_tango

H_tango

private org.ejml.data.DMatrixRMaj H_tango

kalmanFilter

private KalmanFilter kalmanFilter

Simple Kalman filter for smoothing Tango position information and computing speed, travel direction and predicted positions

observations

private java.util.concurrent.CopyOnWriteArrayList<Transform.Observation> observations

List of observations used in determining the transformation between TAngo and external coordiantes

declination

private float declination

compass_dir

private float compass_dir

tango_dir

private float tango_dir

compass_az

private float compass_az

xform

private Transform xform

xformOk

private volatile boolean xformOk

lastLocation

private volatile android.location.Location lastLocation

predictionMatrixUpdater

private TangoPositionOrientationProvider.PrivPredictionMatrixUpdater predictionMatrixUpdater

transformWorkerThread

private java.lang.Thread transformWorkerThread

transformWorker

private TangoPositionOrientationProvider.TransformWorker transformWorker

tmpRotA

private final float[] tmpRotA

tmpRotMatrix

private final float[] tmpRotMatrix

Constructor Detail

TangoPositionOrientationProvider

public TangoPositionOrientationProvider()

Create and initialize

Method Detail

onDestroy

public void onDestroy()

reset

public void reset()

Description copied from class: PositionOrientationProvider

Initialize everything (when Tango initializes with a new origin)

Specified by:

reset in class PositionOrientationProvider

getTransformationMatrix

public float[] getTransformationMatrix()

Description copied from class: PositionOrientationProvider

Get the current rotation of the device in the rotation matrix format

Specified by:

getTransformationMatrix in class PositionOrientationProvider

Returns:

float[16]

getLocation

public android.location.Location getLocation()

Description copied from class: PositionOrientationProvider

Get the current position of the device

Specified by:

getLocation in class PositionOrientationProvider

Returns:

location

getOrigin

public OriginData getOrigin()

handleLocationObservation

public Transform.Observation handleLocationObservation(android.location.Location location)

Create a Transform.PositionObservation3D object from a GPS (or similar observation). This method is typically called from a LocationListener registred to a LocationManager.

Parameters:

location - location data from GPS or similar location services

Returns:

the created Transform.PositionObservation3D object

handleLatLngObservation

public Transform.Observation handleLatLngObservation(double latitude,
                                                     double longitude,
                                                     float accuracy)

Handle an observation of the current 2D device position. Technically very similar to handleLocationObservation(Location). Can be used for manually adding a position from map

Parameters:

latitude - latitude of geograpic position, in degrees

longitude - longitude of geograpic position, in degrees

accuracy - apriori standard deviation of position, in meters

Returns:

the created Transform.PositionObservation2D object

handleLatLngHObservation

public Transform.Observation handleLatLngHObservation(double latitude,
                                                      double longitude,
                                                      double height,
                                                      float horizontal_accuracy,
                                                      float vertical_accuracy)

Handle an observation of the current 3D device position. Technically very similar to handleLocationObservation(Location). Can be used for manually adding a position from map, combined with an elevation observation from terrain model.

Parameters:

latitude - latitude of geograpic position, in degrees

longitude - longitude of geograpic position, in degrees

height - height of geograpic position, in meters

horizontal_accuracy - apriori standard deviation of horizontal position, in meters

vertical_accuracy - apriori standard deviation of height, in meters

Returns:

the created Transform.PositionObservation3D object

handleRotationVectorObservation

public Transform.Observation handleRotationVectorObservation(float[] rotationVector)

Create a Transform.OrientationObservation from a rotationVector as given from a Rotation Vector Sensor. This method is typically called from a SensorEventListener, where the rotationVector is read from a SensorEvent.

Parameters:

rotationVector - value from a SensorEvent

Returns:

the created Transform.OrientationObservation object

handlePoseObservation

public void handlePoseObservation(com.google.atap.tangoservice.TangoPoseData pose)

Update the provider with the most recent pose (position and orientation) in the Tango coordinate system

Typically called from Tango.TangoUpdateCallback.onPoseAvailable(TangoPoseData)

Parameters:

pose - the position orientation structure given from Tango

handlePointCloudObservation

public java.util.ArrayList<Transform.Observation> handlePointCloudObservation(int numPoints,
                                                                              java.nio.FloatBuffer points,
                                                                              DTMGrid grid,
                                                                              float cloud_accuracy)

Create a set of Transform.PointInTerrainObservation from a point cloud observation and a gridded terrain model.

Typically called from Tango.TangoUpdateCallback.onPointCloudAvailable(com.google.atap.tangoservice.TangoPointCloudData)

Parameters:

numPoints - the number of points

points - a FloatBuffer of point data, with stride 4 as points are stored in the buffer as x1, y1, z1, w1, x2, y2, z2, w2.... where w is a "reliability" measure (not used here)

grid - A gridded terrain model

cloud_accuracy - apriori standard deviation of a point to terrain model distance observation

removeObservation

public boolean removeObservation(Transform.Observation obs)

Remove an observation and initialize recompute of parameters

Parameters:

obs - the observation to remove

Returns:

true if the observation was in the observation list

removeObservations

public boolean removeObservations(java.util.Collection<Transform.Observation> obs_set)

Remove a set of observations and initialize recompute of parameters

Parameters:

obs_set - the list of observations to remove

Returns:

true if at least one of the argument observations was removed from the observation list