ethzasl_sensor_fusion tutorial is not working properly

[amajan]

Forwarded communication from Gmail

From: Ahmed D. Hunzai
Sent: Monday, January 21, 2013 12:14 AM
To: Weiss Stephan; Achtelik Markus

Hi,

Thanks for sharing framework and such nice tutorials, I am intended to use ethzasl_sensor_fusion package to integrate my 6dof visual odometry and IMU. I have tried the tutorial and got it working, but have a problem in results (ssf_core/state_out etc), mostly it is showing nothing on rxplot even after fallowing all instructions in tutorial, some times it shows good results for few seconds are then giving error, as given below and screenshot of the rxgraph is attached.

My second question is regarding using package with my visual odoemtry and IMU, initially i will use with fixed covariance. Please confirm if following steps are correct ( uisng viconpose_ example given in second tutorial)

• remap visual odometry and IMU reading to ssf_core/viconpose_measurement and ssf_core/imu_state_input respectively.
• Change the measurementCallback in viconpos_sensor.cpp file according to Visual Odometry Message
• adjust values in .yaml file.

third question is, how pose_sensor can be used with 6dof visual odometry?

can you please share code that is running correctly with the dateset given in tutorials?

---

subscribed to topics:
/auk/fcu/imu
/ssf_core/hl_state_input
/vicon/auk/auk
advertised topics:
/rosout
/ssf_core/state_out
/ssf_core/correction
/ssf_core/pose
/ssf_core/ext_state
/viconpos_sensor/parameter_descriptions
/viconpos_sensor/parameter_updates

