Detailed Description

Provides vehicle egomotion functionality.

The egomotion module provides multiple implementation of different motion models. Starting from a simple Ackerman-based odometry only model, to a full fledged fusion of inertial as well as visual sensor information.

This module provides access to a history of motion data with a predefined cadence and length. At any point of time an access into the history can be made to retrieve previous result. The access into the history is timestamp based. If an access falls between two history entries it will be interpolated.

In addition to history based access, any motion model implementation enables prediction of the motion into the future.

Data Structures
struct	dwEgoMotionParameters
	Holds initialization parameters for the EgoMotion module. More...

struct	dwEgomotionResult
	Holds egomotion results that contain multiple fields. More...

struct	dwEgoMotionSensorCharacteristics
	Sensor measurement noise characteristics for the individual used sensors. More...

struct	dwEgomotionUncertainty
	Holds egomotion result uncertainties. More...

Typedefs
typedef struct dwEgomotionObject *	dwEgomotionHandle_t

Enumerations
enum	dwEgomotionDataField { DW_EGOMOTION_ROLL = 1 << 3, DW_EGOMOTION_PITCH = 1 << 4, DW_EGOMOTION_YAW = 1 << 5, DW_EGOMOTION_VEL_X = 1 << 6, DW_EGOMOTION_VEL_Y = 1 << 7, DW_EGOMOTION_VEL_Z = 1 << 8, DW_EGOMOTION_ACC_X = 1 << 9, DW_EGOMOTION_ACC_Y = 1 << 10, DW_EGOMOTION_ACC_Z = 1 << 11, DW_EGOMOTION_GYR_X = 1 << 12, DW_EGOMOTION_GYR_Y = 1 << 13, DW_EGOMOTION_GYR_Z = 1 << 14, DW_EGOMOTION_GPS_LAT = 1 << 15, DW_EGOMOTION_GPS_LON = 1 << 16, DW_EGOMOTION_GPS_ALT = 1 << 17 }
	Defines flags that indicate valid data fields. More...

enum	dwMotionModel { DW_EGOMOTION_ODOMETRY = 1 << 0, DW_EGOMOTION_IMU_ODOMETRY = 1 << 1 \| DW_EGOMOTION_ODOMETRY, DW_EGOMOTION_IMU_ODOMETRY_GPS = 1 << 2 \| DW_EGOMOTION_IMU_ODOMETRY }
	Defines the egomotion models. More...

enum	dwMotionModelMeasurement { DW_EGOMOTION_MEASURMENT_VELOCITY = 0, DW_EGOMOTION_MEASURMENT_STEERINGANGLE = 1 }
	Defines motion measurements. More...

Functions
DW_API_PUBLIC dwStatus	dwEgomotion_addGPSMeasurement (const dwGPSFrame *location, dwEgomotionHandle_t obj)
	Adds location measurement to the egomotion module. More...

DW_API_PUBLIC dwStatus	dwEgomotion_addIMUMeasurement (const dwIMUFrame *imu, dwEgomotionHandle_t obj)
	Adds an inertial motion unit measurement to the egomotion module. More...

DW_API_PUBLIC dwStatus	dwEgomotion_addOdometry (dwMotionModelMeasurement measuredType, float32_t measuredValue, dwTime_t timestamp_us, dwEgomotionHandle_t obj)
	Notifies the egomotion module of a new odometry measurement. More...

DW_API_PUBLIC dwStatus	dwEgomotion_applyRelativeTransformation (dwTransformation newVehicle2World, const dwTransformation poseOld2New, const dwTransformation *oldVehicle2World)
	Applies the estimated relative motion as returned by dwEgomotion_computeRelativeTransformation() to a given vehicle pose. More...

DW_API_PUBLIC dwStatus	dwEgomotion_computeRelativeTransformation (dwTransformation *poseAtoB, dwTime_t timestamp_a, dwTime_t timestamp_b, dwEgomotionHandle_t obj)
	Computes the relative pose between two timestamps in the past. More...

