Odometry in Driveworks

Hello,

we’ve been trying to use the Egomotion module provided in Driveworks to compute Odometry and we wrote a simple test for it. The test consists of completing a circle, thus the steering angle is constant, speed is constant, time growth between individual odometry entries is constant. The code goes roughly like this:

const auto start = getTime();
auto lastTime = start;
auto circumference = static_cast<float>(2 * M_PI * turningRadius);    
float deltams = 1000.0f / fps;
dwTransformation lastTF = DW_IDENTITY_TRANSFORMATION;
float angle = steeringAngleFromRadius(turningRadius, wheelbase);
for (int i = 0 ; i <= (circumference * fps) / speed; i++) {
    auto now = toDW(start + timeFromMilliseconds(deltams * i));    
    dwEgomotion_addOdometry(DW_EGOMOTION_MEASURMENT_STEERINGANGLE, angle, now, egomotion_);
    dwEgomotion_addOdometry(DW_EGOMOTION_MEASURMENT_VELOCITY, speed, now, egomotion_);
    dwEgomotion_update(now, egomotion_);
    dwTransformation nowTf;
    dwEgomotion_computeRelativeTransformation(&nowTf, nullptr, lastTime, now, egomotion_);
    lastTime = now;
    dwTransformation newTf;
    dwEgomotion_applyRelativeTransformation(&newTf, &nowTf, &lastTF);
    lastTF = newTf;
    if (i >= (circumference / 4. * fps / speed) - 1 && i <= (circumference / 4. * fps / speed) + 1) {
            EXPECT_NEAR(newTf.array[12] * 100, turningRadius * 100, cmMaxDiff);
            EXPECT_NEAR(newTf.array[13] * 100, turningRadius * 100, cmMaxDiff);
    }
    // ... checks for circ/2, 3circ/4, full circ
}

We even tried using dwEgomotionParamaters directly from the samples, but the results are completely off (up to 18 meters on a circle with 65m radius at only 5 m/s). Is there something wrong with the way we’re trying to use the Egomotion module?

Here’s an image comparing an actual circle (green) with odometry results (red). Radius is 50 meters, frequency 100Hz, speed 10 m/s. https://imgur.com/a/TNy5B81