search

iris-ua / iris_lama_ros

LaMa on ROS
BSD 3-Clause "New" or "Revised" License
208 stars 59 forks source link

covariance of position #36

Closed TZVIh closed 3 years ago

TZVIh commented 3 years ago

Hi, I am working with this package. How can I know the covariance of the solution? Thanks

eupedrosa commented 3 years ago

Hi @TZVIh, from which solution? localization, SLAM?

The localization covariance is already publish in the ~pose topic. The covariance of the SLAM solutions are yet to be published on a topic.

TZVIh commented 3 years ago

Hi, Thanks for the fast response! I am using the localization mode.

I checked the "/pose" topic and the covariance is always zero. attached pic of echo to this topic Selection_209

eupedrosa commented 3 years ago

Are you using the master branch? The feature is very recent.

TZVIh commented 3 years ago

I just pull the last commit. The build failed (catkin build or catkin make) Selection_210

eupedrosa commented 3 years ago

You also have to use the latest version from https://github.com/iris-ua/iris_lama

TZVIh commented 3 years ago

works!!!!!! Thanks!

TZVIh commented 3 years ago

one more question, why the frame of the pose msg is empty?

eupedrosa commented 3 years ago

It should not be empty, it should have the global frame id. It is a bug, I will fix that.

TZVIh commented 3 years ago

Hi, I want to share with you that I tested LaMa's localization mode against AMCL and other packages and I must say that LaMa's performance was the best.

I'm checking now the covariance of the solution.

I set the initial pose to 0.0 (this is not the right pose) on the launch file (loc2d)

At the beginning of the bag file, the variance of the variables (diagonal) is 1. Selection_226

When the robot starts to move the variance drops to a very small value, you can see that the robot didn't localize right. Selection_228 Selection_227

If I understand correctly I would expect to see that the variance is still high because the robot did not find its correct position.

Can you explain to me why the variance drops to a small value?

Thanks!

TZVIh commented 3 years ago

Hi @eupedrosa, Can you please help me?

eupedrosa commented 3 years ago

Hi @TZVIh, yes I can help you.

The covariance that I publish results from the Information matrix used in the optimization. As such, it will reflect the end state of the optimization. The optimization will always find a local minimum, therefore it will most likely underestimate the true covariance.

But I believe there are 2 changes that you can try to see if the covariance raises when it is on a bad location:

  1. Increse the value of the parameter ~l2_max to 5.0 or even 10.0. This will increase the time it takes to load the map, but it is OK because it happens only once. Whan happens is that for a small value such as 0.5 (the default), on a bad localization situation most laser points will have null gradient, and the gradient contributes to the covariance. By increasing this parameter you will be adding more points to the calculation of the covariance.
  2. The covariance is scaled for robust fitting. Maybe this should not be done when return the covariance. Go to
    https://github.com/iris-ua/iris_lama/blob/37f49b91c70aafbc91a6a713dac09037026ea146/src/nlls/solver.cpp#L106 and remove lines 108, 109 and 109.
TZVIh commented 3 years ago

Hi, Thanks for the response.

I changed l2_max to 100.0. in addition, I removed lines 108-110 from the solver.cpp as you write.

The variance still has a small value (0.002)

Do you have more ideas?

eupedrosa commented 3 years ago

Changin l2_max to 100 is not a good idea, it will create a huge distance map. 10 is a better number.

Like I said, the optimization covariance will underestimate the true covariance. It will not provide the measurement that you are expecting. The only way to have something similar to what you are expecting, is to sample around the calculated pose and estimate a covariance weighted by the likelihood.

TZVIh commented 3 years ago

I really appreciate your help. I will do it.