DW_API_PUBLIC dwStatus	dwEgomotion_estimate (dwEgomotionResult pose, dwEgomotionUncertainty sigma, dwTime_t timestamp_us, dwEgomotionHandle_t obj)
	Runs the motion model estimation for a given timestamp and predicts motion as it would happen for the provided timestamp. More...

DW_API_PUBLIC dwStatus	dwEgomotion_getEstimation (dwEgomotionResult *result, dwEgomotionHandle_t obj)
	Gets the current estimated pose. More...

DW_API_PUBLIC dwStatus	dwEgomotion_getEstimationTimestamp (dwTime_t *timestamp_us, dwEgomotionHandle_t obj)
	Gets the timestamp of the available estimation result. More...

DW_API_PUBLIC dwStatus	dwEgomotion_getHistoryEntry (const dwEgomotionResult pose, const dwEgomotionUncertainty sigma, dwTime_t *timestamp_us, size_t index, dwEgomotionHandle_t obj)
	Returns an entry from the motion history that is currently available. More...

DW_API_PUBLIC dwStatus	dwEgomotion_getHistorySize (size_t *num, dwEgomotionHandle_t obj)
	Returns the number of elements currently stored in the motion history. More...

DW_API_PUBLIC dwStatus	dwEgomotion_getMotionModel (dwMotionModel *model, dwEgomotionHandle_t obj)
	Returns the type of the motion model used. More...

DW_API_PUBLIC dwStatus	dwEgomotion_getUncertainty (dwEgomotionUncertainty *result, dwEgomotionHandle_t obj)
	Gets the current estimation uncertainties. More...

DW_API_PUBLIC dwStatus	dwEgomotion_initialize (dwEgomotionHandle_t obj, const dwEgoMotionParameters params, dwContextHandle_t ctx)
	Initializes the Egomotion module. More...

DW_API_PUBLIC dwStatus	dwEgomotion_release (dwEgomotionHandle_t *obj)
	Releases the egomotion module. More...

DW_API_PUBLIC dwStatus	dwEgomotion_reset (dwEgomotionHandle_t obj)
	Resets the egomotion module. More...

DW_API_PUBLIC dwStatus	dwEgomotion_update (dwTime_t timestamp_us, dwEgomotionHandle_t obj)
	Runs the motion model estimation for a given timestamp. More...

Data Structure Documentation

◆ dwEgoMotionParameters

struct dwEgoMotionParameters

Data Fields
dwTime_t	estimationPeriod	Auto update state estimation after a predefined period of time in [usec]. In general to update current estimations a call to `dwEgomotion_update()` is required. When `estimationPeriod != 0` an autoupdate of the estimation happens based on the latest sensor measurement timestamps. If a timestamp of any new measurement is higher than last timestamp plus the predefined period an automatic update of the internal state happens.
float32_t	gyroscopeBias[3]	If a bias is known at initialization time, it can be set here. In general a bias will be estimated. by the module while the filter is running. It is however very beneficial if the filter can be initialized. with already known bias. If not know, set to 0.
uint32_t	historySize	Number of motion results to keep in the history (if 0 specified default of 1000 is used). Any call to `dwEgomotion_update()` adds a new entry into the history. Cadence of the calls to update defines cadence of the history.
dwTransformation	imu2rig	Extrinsics from the IMU coordinate system to the vehicle rig coordinate system.
dwMotionModel	motionModel	Specifies the motion model to be used for pose prediction, for example, Ackermann principle based.
dwEgoMotionSensorCharacteristics	sensorNoise	Pass a sensor noise struct containing all information about sensor noise. If the struct is zero initialized a default assumption about sensor noise are made (see `dwEgoMotionSensorCharacteristics`)
float32_t	wheelBase	Wheel base of the vehicle.

◆ dwEgomotionResult

struct dwEgomotionResult

Data Fields
float32_t	angularVelocity[3]	rotation speed in body frame measured in [rad/s].
float32_t	linearVelocity[3]	Linear speed in body frame measured in [m/s]. Note When driving straight it will be pointing towards x
float64_t	location[3]	lon,lat,alt in [WGS-84].
float32_t	rotation[4]	Rotation represented as quaternion (x,y,z,w) in navigation frame measured in [rad].
uint32_t	validFlags	Bitwise combination of dwEgomotionDataField flags.

