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?