Odometry in Driveworks

Autonomous Vehicles DRIVE PX 2 DriveWorks
1

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?

2

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