◆ dwEgoMotionSensorCharacteristics

struct dwEgoMotionSensorCharacteristics

Data Fields
float32_t	gyroNoiseDensityDeg	Expected zero mean measurement noise of the gyroscope, also known as Noise Density [deg/s/sqrt(Hz)] A default value of 0.05 [deg/s/sqrt(Hz)] will be assumed if no parameter, i.e. 0 or nan, passed
float32_t	imuSamplingRateHz	If known this entry shall indicate expected sampling rate in [Hz] of the imu sensor. A default value of 100Hz is used if no parameter passed

◆ dwEgomotionUncertainty

struct dwEgomotionUncertainty

Data Fields
float32_t	angularVelocity[3]	in body frame [rad/s]
float32_t	location[3]	[m]
float32_t	rotation[3]	in navigation frame [rad]
uint32_t	validFlags
float32_t	velocity[3]	in navigation frame [m/s]

Typedef Documentation

◆ dwEgomotionHandle_t

typedef struct dwEgomotionObject* dwEgomotionHandle_t

Definition at line 72 of file Egomotion.h.

Enumeration Type Documentation

◆ dwEgomotionDataField

enum dwEgomotionDataField

Defines flags that indicate valid data fields.

Note: Even if rotation is represented as a quaternion, it might still not be fully estimated. These flags define what part of the rotation is available.

Enumerator
DW_EGOMOTION_ROLL	rotation around x axis.
DW_EGOMOTION_PITCH	rotation around y axis.
DW_EGOMOTION_YAW	rotation around z-axis.
DW_EGOMOTION_VEL_X
DW_EGOMOTION_VEL_Y
DW_EGOMOTION_VEL_Z
DW_EGOMOTION_ACC_X
DW_EGOMOTION_ACC_Y
DW_EGOMOTION_ACC_Z
DW_EGOMOTION_GYR_X
DW_EGOMOTION_GYR_Y
DW_EGOMOTION_GYR_Z
DW_EGOMOTION_GPS_LAT
DW_EGOMOTION_GPS_LON
DW_EGOMOTION_GPS_ALT

Definition at line 162 of file Egomotion.h.

◆ dwMotionModel

enum dwMotionModel

Defines the egomotion models.

Enumerator

DW_EGOMOTION_ODOMETRY

Valid measurements are velocity and steering angle.

DW_EGOMOTION_IMU_ODOMETRY

Combines IMU and ackerman motion model to estimate egomotion.

DW_EGOMOTION_IMU_ODOMETRY_GPS

Combines IMU and Ackerman motion model to estimate egomotion as well as GPS location.

No correction of the motion by GPS is happening, however GPS location is a fused and filtered version of raw GPS measurements corrected by the IMU and odometry.

Definition at line 75 of file Egomotion.h.

◆ dwMotionModelMeasurement

enum dwMotionModelMeasurement

Defines motion measurements.

Enumerator
DW_EGOMOTION_MEASURMENT_VELOCITY	Car velocity [m/s].
DW_EGOMOTION_MEASURMENT_STEERINGANGLE	Steering angle [rad].

Definition at line 98 of file Egomotion.h.

Function Documentation

◆ dwEgomotion_addGPSMeasurement()

DW_API_PUBLIC dwStatus dwEgomotion_addGPSMeasurement	(	const dwGPSFrame *	location,
		dwEgomotionHandle_t	obj
	)

Adds location measurement to the egomotion module.

Parameters

[in]	location	GPS measurement providing current location.
[in]	obj	Egomotion handle.

Returns: DW_INVALID_ARGUMENTS - if given arguments are invalid
DW_INVALID_HANDLE - if given handle is invalid
DW_NOT_SUPPORTED - if given measurement is not supported by the egomotion model.
DW_SUCCESS

◆ dwEgomotion_addIMUMeasurement()

DW_API_PUBLIC dwStatus dwEgomotion_addIMUMeasurement	(	const dwIMUFrame *	imu,
		dwEgomotionHandle_t	obj
	)

