Relative Egomotion Workflow

Workflow

Initialization

To initialize motion model certain parameters, such as imu calibration or vehicle data, must be passed to the module. These parameters can be extracted from a dwRig module, see Rig Configuration Hence a typical initialization code sequence will look like this:

// read vehicle configuration
dwRig_initializeFromFile(&rig, sdkContext, "path/to/rig.json");
const dwVehicle *vehicle = nullptr;
dwRig_getVehicle(&vehicle, rig);
 
// get IMU sensor from the vehicle configuration
uint32_t imuIdx = 0;
dwRig_findSensorByName(&imuIdx, "imu.xsens", rig);
 
// setup motion model
dwEgomotionParameters params{};
 
params.motionModel                      = DW_EGOMOTION_IMU_ODOMETRY;
params.vehicle                          = *vehicle;
params.automaticUpdate                  = true;
 
params.sensorParameters.imuSamplingRate = 100.f;
 
params.autoupdate = true; // automatically update as appropriate on new sensor event
 
dwRig_getSensorToRigTransformation(&params.imu2rig, imuIdx, rig);
 
// initialize egomotion module
dwEgomotion_initialize(&egomotion, &params, sdkContext);    

Run-time filter update and motion estimation

At runtime the estimator has to be filled with input measurements. The following shows the sequence in a typical application using the Egomotion module:

const dwSensorEvent* sensorEvent = nullptr;
dwVehicleIOSafetyState vehicleIOSafetyState{};
dwVehicleIONonSafetyState vehicleIONonSafetyState{};
dwVehicleIOActuationFeedback vehicleIOActuationFeedback{};
 
while(dwSensorManager_acquireNextEvent(&sensorEvent, 0, sensorManager) == DW_SUCCESS)
{
    switch (sensorEvent->type)
    {
    case DW_SENSOR_CAN:
        dwVehicleIO_consumeCANFrame(sensorEvent->canFrame, 0, vehicleIO);
        dwVehicleIO_getVehicleSafetyState(&vehicleIOSafetyState, vehicleIO)
        dwVehicleIO_getVehicleNonSafetyState(&vehicleIONonSafetyState, vehicleIO)
        dwVehicleIO_getVehicleActuationFeedback(&vehicleIOActuationFeedback, vehicleIO)
        dwEgomotion_addVehicleIOState(&vehicleIOSafetyState, &vehicleIONonSafetyState, &vehicleIOActuationFeedback, egomotion);
    break;
 
    case DW_SENSOR_IMU:
        dwEgomotion_addIMUMeasurement(sensorEvent->imuFrame, egomotion);
    break;
 
    ...
 
    // query filter for a relative motion since last request
    dwTransformation3f motionLastToNow{};
    dwEgomotion_computeRelativeTransformation(&motionLastToNow, nullptr, getLastTime(), getCurrentTime(), egomotion);
    ...
 
    // query filter for the latest known absolute estimation
    dwEgomotionResult estimation{};
    dwEgomotion_getEstimation(&estimation, motion);
}

This workflow is demonstrated in the following sample: Egomotion Sample