No outputs on msf_core/pose topic about ethzasl_msf HOT 34 CLOSED

Did you initialize the filter? You can make a service call or use dynamic
reconfigure.
On Jul 15, 2014 10:05 AM, "mhkabir" [email protected] wrote:

I am using you framework for fusing IMU data with visual odometry.

I am sending IMU data at 200Hz into the framework and visual odometry at
30Hz. Yet, there is no output on the pose topic.

I managed to get it working before, but somehow, cannot get it working
anymore.

Kabir

—
Reply to this email directly or view it on GitHub
#73.

from ethzasl_msf.

Yes, I did try to initialize the filter using rqt_reconfigure...

I get this :

[ INFO] [1405471483.178157644]: Initializing msf_core (built: Jul 14 2014)
[ INFO] [1405471483.178403310]: Core parameters:
        fixed_bias:     0
        fuzzythres:     0.1
        noise_acc:      0.013
        noise_accbias:  0.000108
        noise_gyr:      0.00065
        noise_gyrbias:  2.12e-06

[ INFO] [1405471483.178809059]: Core init with state:
--------- State at time 1483.07s: ---------
0 : [0-2]        : Matrix<3, 1>         : [-nan -nan -nan]
1 : [3-5]        : Matrix<3, 1>         : [0 0 0]
2 : [6-9]        : Quaternion (w,x,y,z) : [nan, nan, nan, nan]
3 : [10-12]      : Matrix<3, 1>         : [0 0 0]
4 : [13-15]      : Matrix<3, 1>         : [0 0 0]
5 : [16-16]      : Matrix<1, 1>         : [1]
6 : [17-20]      : Quaternion (w,x,y,z) : [1, 0, 0, 0]
7 : [21-23]      : Matrix<3, 1>         : [0 0 0]
8 : [24-27]      : Quaternion (w,x,y,z) : [nan, nan, nan, nan]
9 : [28-30]      : Matrix<3, 1>         : [0 0 0]
-------------------------------------------------------

from ethzasl_msf.

This looks like your initial (or all) pose measurements are corrupted. For instance the position (first element) is initialized directly from the measurement and it is initialized to nan. Did you double check the messages that arrive at your pose measurement topic?

from ethzasl_msf.

Yep, the pose measurement should be fine, since

linaro@linaro-ubuntu-desktop:~$ rostopic info /svo/pose
Type: geometry_msgs/PoseWithCovarianceStamped

