[Feature request] Add support for EDGE_XYZ_PRIOR

[TSC21]

After doing several tests with inputting GPS data as prior edges to g2o, I find that using EDGE_SE3_PRIOR is not the best solution for GPS data, since GPS doesn't give us a full pose but rather a position. Adding another source of orientation to the GPS data usually results on weird results that tend to break the graph optimisation.

The proposed solution is to add a /global_position topic (already considering the rtabmap_ros modification as well), to where the GPS position data is published and then add that data as EDGE_XYZ_PRIOR instead of EDGE_SE3_PRIOR. The EDGE_SE3_PRIOR should be kept for the case we want to use mocap systems instead of GPS. But for GPS, we need the priors to be a position vector instead of a full pose.

@matlabbe what's your opinion on this? Is this something you can bring in? Thanks!

[matlabbe]

This is a good idea! I was integrating landmarks usage with g2o optimization and I found the same problem, avoiding the rotation to get better results. For example, the orientation of apriltags2's detections are not reliable (in my tests, though maybe not with tag bundles), so I optimized using only XYZ constraints.

For your question, yes, it is something we can bring in. I was trying to find a unified way to tell if we want SE3 or just XYZ for the landmarks too, I could check to implement the same for PRIORs. For example, if angular covariance is 0 or very high (>=9999), use XYZ constraints, otherwise use SE3.

The gps input in rtabmap_ros is through "rtabmap/gps/fix" topic: https://github.com/introlab/rtabmap_ros/blob/e0c58ebe35e3cbb2f6a2087088fabc3201c6821a/src/CoreWrapper.cpp#L2148-L2169 It would be converted to prior by uncommmenting the code here: https://github.com/introlab/rtabmap/blob/6bd9dd55b74d2cdacd80f89f289ccbaafefe2714/corelib/src/Memory.cpp#L4984-L5005 Then use EdgeXYZPrior here instead of EdgeSE3Prior if angular variance is null or very high (like set previously in Memory.cpp).

Hopefully I'll find back the database I used for this post and see if it fixes the problems I had (another database to try).

I am preparing for my thesis's defense next week, I'll check this afterwards.

cheers, Mathieu

[TSC21]

@matlabbe good look for your defense! And thanks for the feedback!

[TSC21]

Then use EdgeXYZPrior here instead of EdgeSE3Prior if angular variance is null or very high (like set previously in Memory.cpp).

@matlabbe NavSatFix will never contain attitude data, so why would we even consider to use EdgeSE3Prior to this kind of data? Or do you by default add prior links and then just set the type afterwards? Would it make sense to add a field to the link that states which type it is, instead of asserting over the covariances?

[matlabbe]

Hi,

Sorry for the delay, I'll check this more in depth at the end of this week. NavStatFix input has been added after the global_pose input to rtabmap_ros. The global pose is meant for ground truth reference like VICON localization with all 6DoFs, so EdgeSE3Prior was used for that. This has been tested only on TUM's dataset, see this test_prior.launch example.

To adapt the same code to GPS, I was passing high covariance on attitude data, but it didn't work. I am looking your commits in this pull request and I think for the 2D slam case, using EdgeSE2XYPrior is exactly what we need. However, for the 3D slam case, I don't think we can EdgeXYZPrior for VertexSE3 vertices, is it just for VertexPointXYZ vertices? We may have to create our own EdgeSE3XYZPrior class (or pull request g2o repo for this new feature). I just saw your comment here about that, so yes we should create a EdgeSE3XYZPrior class. Are all gps giving relatively reliable altitude, or we should also handle the case where the Z is not usable (like creating a EdgeSE3XYPrior type too)?

For the incompatibility with g2o binary version on Indigo/Kinetic, adding EdgeSE3XYZPrior to this project (similar to other custom edges defined for vertigo here) would fix at the same time the problem.

cheers, Mathieu

[TSC21]

@matlabbe thanks for your feedback!

We may have to create our own EdgeSE3XYZPrior class (or pull request g2o repo for this new feature).

@matlabbe I actually already did that here. And I also got some results described in https://github.com/introlab/rtabmap/pull/347#issuecomment-450584450. Though, they are still not the desired. Maybe you can have a look there? Happy new year!

[TSC21]

For the incompatibility with g2o binary version on Indigo/Kinetic, adding EdgeSE3XYZPrior to this project (similar to other custom edges defined for vertigo here) would fix at the same time the problem.

