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;
dwVehicleIOState vehicleIOState{};
while(dwSensorManager_acquireNextEvent(&sensorEvent, 0, sensorManager) == DW_SUCCESS)
{
    switch (sensorEvent->type)
    {
    case DW_SENSOR_CAN:
        dwVehicleIO_consumeCANFrame(sensorEvent->canFrame, 0, vehicleIO);
        dwVehicleIO_getVehicleState(&vehicleIOState, vehicleIO);
        dwEgomotion_addVehicleState(&vehicleIOState, 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