Publishers:
 * /svo (http://linaro-ubuntu-desktop:41572/)

Subscribers:
 * /msf_pose_sensor (http://linaro-ubuntu-desktop:60837/)

On Wed, Jul 16, 2014 at 6:25 AM, Simon Lynen [email protected]
wrote:

This looks like your initial (or all) pose measurements are corrupted. For
instance the position (first element) is initialized directly from the
measurement and it is initialized to nan. Did you double check the messages
that arrive at your pose measurement topic?

—
Reply to this email directly or view it on GitHub
#73 (comment).

from ethzasl_msf.

Which quaternion convention are you using and what are the measurement
values that you get out of SVO?

On Tue, Jul 15, 2014 at 6:07 PM, mhkabir [email protected] wrote:

Yep, the pose measurement should be fine, since

linaro@linaro-ubuntu-desktop:~$ rostopic info /svo/pose
Type: geometry_msgs/PoseWithCovarianceStamped

Publishers:
* /svo (http://linaro-ubuntu-desktop:41572/)

Subscribers:
* /msf_pose_sensor (http://linaro-ubuntu-desktop:60837/)

On Wed, Jul 16, 2014 at 6:25 AM, Simon Lynen [email protected]
wrote:

This looks like your initial (or all) pose measurements are corrupted.
For
instance the position (first element) is initialized directly from the
measurement and it is initialized to nan. Did you double check the
messages
that arrive at your pose measurement topic?

—
Reply to this email directly or view it on GitHub
#73 (comment).

—
Reply to this email directly or view it on GitHub
#73 (comment).

from ethzasl_msf.

In SVO, take care that you publish the right transform. You will probably have to set

  <param name="publish_world_in_cam_frame" value="false" />

which means that you want the output pose to be in the world frame of reference.
maybe, you want also to set

 <param name="init_rx" value="3.1415289878" />

depending on how your camera is positioned relative to the imu.
also, don't take the covariance values from SVO, they are not correct.

from ethzasl_msf.

Thanks @cfo I can now initialize the sensor properly atleast. But still no outputs from pose topic.

[ INFO] [1405501986.023310941]: Initializing msf_core (built: Jul 14 2014)
[ INFO] [1405501986.023614774]: Core parameters:
        fixed_bias:     0
        fuzzythres:     0.1
        noise_acc:      0.013
        noise_accbias:  0.000108
        noise_gyr:      0.00065
        noise_gyrbias:  2.12e-06

[ INFO] [1405501986.024128439]: Core init with state:
--------- State at time 1985.88s: ---------
0 : [0-2]        : Matrix<3, 1>         : [   0.149152  -0.0295024 -0.00556019]
1 : [3-5]        : Matrix<3, 1>         : [0 0 0]
2 : [6-9]        : Quaternion (w,x,y,z) : [-0.0347, -0.999, 0.0186, -0.00373]
3 : [10-12]      : Matrix<3, 1>         : [0 0 0]
4 : [13-15]      : Matrix<3, 1>         : [0 0 0]
5 : [16-16]      : Matrix<1, 1>         : [1]
6 : [17-20]      : Quaternion (w,x,y,z) : [1, 0, 0, 0]
7 : [21-23]      : Matrix<3, 1>         : [0 0 0]
8 : [24-27]      : Quaternion (w,x,y,z) : [0, 1, 0, 0]
9 : [28-30]      : Matrix<3, 1>         : [0 0 0]
-------------------------------------------------------

Do the values look right?

I think there is some problem with the IMU data...? Not getting received by MSF?

from ethzasl_msf.

I'm getting good IMU and SVO data now, and yet no outputs even after initializing the framework :P

Any ideas @simonlynen ?

Sample data :

SVO :

---
header:
  seq: 795
  stamp:
    secs: 1405616441
    nsecs: 649726629
  frame_id: /cam
pose:
  pose:
    position:
      x: 0.12285393062
      y: -0.0143869148258
      z: -0.235896560432
    orientation:
      x: 0.0320621401375
      y: -0.0434816732092
      z: -0.044439962404
      w: 0.997550225806
  covariance: [1.4327927225791888e-06, 3.7568406184567987e-07, -1.5461459290652766e-07, -3.7023305420607127e-07, 1.246622936200189e-06, 1.714369119458573e-08, 3.7265086058050714e-07, 2.1106690594245825e-06, 5.783017015886868e-08, -1.929246011500069e-06, 3.5268046543407516e-07, 6.942251399055185e-08, -3.898435458353329e-07, 5.301791780673941e-08, 2.0593686137025986e-07, -4.2834023744738594e-08, -3.7083929854001214e-07, -5.3366803955698654e-09, -3.8150889613226906e-07, -1.9667239329618632e-06, -5.412466577565975e-08, 1.8379541105277403e-06, -3.587553510464015e-07, -3.372064170257285e-08, 1.2901010879815966e-06, 3.611256305212243e-07, -1.9225907486604757e-07, -3.546876609492482e-07, 1.1665182911554597e-06, 1.2339031590992768e-08, 8.38221036346974e-08, 3.167053208528906e-07, 7.538977248618598e-10, -2.925159193978575e-07, 7.699077664823163e-08, 2.469606138675778e-07]
---

IMU :

---
header:
  seq: 3213
  stamp:
    secs: 1405616893
    nsecs: 153642228
  frame_id: fcu
orientation:
  x: 0.000445707686385
  y: 0.00407678966446
  z: -0.249429305623
  w: 0.968384325889
orientation_covariance: [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0]
angular_velocity:
  x: 0.00331596867181
  y: 0.00372681044973
  z: 0.00491053424776
angular_velocity_covariance: [1.2184696791468346e-07, 0.0, 0.0, 0.0, 1.2184696791468346e-07, 0.0, 0.0, 0.0, 1.2184696791468346e-07]
linear_acceleration:
  x: 0.0963661894202
  y: -0.147878572345
  z: 9.74281692505
linear_acceleration_covariance: [8.999999999999999e-08, 0.0, 0.0, 0.0, 8.999999999999999e-08, 0.0, 0.0, 0.0, 8.999999999999999e-08]
---

PLEASE HELP!!

from ethzasl_msf.

Do you use script to check output? Just type this command:
rosrun msf_core plot_relevant

from ethzasl_msf.

@mhkabir Which topic are you listening on? Did you look at the graph with rqt_graph?

from ethzasl_msf.

I can now get data on state_out. I think something was wrong with another node of mine outputting the IMU data.

I can also see the plot in rqt_plot, but cannot figure out a way to add labels to the plots. @simonlynen could you please update the plot scripts in msf_core to use rqt_plot?

Now I get :

[ INFO] [1405643621.513229317]: initial measurement pos:[0.0276036  0.173105 0.0276151] orientation: [0.994, 0.0416, -0.0389, -0.088]
[ WARN] [1405643621.700300634]: Using simulated core plus fixed diag initial error state covariance.
[ INFO] [1405643621.710855666]: Initializing msf_core (built: Jul 14 2014)
[ INFO] [1405643621.711238667]: Core parameters:
        fixed_bias:     0
        fuzzythres:     0.1
        noise_acc:      0.013
        noise_accbias:  0.000108
        noise_gyr:      0.00065
        noise_gyrbias:  2.12e-06

[ INFO] [1405643621.711982086]: Core init with state:
--------- State at time 3621.6s: ---------
0 : [0-2]        : Matrix<3, 1>         : [0.0276036  0.173105 0.0276151]
1 : [3-5]        : Matrix<3, 1>         : [0 0 0]
2 : [6-9]        : Quaternion (w,x,y,z) : [0.0416, -0.994, 0.088, -0.0389]
3 : [10-12]      : Matrix<3, 1>         : [0 0 0]
4 : [13-15]      : Matrix<3, 1>         : [0 0 0]
5 : [16-16]      : Matrix<1, 1>         : [1]
6 : [17-20]      : Quaternion (w,x,y,z) : [1, 0, 0, 0]
7 : [21-23]      : Matrix<3, 1>         : [0 0 0]
8 : [24-27]      : Quaternion (w,x,y,z) : [0, 1, 0, 0]
9 : [28-30]      : Matrix<3, 1>         : [0 0 0]
-------------------------------------------------------
[ WARN] [1405643622.012191787]: You tried to give me a measurement which is too far in the past. Are you sure your clocks are synced and delays compensated correctly? [measurement: 3621.45 (s) first state in buffer: 3621.52 (s)]
[ WARN] [1405643622.013234332]: You tried to give me a measurement which is too far in the past. Are you sure your clocks are synced and delays compensated correctly? [measurement: 3621.51 (s) first state in buffer: 3621.52 (s)]

from ethzasl_msf.

Okay, so now I think its working, but I keep getting "Fuzzy tracking triggered" which I think indicates something off with my camera-IMU transformation.

@cfo I think you can help with this. Camera-IMU transformations are doing my head in :P
I'm using a Pixhawk, publishing IMU in ENU, and the camera is mounted on the bottom, top side facing towards the front of the MAV. What would be the required transformations?

from ethzasl_msf.

I can now initialize MSF, but yet no outputs on the pose topic. I fixed the fuzzy tracking problem, and only get fuzzy tracking errors very rarely(when SVO is close to losing track).

Can someone please tell me the full and proper initialization sequence? PLEASE!! Do I have to set height, etc.?

Also, the plots use rxgraph which is not available in Hydro. Can you give me a command for rqt_plot which will allow it me to use proper labels on the graphs? @simonlynen

Kabir

from ethzasl_msf.

for rqt_plot, only thing you need to do is rosrun rqt_plot rqt_plot [full topic path].
For example, if you want to plot the position of EKF output, you can do that:
rosrun rqt_plot rqt_plot msf_core/state_out/data[0]:data[1]:data[2]

BTW, @simonlynen, Is it ok I create a pull request to replace this legacy plotting script(use rqt_plot )? Actually I wrote some of them, but I didn't complete. Since new version of ROS(e.g. hydro) doesn't have this package anymore, it is good choice to migrate to new plotting tool.

from ethzasl_msf.

So I get plots in rqt_plot, but I have no idea how to proceed?

I have my camera and IMU working, but how to do a proper initialization?
Please elaborate!

Thanks!!

On Fri, Jul 18, 2014 at 9:26 PM, Da-Feng Huang [email protected]
wrote:

for rqt_plot, only thing you need to do is rosrun rqt_plot rqt_plot [full
topic path].
For example, if you want to plot the position of EKF output, you can do
that:
rosrun rqt_plot rqt_plot msf_core/state_out/data[0]:data[1]:data[2]

BTW, @simonlynen https://github.com/simonlynen, Is it ok I create a
pull request to replace this legacy plotting script(use rqt_plot )?
Actually I wrote some of them, but I didn't complete. Since new version of
ROS(e.g. hydro) doesn't have this package anymore, it is good choice to
migrate to new plotting tool.

—
Reply to this email directly or view it on GitHub
#73 (comment).

from ethzasl_msf.

@fboris What process did you follow to get outputs from the /msf_core/pose topic of MSF?

I want to use it for vehicle control.

from ethzasl_msf.

Usually, I start the IMU topic and camera topic first then start visual odometry(e.g. PTAM or SVO) . But I don't want to waste time to type many commands. I rewrite the launch file that helps me to run camera driver and visual odometry at same time. In addition, you should launch MSF and make the coordinate transform correctly(which causes fuzzy tracking at startup). If MSF gets the visual odometry topic, it will show some messages on terminal. Last step, MSF should be initialized and output some data. You can use rqt_plot to check. Sometimes, I follow these procedure but it still does not output anything. I'm finding the solution currently(might not relate with MSF).

from ethzasl_msf.

@fboris Yes I would very much appreciate getting an update to the plotting scripts. So yes if you find the time, I am happy accepting PRs. Thanks!

from ethzasl_msf.

I do the same thing ...

I also get data in state_out, but nothing on the msf_core/pose topic. @simonlynen Could you comment on this?

from ethzasl_msf.

@mhkabir The pose topic should be published after every propagation (e.g. at IMU frequency). https://github.com/ethz-asl/ethzasl_msf/blob/master/msf_core/include/msf_core/msf_sensormanagerROS.h#L205 However this can also be changed to any other class that derives from msf_sensormanager. Are you using the msf_sensormanagerROS or did you change that in your filter-setup?

from ethzasl_msf.

@mhkabir to be more precise: In your msf_updates package, are you deriving your sensor-manager from sensor_managerROS or sensor_manager?

from ethzasl_msf.

@simonlynen I'm using the standard pose_sensor node.
pkg="msf_updates" type="pose_sensor"

from ethzasl_msf.

@mhkabir Can you then add a debug output line to https://github.com/ethz-asl/ethzasl_msf/blob/master/msf_core/include/msf_core/msf_sensormanagerROS.h#L205 and see if msf actually detectors your subscribers? (The topic is only published if there are subscribers)

from ethzasl_msf.

Yep, I'll do that and get back.

from ethzasl_msf.

@simonlynen Looks like the issue was with the subscriber detection. I set it to publish always and now there is good output on the topic.

But there is also a problem, if I lose vision estimate from SVO, the filter keeps publishing gibberish and position values keep on increasing.

from ethzasl_msf.

@mhkabir An indirect EKF uses the IMU measurements for state and covariance prediction. If there are no measurements your estimate on the states which are not observable from IMU alone will not remain stationary. This is the correct behaviour.

Of course you can put in additional engineering approaches which would switch filter-mode in case there are no measurements. But how do you want to detect that there are no measurements because of a drop-out vs. the normal time-gap between measurements? Putting in an appropriate timeout is very application specific which is why there is no such implementation in the ethzasl_msf library.

from ethzasl_msf.

Thanks Simon. I will look into adding a method of detecting drop outs. Do
you have any suggestions for this? Dowe reset velocities to zero when
there is loss of estimate?

Also, why do you think subscribers to the pose topic are not detected?

Kabir

On Mon, Jul 21, 2014 at 8:58 PM, Simon Lynen [email protected]
wrote:

@mhkabir https://github.com/mhkabir An indirect EKF uses the IMU
measurements for state and covariance prediction. If there are no
measurements your estimate on the states which are not observable from IMU
alone will not remain stationary. This is the correct behaviour.

Of course you can put in additional engineering approaches which would
switch filter-mode in case there are no measurements. But how do you want
to detect that there are no measurements because of a drop-out vs. the
normal time-gap between measurements? Putting in an appropriate timeout is
very application specific which is why there is no such implementation in
the ethzasl_msf library.

—
Reply to this email directly or view it on GitHub
#73 (comment).

from ethzasl_msf.

@mhkabir There is a watchdog implementation in the msf which checks if messages are continuous, you can see if you can adapt that s.t. you can monitor the delta-time diffs between messages.

No I don't think plainly setting velocities to zero is a good idea for a quadrotor. You can make zero-velocity updates to make the filter estimate orientation only.

w.r.t. the subscribers. I don't know your setup. This has been working on all platforms we have run the framework on.

from ethzasl_msf.

As @simonlynen mentioned, IMU alone will not "kick" your estimation back. To acknowledge this, you should know what properties EKF has. Prediction step that is described in (http://wiki.ros.org/ethzasl_sensor_fusion/Tutorials/getting_started) will output the state by means of propagation in rate of IMU reading. The key point is here. It will propagate thousand times and only trust them when your system lost visual measurements. If you are using "bad" IMU, the noise will also be propagated thousands times.

Also, the zero-velocity estimate is not good idea. Drones are always not in zero velocity. Alternatively, there is another way which is called optical flow. This method can deal with the situation that lost feature tracking. But this part is out of my knowledge.

from ethzasl_msf.

Thank you for all the inputs.

Infact, I do have an optical flow sensor onboard, streaming to ROS @50Hz. I
think we could implement this into MSF for robustness and handling loss of
vision estimate.

I think we can use zero-velocity updates to send only orientation
estimates. The low-level FC could be then handle position with '0' as lost
estimate.

Kabir

On Mon, Jul 21, 2014 at 9:47 PM, Da-Feng Huang [email protected]
wrote:

As @simonlynen https://github.com/simonlynen mentioned, IMU alone will
not "kick" your estimation back. To acknowledge this, you should know what
properties EKF has. Prediction step that is described in (
http://wiki.ros.org/ethzasl_sensor_fusion/Tutorials/getting_started) will
output the state by means of propagation in rate of IMU reading. The key
point is here. It will propagate thousand times and only trust them when
your system lost visual measurements. If you are using "bad" IMU, the noise
will also be propagated thousands times.

Also, the zero-velocity estimate is not good idea. Drones are always not
in zero velocity. Alternatively, there is another way which is called
optical flow. This method can deal with the situation that lost feature
tracking. But this part is out of my knowledge.

—
Reply to this email directly or view it on GitHub
#73 (comment).

from ethzasl_msf.

@simonlynen Do you have any suggestions as to implementing the optical flow into MSF? I'm not really comfortable around the MSF code yet, so would you please consider helping?

My node is streaming filtered flow velocity in X-Y axes in m/s so it shouldn't be too difficult to do this, I think...?

Also, I will be adding support for a baro (non-Astec, so using generic ROS FluidPressure interface) and a SONAR or Lidar-Lite.

Kabir

from ethzasl_msf.

Yes if you do the tutorials on the old ethzasl_sensor_fusion package you should be able to write an update module for your velocity update.

from ethzasl_msf.

You can evaluate the callback from dynamic reconfigure here:
https://github.com/ethz-asl/ethzasl_msf/blob/master/msf_core/include/msf_core/msf_sensormanagerROS.h#L138

On Tue, Jul 29, 2014 at 6:09 AM, mhkabir [email protected] wrote:

@simonlynen https://github.com/simonlynen Now I have the framework
working very well on my MAV. I was trying to add a dynamic reconfigure
param to SensorManagerROS, but am failing to do so.

Can you tell me in brief how I would go about this?

—
Reply to this email directly or view it on GitHub
#73 (comment).

from ethzasl_msf.

Assuming this is resolved.

from ethzasl_msf.