@matlabbe we probably do want to bring this to a docker container based CI instead, since the problem is not the support to EdgeSE3XYZPrior (which was not added yet), but rather to EdgeXYZPrior, which support has already been added to g2o upstream. Anwyay, this won't be the g2o type we will be using for this, so yes probably we can still work with EdgeSE3XYZPrior added to the project.

[matlabbe]

I merged the pull request in this commit https://github.com/introlab/rtabmap/commit/36d7e58ff298d05d472883793e0cc3061f7bf181

I copied EdgeSE3XYZPrior from your pull request on g2o repo to rtabmap repo (here) for convenience until it is officially added to g2o repo. I removed the g2o namespace so that when it is added to g2o repo, it won't break the version in this repo.

I tested with loop_3it_gps.db database from this tutorial. It looks working, the resulting map is correctly oriented in ENU coordinates. The only problem is on beginning when all first nodes are inside the gps error, the optimization is bad. After the camera moved a couple of meters, the optimization is good.

I didn't not tested under ROS though.

cheers, Mathieu

[TSC21]

@matlabbe thanks for merging the PR and adapting it. Though all compiles good, when I try to use it on my pipeline, Rtabmap immediately crashes when it seems to try to add a prior:

terminate called after throwing an instance of 'std::bad_typeid'
  what():  std::bad_typeid
[ INFO] [1546619601.781008616, 8.020000000]: Odom: quality=576, std dev=0.006345m|0.050171rad, update time=0.048637s
[ INFO] [1546619601.818459723, 8.060000000]: Odom: quality=202, std dev=0.004603m|0.038960rad, update time=0.034523s
[ INFO] [1546619601.868743377, 8.108000000]: Odom: quality=510, std dev=0.007614m|0.038960rad, update time=0.048828s
[ INFO] [1546619601.911851762, 8.152000000]: Odom: quality=214, std dev=0.017195m|0.050171rad, update time=0.041560s
[ INFO] [1546619601.963890029, 8.204000000]: Odom: quality=544, std dev=0.007499m|0.038960rad, update time=0.050928s
[rtabmap/rtabmap-11] process has died [pid 2101, exit code -6, cmd /home/nuno/Client_Projects/KARI/kari2_ws/devel/lib/rtabmap_ros/rtabmap --delete_db_on_start rgb/image:=/astra/rgb/image_raw depth/image:=/astra/depth/image_raw rgb/camera_info:=/astra/rgb/camera_info odom:=/mavros/odometry/out global_pose:=/global_pose __name:=rtabmap __log:=/home/nuno/.ros/log/7c4f9960-102b-11e9-8e9d-3c6aa740f2cc/rtabmap-rtabmap-11.log].
log file: /home/nuno/.ros/log/7c4f9960-102b-11e9-8e9d-3c6aa740f2cc/rtabmap-rtabmap-11*.log

we can see a

terminate called after throwing an instance of 'std::bad_typeid'
  what():  std::bad_typeid

have not found the cause for this. This happens when I use global_pose topic or gps/fix topic, and it doesn't matter if I set the LoopGPS param to 0. It doesn't happen if I set PriorsIgnored to 1, meaning it's not using priors. It also happens with Robust set or not.

[TSC21]

This may be related: https://github.com/RainerKuemmerle/g2o/issues/34

Update: Seems we are missing setting the parameter offset to EdgeSE3XYZPrior. I will fix it and retest.

[TSC21]

@matlabbe the above PR fixed the issue. Though I am still having the same results as in https://github.com/introlab/rtabmap/pull/347#issuecomment-450584450. Any idea of what may be causing that "warp" on the graph orientation? Any chance you can have some testing with ROS? Thanks!

[matlabbe]

Do you have the corresponding database?

[TSC21]

This is not from a database. This is real time simulated camera and GPS data coming from Gazebo. Would a rosbag help?

[matlabbe]

A rosbag is fine too

[TSC21]

Ok I will send you one asap. Though, would the prior test that you have on rtabmap_ros be enough to test this on your side for now?

[matlabbe]

I tested on this database: loop_3it_gps.db

I could try on the TUM's rosbag for a ROS test, though I will only be able to test the global_pose and not actual GPS data.

[TSC21]

I think that we can emulate the global pose as GPS data by setting the covariances on the python script to high values right?

[matlabbe]

Indeed, I'll try it

[matlabbe]

Ok,it doesn't work. I forgot to change graph optimisation approach from GTSAM to g2o in my previous tests. Currently if I set g2o as optimisation, with or without priors the result is the same. I'll do more tests.

[TSC21]

Alright thanks for that Mathieu. Anything I can help with?

[matlabbe]