[ INFO] [1358713846.549348267]: filter initialized to:
position: [0, 1.42598e-311, -6.19637e-42]
scale:1
attitude (w,x,y,z): [-3.58916e-45, -1.6005e-41, -4.27156e-42, -1.70691e-41
p_ci: [0, 0, 0
q_ci: (w,x,y,z): [1, 0, 0, 0]
[ WARN] [1358713866.497728980]: large time-gap re-initializing to last state

=== ERROR === update: NAN at index 0
=== ERROR === update: NAN at index 0
=== ERROR === prediction p: NAN at index 0
=== ERROR === update: NAN at index 0
......

---

Thanks in advance.

cheers,
Hunzai

------------------------------- ==== Answer ==== ---------------------------------
from: Weiss Stephan

Dear Hunzai,

Thank you for the feedback and for using our framework. If the rxplots do not show anything but the time-line still advances in the plot this means that the filter throws NANs somewhere. Code and tutorial should work - unless an error slipped in when I was testing it.

1. NAN errors: your attitude is initialized to [0 0 0 0] which should be a quaternion of rotation (i.e. norm=1). Please check if the initialization file viconpos_measurements.h correctly initializes the attitude quaternion q. If you think it is, then please add the following output to the ROS_INFO_STREAM in viconpos_measurements.h:

        "q_cv: (w,x,y,z): [" << q_cv_.w() << ", " << q_cv_.x() << ", " << q_cv_.y() << ", " << q_cv_.z() << "]" << std::endl <<
        "q_wv: (w,x,y,z): [" << q_wv_.w() << ", " << q_wv_.x() << ", " << q_wv_.y() << ", " << q_wv_.z()  << "]"<< std::endl <<

and send me the output.

2. Using your VO/IMU: the general steps you describe are correct. However, I think your VO computes a 6DoF pose, whereas the tutorial is made for a 3DoF position. Therefor, you may actually want to do similar steps as described in the tutorial but using the pose_sensor files as a basis (instead of the position_sensor files as in the tutorial). Keep in mind that the values you should change in the *_fix.yaml file are the camera-imu calibration parameters (q_ci, p_ci), the delay and the noise of your VO. If the filter does not do what you expect, usually q_ci is not set correctly. The noise parameters should be computed once and then kept fix.

3. using VO as 6DoF: see in 2) the pose (=6DoF) sensor module essentially provides everything you need if I am not mistaken.

Please let me know if the above helps and how things are going.

Best
Stephan

PS: Do you mind posting this conversation as an issue on github? This way, other users have access to the information as well.

[stephanweiss]

------------------------------- ==== Follow up from Hunzai ==== ---------------------------------

Dear Stephan!

thanks for reply, Now first tutorial code (position_ for 3DoF global position ) using dataset is working properly, i had mistakenly commented a "take fix covariance" code in viconpos_sensor.cpp. But second tutorial code( for 6Dof ) is still throwing the NAN, output (q_cv_ and q_wv ) is as fallows:

..........................................................................
[ WARN] [1358804676.143570934]: No measurements received yet to initialize position - using [0 0 0]
[ WARN] [1358804676.143689352]: No measurements received yet to initialize attitude - using [1 0 0 0]
[ INFO] [1358804676.145956192]: q_cv: (w,x,y,z): [1, 0, 0, 0]
q_wv: (w,x,y,z): [1, 0, 0, 0]

[ INFO] [1358804676.146102699]: filter initialized to:
position: [0, 0, 0]
scale:1
attitude (w,x,y,z): [1, -0, -0, -0
p_ci: [0, 0, 0
q_ci: (w,x,y,z): [1, 0, 0, 0]
[ WARN] [1358804678.177443649]: large time-gap re-initializing to last state

=== ERROR === update: NAN at index 0
=== ERROR === update: NAN at index 0
=== ERROR === prediction p: NAN at index 0
=== ERROR === update: NAN at index 0
......
...................................................................

cheers,

[stephanweiss]

Dear Hunzai,

According to the ROS warnings thrown it seems that the measurement messages are not correctly received (i.e not received). Hence the initial attitude and position may be too far off of the true values destabilizing the filter.
Please check if the measurements are correctly received.

Best
Stephan

[amajan]

Dear Stephan!

Thanks, Now measurements are received correctly, both tutorials
and pose_sensor is working perfectly with dataset given with tutorials. I
have tried my Visual Odometry (using pose_sensor), it is working with IMU
vaules from dataset given with tutorial. But pose_sensor is not
working with my IMU, and throwing NANs, i have tried imu with both visual
odometry (form my camera) and vicon values from dataset. I have checked imu
is advertising correct linear_acceleration and angular_velocity
on sensor_msgs::Imu, at the moment i am not sending covariance
in sensor_msgs::Imu and suppose that it will use covariance, there is not
much detail about the IMU values and covariance in tutorial. Output of
pose_sensor using my IMU + camera is given below, [note: Translation and
rotation are the values received in measurement callback from visual
odometry module]

=== ERROR === update: NAN at index 0
=== ERROR === prediction p: NAN at index 0
=== ERROR === prediction p: NAN at index 0
[ INFO] [1359060863.812568649]: Trnaslation is: [0.120994, 0.293181,
-0.102633, ]
rotation is: [0.891973, -0.0152079, 0.0221302, -0.451383]

=== ERROR === update: NAN at index 0
=== ERROR === prediction p: NAN at index 0
[ INFO] [1359060863.905235090]: Trnaslation is: [0.127115, 0.353165,
-0.10901, ]
rotation is: [0.874304, -0.117994, 0.00782123, -0.470841]

cheers,

Hunzai

On Mon, Jan 21, 2013 at 11:49 PM, stephanweiss notifications@github.comwrote:

Dear Hunzai,

According to the ROS warnings thrown it seems that the measurement
messages are not correctly received (i.e not received). Hence the initial
attitude and position may be too far off of the true values destabilizing
the filter.
Please check if the measurements are correctly received.

Best
Stephan

—
Reply to this email directly or view it on GitHubhttps://github.com//issues/6#issuecomment-12521887.

[amajan]

Thanks, Problem solved. The problem was in time stamp in sensor_msgs::Imu.

cheers,

Hunzai

[stephanweiss]

Dear Hunzai,

Sorry for my late (actually too late) reply. I'm currently buried in work.
Glad to hear that you could solve the problem on your own. Is now everything working as it should? Let me know if I can be of further assistance or if I can close this issue.

Thanks
Stephan

---

From: amajan [notifications@github.com]
Sent: Saturday, January 26, 2013 6:11 PM
To: ethz-asl/ethzasl_sensor_fusion
Cc: Weiss Stephan
Subject: Re: [ethzasl_sensor_fusion] ethzasl_sensor_fusion tutorial is not working properly (#6)

Thanks, Problem solved. The problem was in time stamp in sensor_msgs::Imu.

cheers,

Hunzai

—
Reply to this email directly or view it on GitHubhttps://github.com//issues/6#issuecomment-12738250.