Adds an inertial motion unit measurement to the egomotion module.

Parameters

[in]	imu	IMU measurement.
[in]	obj	Egomotion handle.

Returns: DW_INVALID_ARGUMENTS - if the given arguments are invalid
DW_INVALID_HANDLE - if the given handle is invalid
DW_NOT_SUPPORTED - if the given measurement is not supported by the egomotion model.
DW_SUCCESS

◆ dwEgomotion_addOdometry()

DW_API_PUBLIC dwStatus dwEgomotion_addOdometry	(	dwMotionModelMeasurement	measuredType,
		float32_t	measuredValue,
		dwTime_t	timestamp_us,
		dwEgomotionHandle_t	obj
	)

Notifies the egomotion module of a new odometry measurement.

The timestamp must not be before the last measurement.

Parameters

[in]	measuredType	Type of what was measured. For example: velocity, steering angle.
[in]	measuredValue	Value that was measured. For example: 3.6 m/s, 0.1 rad.
[in]	timestamp_us	Timestamp for the pose.
[in]	obj	Egomotion handle.

Returns: DW_INVALID_HANDLE - if provided, the egomotion handle is invalid.
DW_INVALID_ARGUMENT - if timestamp_us is before last measurement or measuredType is not valid.
DW_NOT_SUPPORTED - if odometry measurements are not supported by the egomotion model.
DW_SUCCESS

◆ dwEgomotion_applyRelativeTransformation()

DW_API_PUBLIC dwStatus dwEgomotion_applyRelativeTransformation	(	dwTransformation *	newVehicle2World,
		const dwTransformation *	poseOld2New,
		const dwTransformation *	oldVehicle2World
	)

Applies the estimated relative motion as returned by dwEgomotion_computeRelativeTransformation() to a given vehicle pose.

Parameters

[out]	newVehicle2World	Transformation representing new pose of a vehicle after applying the relative motion.
[in]	poseOld2New	Relative motion between two timestamps.
[in]	oldVehicle2World	Current pose of a vehicle.

Returns: DW_INVALID_ARGUMENT - if any of the given arguments is nullptr
DW_SUCCESS

◆ dwEgomotion_computeRelativeTransformation()

DW_API_PUBLIC dwStatus dwEgomotion_computeRelativeTransformation	(	dwTransformation *	poseAtoB,
		dwTime_t	timestamp_a,
		dwTime_t	timestamp_b,
		dwEgomotionHandle_t	obj
	)

Computes the relative pose between two timestamps in the past.

This is the same as calling dwEgomotion_estimate() twice and then computing the relative motion between them.

Parameters

[out]	poseAtoB	Transformation mapping a point at time `a` to a point at time `b`.
[in]	timestamp_a	Timestamp of first motion event.
[in]	timestamp_b	Timestamp of second motion event.
[in]	obj	Egomotion handle.

Returns: DW_INVALID_HANDLE - if provided, the egomotion handle is invalid.
DW_NOT_AVAILABLE - if there is currently not enough data available to make a prediction or requested timestamp is too far into the past
DW_INVALID_ARGUMENT - if provided pose is nullptr or one of the timestamps is in the future
DW_SUCCESS

◆ dwEgomotion_estimate()

DW_API_PUBLIC dwStatus dwEgomotion_estimate	(	dwEgomotionResult *	pose,
		dwEgomotionUncertainty *	sigma,
		dwTime_t	timestamp_us,
		dwEgomotionHandle_t	obj
	)

Runs the motion model estimation for a given timestamp and predicts motion as it would happen for the provided timestamp.

This method does not modify internal state and can only be used to extrapolate motion into the future or interpolate motion from the past.

Parameters

[out]	pose	Struct where to store predicted pose.
[out]	sigma	Struct where to store uncertainties of the prediction. Can be nullptr.
[in]	timestamp_us	Timestamp to predict pose for.
[in]	obj	Egomotion handle.