With the test_prior.launch example, I only get "good" results with GTSAM (with large angular covariance or not). For g2o, I only get good results when angular part of covariance is used for estimation (normal covariance). When I set 9999 to angular covariance, g2o gives very bad optimizations. On my database with GPS, it just doesn't optimize at all. It is like ignoring the prior. The problem may be in EdgeSE3XYZPrior class, as EdgeSE3Prior seems working ok when full covariance is used. Comparing to EdgeSE2XYPrior, I changed the linearizeOplus() function to:

void EdgeSE3XYZPrior::linearizeOplus() {
  _jacobianOplusXi << 1,0,0,0,0,0, 0,1,0,0,0,0, 0,0,1,0,0,0;
}

though results are the same. I am not sure what part is missing or wronly implemented for the SE3 case. I also tried a EdgeSE3XYPrior version (without z), copying everything from EdgeSE2XYPrior and still no changes. I am not an expert in g2o, maybe having some feedback on your pull request on g2o repo (https://github.com/RainerKuemmerle/g2o/pull/335) could help to see if it should work or not.

EDIT: I tried on the GPS dataset optimizing in 2D instead, so that EdgeSE2XYPrior is used. g2o is then able to use the prior, though a lot of iterations are required to align the path with the GPS prior.

[TSC21]

@matlabbe thank you very much for your testing! Yeah I have tried a lot of different combinations in some of those functions. Even tried to drop the parameter offset. No luck. I guess we need some input from @RainerKuemmerle, if he can help us.

[TSC21]

@matlabbe did you make any further tests regarding this?

[matlabbe]

@TSC21 I didn't do more tests for the prior using g2o.

[TSC21]

I actually tested this with GTSAM as well but I couldn't get good results either...

[TSC21]

Hey @matlabbe I would like to reboot this. Any ideas you want to discuss about this?

[matlabbe]

Unfortunately, I don't have a platform to test this right now. If you have a rosbag taken on a drone with GPS + images recorded (containing a loop closure), it will be easier to test here. I cannot use my Tango phone to test outside, as there is too much snow (VIO doesn't work when it is white everywhere). I may have access to a drone somewhere in spring though.

[TSC21]

@matlabbe actually all of my tests are being done on a simulated environment. I can probably prepare you something you can test or retrieve a rosbag of it. I will reboot the work on this this next week.

BTW, https://github.com/RainerKuemmerle/g2o/pull/335 got merged, so probably we can use it as a default as soon as it gets released on the ROS build farm.

[matlabbe]

Ok thanks for the update. For the pull request, the problem with g2o is that they don't keep version of the library, which is difficult for third party library like RTAB-Map to use preprocesor definition to know if some specific codes are there or not (as we cannot check the g2o version). EDIT Looks like they will now provide version, well we will see with the next official g2o release if I can detect it and use their code directly instead of the duplicate in rtabmap.

[matlabbe]

Just saw this: https://github.com/koide3/hdl_graph_slam/tree/master/include/g2o There is an example for GPS and SE3 very similar to what we did, but they don't reimplement linearizeOplus(), I'll give a try if this works.

[TSC21]

Interesting. But I do remember ignoring the implemenation of linearizeOplus() but still get the same results. @RainerKuemmerle merged my PR with this support so I suppose that approach is correct.

[matlabbe]

As you can see in this change, I removed linearizeOplus() from the code and it started to work. I had some fine tuning to do to avoid the map tilting when optimizing close GPS values (all in same range of the GPS error), but it looks fine with the tests I can do. I also added a new GPS factor for GTSAM.

[TSC21]

Awesome! Perfect! I should make that change on upstream g2o as well then. I will have a try to this on my setup and I will let you know how it goes. Thanks for testing and cleaning this @matlabbe!

[TSC21]

Btw,

I had some fine tuning to do to avoid the map tilting when optimizing close GPS values (all in same range of the GPS error)

Where are you doing these optimisations on the src code? Can you point them? Thanks!

[matlabbe]

https://github.com/introlab/rtabmap/blob/77ae8e108afd700533d058d3de947f813f07817e/corelib/src/Memory.cpp#L5098

The problem is that as we don't set prior in 6DoF or we don't fix a node in the graph, providing only XY values cause an underestimated problem (when we don't have a lot of poses). If we have already a full map, optimization would converge even if we use only XY constraints. I tried adding a new XY-prior (without Z) but optimization failed every time at the beginning so I kept XYZ-prior but by setting z. Here z is set as the current height of the computed pose in odometry frame and the z variance should not be too high (to get convergence when we don't still move a lot). For angles, we need to set also a prior for GTSAM (see here), but for g2o it doesn't seem required.