Hello,
I have valid odometry, GPS and IMU data. Odometry consists of all 4 wheel speeds and steering angle. IMU has acceleration and rotate rates in all axis. GPS contains lon, lat, alt info. All have valid timestamps.
I have tried following egomotion sample but I always get not available when getting estimation.
Where should I look to figure this out? What parts of egomotion need more focus?
Hi marek.piirikivi, What’s the messages from running the sample? Thanks!
Hi,
I am not trying to run sample, I am trying to use driveworks API to achieve similar to sample. I have read the documentation, but it appears that there is some lack of information about what is required to get estimation from odometry/IMU/GPS.
Please check if the information about dwMotionModel enumeration in DriveWorks SDK Reference Documentation is what you’re looking for. Thanks!
It appears to be DW_EGOMOTION_IMU_ODOMETRY with estimateInitialOrientation = true, automaticUpdate = true.
Oh, I see that there are a lot of details defined under that motion model about all the requirements. Are these all valid and should be strictly followed?
I am thinking whether throttle info as an example is strictly mandatory to have?
If doc states, I guess it’s necessary. You can check with the sample. Its default recording data should be under /usr/local/driveworks/data/samples/recordings/suburb0.
I got it working to a state where when asking dwGlobalEgomotion_computeEstimate for timestamp got by dwGlobalEgomotion_getTimestamp, I am getting estimation with validPosition = true, but position.lon|lat|height remain unchanged (and values describe first dwGlobalEgomotion_addGPSMeasurement). I have confirmed that all the GPS measurements are different (it’s pre-recorded track of vehicle driving and GPS coordinates draw out valid map). I also checked timestamps of GPS frames. All are valid.
Looking at global estimation result, timestamp is changing but values are not
What could be the reason why estimate does not change?
Could you help check the return values of dwGlobalEgomotion_computeEstimate() calls? Thanks!
It returns DW_SUCCESS. I am trying to construct minimal viable code to get global estimations.
Should this flow work as expected?
Please take source under /usr/local/driveworks/samples/src/egomotion on your host as a baseline for your implementation. If you can provide a patch on it for us to reproduce your issue, it will be easier to look into your it. Thanks!
I will provide you with smashed together version of what I have with sample data. I was unable to access data server at weekend, but I was able to somewhat re-generate data.bin files from existing snippets of text log (as a result GPS hacc and vacc were hardcoded to 0.3m).
There might be some minor compilation issues (missing driveworks includes etc. - should be easy to get it working).
Feel free to add logging wherever you please to investigate further. Data contains 2 .bin files (gps and odometry) and a rig file. All the timestamps are relative (starting from 0).
I also added a picture describing program flow
odo_gsp_egomotion.zip (29.4 KB)
I think I caught something… my relative transformation between 2 legitimate timestamps contains nan translate variables (in transformation matrix positions 12, 13, 14). Rotation part of transformation matrix is present and appears to be valid.
It is weird that DW_SUCCESS is returned in that instance (when in reality, resulted transformation matrix is unusable when containind nan value).
Update: I got estimation transformation correct with simulated odometry data. It appears that there might have problems with live recorded vehicle data.
How did you get the transformation matrix? Is this anything to do with DriveWorks APIs?
May I know which DriveWorks API you are talking about?
Thanks!
There is a function dwEgomotion_computeRelativeTransformation that I used to get transformation matrix.
I was actually able to get relative estimation working.
The problem was about having weird odometry data point that screw up estimation. As soon as I filtered that out, the problem with relative egomotion was gone.
I am still unable to addRelativeMotion to global egomotion. I am always getting NOT AVAILABLE.
After getting uncertainty flags correct,
I keep on getting DW_NOT_READY when calling dwGlobalEgomotion_addRelativeMotion. According to documentation: “no new estimate was added to history; filter missing GPS data or estimate timestamp isn’t more recent than current estimate timestamp.”
I checked timestamps. As an example:
dwEgomotion_getEstimate result contained timestamp: 1582879224332135
dwGlobalEgomotion_getTimestamp result contained: 1582879224332135
and dwContext_getCurrentTime result contained: 1582879224380151
I know by fact that GPS data should be present, because dwGlobalEgomotion_addGPSMeasurement is called multiple times with success before that.
It appears that all the conditions are met, but I might look wrong timestamps there.
What else should I check? Maybe you can look at the source code of dwGlobalEgomotion_addRelativeMotion and see what conditions it checks there? Or ask someone who can?
I found below comments in dwGlobalEgomotion_addRelativeMotion() implementation.
Please make sure your sesnors data from a moving car. Thanks!
// GPS measurements drift quite a lot when vehicle is standing still
// Only incorporate measurements while vehicle is moving
Thank you for your reply.
Does that mean that I should check whether vehicle is moving before dwGlobalEgomotion_addGPSMeasurement call to ensure that no invalid gps is included?
Which makes sense. For now, I have implemented my own global estimation and fusion with relative motion. And I also had a lot of problems when estimating heading when vehicle was stationary.
Does dwGlobalEgomotion_addGPSMeasurement() call is before the first dwGlobalEgomotion_addRelativeMotion() call?
Do you get DW_NOT_READY even from the very first dwGlobalEgomotion_addRelativeMotion() call?
Please check egomotion sample API call sequence and mimic it on your application.
Hello,
I decided to retry global estimation with imu/odometry and gps data again.
I got everything working, but estimation result drifts.
In simulator, no IMU forces (other than -9.81 in z-direction is applied). Vehicle is driving with constant 25 km/h in straight line. GPS measurements are added every 100 ms and IMU and odometry every 10 ms. I am using front speed mode and supplying front speed and steering angle (which is 0).
In 10 seconds of simulation time, vehicle has drifted about 5 meters off from straight path.
Do you have any ideas what might cause this?