Returns: DW_INVALID_HANDLE - if provided, the egomotion handle is invalid.
DW_NOT_AVAILABLE - if there is currently not enough data available to make a prediction or requested timestamp is too far into the past
DW_INVALID_ARGUMENT - if provided result is nullptr or timestamp is not in the future
DW_SUCCESS

Note: Interpolation into the past can only happen within the usable history range.

◆ dwEgomotion_getEstimation()

DW_API_PUBLIC dwStatus dwEgomotion_getEstimation	(	dwEgomotionResult *	result,
		dwEgomotionHandle_t	obj
	)

Gets the current estimated pose.

Parameters

[out]	result	A pointer to the collection of data field of the last estimate call.
[in]	obj	Egomotion handle.

Returns: DW_INVALID_HANDLE - if provided, the egomotion handle is invalid.
DW_NOT_AVAILABLE - if there is currently no estimation available
DW_SUCCESS

◆ dwEgomotion_getEstimationTimestamp()

DW_API_PUBLIC dwStatus dwEgomotion_getEstimationTimestamp	(	dwTime_t *	timestamp_us,
		dwEgomotionHandle_t	obj
	)

Gets the timestamp of the available estimation result.

The timestamp will be updated after each dwEgomotion_update() call.

Parameters

[out]	timestamp_us	Pointer to be filled with timestamp in [usec] of availabel motion result.
[in]	obj	Egomotion handle.

Returns: DW_INVALID_HANDLE - if provided, the egomotion handle is invalid.
DW_NOT_AVAILABLE - if there is currently no estimation available
DW_SUCCESS

◆ dwEgomotion_getHistoryEntry()

DW_API_PUBLIC dwStatus dwEgomotion_getHistoryEntry	(	const dwEgomotionResult **	pose,
		const dwEgomotionUncertainty **	sigma,
		dwTime_t *	timestamp_us,
		size_t	index,
		dwEgomotionHandle_t	obj
	)

Returns an entry from the motion history that is currently available.

Parameters

[out]	pose	Return pose of the estimation motion at the requested index.
[out]	sigma	Return uncertainty of the estimation pose at the requested index.
[out]	timestamp_us	Return timestamp in [usec] of the estimated pose at the requested index.
[in]	index	Index into the history, in the range [0; `dwEgomotion_getHistorySize`], with 0 being current entry and last element pointing into most aged entry.
[in]	obj	Egomotion handle.

Returns: DW_INVALID_HANDLE - if provided, the egomotion handle is invalid.
DW_INVALID_ARGUMENT - if provided num is nullptr
DW_SUCCESS

◆ dwEgomotion_getHistorySize()

DW_API_PUBLIC dwStatus dwEgomotion_getHistorySize	(	size_t *	num,
		dwEgomotionHandle_t	obj
	)

Returns the number of elements currently stored in the motion history.

Parameters

[out]	num	A pointer to the number of elements in the history.
[in]	obj	Egomotion handle.

Returns: DW_INVALID_HANDLE - if provided, the egomotion handle is invalid.
DW_INVALID_ARGUMENT - if provided num is nullptr
DW_SUCCESS

◆ dwEgomotion_getMotionModel()

DW_API_PUBLIC dwStatus dwEgomotion_getMotionModel	(	dwMotionModel *	model,
		dwEgomotionHandle_t	obj
	)

Returns the type of the motion model used.

Parameters

[out]	model	Type of the motion model which is used by the instance specified by the handle.
[in]	obj	Egomotion handle.

Returns: DW_INVALID_HANDLE - if provided, the egomotion handle is invalid.
DW_INVALID_ARGUMENT - if provided model is nullptr
DW_SUCCESS

◆ dwEgomotion_getUncertainty()

DW_API_PUBLIC dwStatus dwEgomotion_getUncertainty	(	dwEgomotionUncertainty *	result,
		dwEgomotionHandle_t	obj
	)

Gets the current estimation uncertainties.

Parameters

[out]	result	A pointer to the collection of uncertainties of the last estimated results.
[in]	obj	Egomotion handle.

Returns: DW_INVALID_HANDLE - if provided, the egomotion handle is invalid.
DW_NOT_AVAILABLE - if there is currently no estimation available
DW_SUCCESS

Note: Not all values do have valid uncertainties. Check dwEgomotionUncertainty.validFlags.

◆ dwEgomotion_initialize()

DW_API_PUBLIC dwStatus dwEgomotion_initialize	(	dwEgomotionHandle_t *	obj,
		const dwEgoMotionParameters *	params,
		dwContextHandle_t	ctx
	)

Initializes the Egomotion module.

Parameters

[out]	obj	A pointer to the egomotion handle for the created module.
[in]	ctx	Specifies the handler to the context under which the Egomotion module is created.
[in]	params	A pointer to the configuration parameteres of the module.

Returns: DW_BAD_CAST - if the provided context handle is invalid.
DW_INVALID_ARGUMENT - if provided parameters are invalid
DW_SUCCESS

◆ dwEgomotion_release()

DW_API_PUBLIC dwStatus dwEgomotion_release ( dwEgomotionHandle_t * obj )

Releases the egomotion module.

Note: This method renders the handle unusable.

Parameters

[in] obj Egomotion handle to be released.

Returns: DW_INVALID_HANDLE - if the provided egomotion handle is invalid.
DW_SUCCESS

◆ dwEgomotion_reset()

DW_API_PUBLIC dwStatus dwEgomotion_reset ( dwEgomotionHandle_t obj )

Resets the egomotion module.

This method resets the pose and any state. All consecutive pose update will start from the origin.

Parameters

[in] obj Egomotion handle to be released.

Returns: DW_INVALID_HANDLE - if the provided egomotion handle is invalid.
DW_SUCCESS

◆ dwEgomotion_update()

DW_API_PUBLIC dwStatus dwEgomotion_update	(	dwTime_t	timestamp_us,
		dwEgomotionHandle_t	obj
	)

Runs the motion model estimation for a given timestamp.

The internal state is modified. The motion model advances to the given timestamp. To retrieve the result of the estimation, use dwEgomotion_getEstimation().

Parameters

[in]	timestamp_us	Time in the future to estimate motion for. The provided timestamp must be larger then the last one used in dwEgomotion_estimate().
[in]	obj	Egomotion handle.

Returns: DW_INVALID_HANDLE - if provided the egomotion handle is invalid.
DW_INVALID_ARGUMENT - if given timestamp is not strictly monotonically increasing
DW_SUCCESS

Note: Any consecutive call to this method must provide a monotonically increasing timestamp.; A full set of measurements must be added before calling this method. Doing so helps to initialize the internal estimation.; Updating internal state does not guarantee that a motion estimation is available, i.e. there might be not enough data to provide proper prediction. Use dwEgomotion_getEstimation() and/or dwEgomotion_computeRelativeTransformation() to query estimation if it is available.

DriveWorks SDK Reference

| 0.6.67 Release

Detailed Description

Data Structures

Typedefs

Enumerations

Functions

Data Structure Documentation

◆ dwEgoMotionParameters

◆ dwEgomotionResult

◆ dwEgoMotionSensorCharacteristics

◆ dwEgomotionUncertainty

Typedef Documentation

◆ dwEgomotionHandle_t

Enumeration Type Documentation

◆ dwEgomotionDataField

◆ dwMotionModel

◆ dwMotionModelMeasurement

Function Documentation

◆ dwEgomotion_addGPSMeasurement()

◆ dwEgomotion_addIMUMeasurement()

◆ dwEgomotion_addOdometry()

◆ dwEgomotion_applyRelativeTransformation()

◆ dwEgomotion_computeRelativeTransformation()

◆ dwEgomotion_estimate()

◆ dwEgomotion_getEstimation()

◆ dwEgomotion_getEstimationTimestamp()

◆ dwEgomotion_getHistoryEntry()

◆ dwEgomotion_getHistorySize()

◆ dwEgomotion_getMotionModel()

◆ dwEgomotion_getUncertainty()

◆ dwEgomotion_initialize()

◆ dwEgomotion_release()

◆ dwEgomotion_reset()

◆ dwEgomotion_update()