Pose covariance from mapping missing

[matlabbe]

RTAB-Map doesn't keep track of the accumulated pose covariance over time, neither output a covariance for the latest pose (with graph optimization). Well, I think this covariance could be computed as old node accumulated covariance x the loop closure covariance.

Note that if the map is optimized from the end (current pose), the current node covariance would be ~null, while farther nodes would have higher covariance. In a multi-session setup and where some parts of the map are not available during graph optimization, would we need to send new covariances from the graph optimization along the raw covariances in the published graph? That way we could recompute current covariance for each node while the graph is modified.

At the same time, pose covariance (not twist) of the odometry should increase over time the same way.

From this post: http://official-rtab-map-forum.206.s1.nabble.com/Camera-Pose-with-Covariance-td1555.html

[eliabntt]

any news about this?

[matlabbe]

Echo topic localization_pose, it contains a covariance, which is the marginal error after graph optimization (see g2o or GTSAM for more info about marginals).

$ rostopic echo /rtabmap/localization_pose

This topic is published only when a graph optimization is done (e.g., on loop closures). See:

rtabmap/corelib/src/Rtabmap.cpp

Line 2857 in 356094f

optimizeCurrentMap(signature->id(), false, poses, covariance, &constraints, &optimizationError, &optimizationIterations);

g2o

If rtabmap is using g2o for graph optimization (Optimizer/Strategy=1), see for 2D:

rtabmap/corelib/src/optimizer/OptimizerG2O.cpp

Line 1043 in 356094f

optimizer.computeMarginals(spinv, v);

for 3D

rtabmap/corelib/src/optimizer/OptimizerG2O.cpp

Line 1127 in 356094f

optimizer.computeMarginals(spinv, v);

GTSAM

If rtabmap is using GTSAM for graph optimization (Optimizer/Strategy=2), see:

rtabmap/corelib/src/optimizer/OptimizerGTSAM.cpp

Lines 662 to 663 in 356094f

	gtsam::Marginals marginals(graph, optimizer->values());
	gtsam::Matrix info = marginals.marginalCovariance(poses.rbegin()->first);

Note

This is experimental, we don't have projects using this info, so it is not tested. In practice, sometimes marginals cannot be computed, a warning is then shown on terminal. Depending on what you need, the odometry should already provide a covariance (though it would increase out of bound).

cheers,
Mathieu

[eliabntt]

ok, yeah I've seen that. Is that the covariance of the last position/node of the graph? what if I want that for each one of the nodes? is it possible?

[matlabbe]

It is the marginal of the last node. GTSAM seems computing by default the marginals of all nodes. For g2o, it is computed only for last node, not sure if it would be efficient to do it for all nodes with how it is currently done in rtabmap (see links of my previous post). To expose this info, we would need to check if there is an overhead to do this (not seem the case for GTSAM as they are all computed already) and if this is a really useful information for you. You may hard-code an export of the marginals when it happens and see if this is the kind of covariance you need. If it can be useful, we could check how to propagate this information up with the poses, so that we can publish on ROS a graph of poses with those covariances.

Another way to see if it could work is to export the graph in g2o format (File->Export poses...) in rtabmap, then import to g2o tools to play with it.

cheers,
Mathieu

[eliabntt]

Ok, perfect. I'll peek into this probably next week. I'll keep you posted, maybe will do that with GTSAM (currently I'm working with g2o).

I've also the odom topic but the covariance is crazily high and fixed. We have a RS200 and a laser scanner, doing visual odometry and would like to have the covariance of the current robot position...

Working on gazebo simulation I have also some other issues right now.
For example when the robot rotates he create "fake" walls due to laser scan displacement. I think I can solve this reducing the rotational speed of the robot or changing the maximum allowed rotation in rtabmap[not working]. I have the feeling that when the robot rotates on its axis since the laser is joint with the base of the robot there is a synchronization issue.

Another one is the clearing of obstacles in 2D/3D that is not really working for me. In 2D is just fine(even if slow) unless I save the map. In this latter case every time I relocalize myself the objects re-appears.. As for the 3D map I don't think there are already filtering/cleaning methods for previously constructed clouds right?

[Solved using RGBD/LocalRadius] There are "local closures" (yellows matches on rtabviz) that go very far with respect to where the robot is localized (even a couple of meter). The localization pose covariance is good, the map is not being distorted by that but still it happens.
Here though I would like to merge more nodes when local closure happens. You feel that this is possible? As of now the algorithm merges nodes with global loop closures and neighborhood merging (which I would like to improve too)...

I've attached my launch file
mapping1.txt and some parameters
params.txt
that I'm currently working on if you have time to get a look at that :)

In the pic there's everything I am saying. Yellow lines are local loop closures[Solved], while on the right you can see my walls.

[matlabbe]

Hi,

With a lidar, use Reg/Strategy=1. Set Rtabmap/TimeThr=0 if you don't understand perfectly what this parameter means, otherwise you won't understand why some parts of the map would disappear at some point. I generally set Optimizer/Robust=false with RGBD/OptimizeMaxError=3, to actually reject the bad loop closures instead of just ignoring them on optimization.

The yellow lines are proximity detections using the lidar. RGBD/LocalRadius (default 10 meters) can be indeed decreased to limit how far we do and accept a proximity detection.

cheers,
Mathieu

[eliabntt]

Reg/Strategy=2 shouldn't use both Visual+ICP registration? we're doing visual odometry so I don't get why you say that I should set 1...

I've checked the source code of gtsam(here an example) and g2o and both should compute the marginals for all the nodes so for that I should be good. I'll peek more next week..

Yup RGBD/LocalRadius solved my problem.

I'll try your other suggestion thanks a lot.

Regarding walls/odometry/cleaning obstacles problems do you have any suggestion?

Best,
Elia

[matlabbe]

For info about Reg/Strategy=2, see http://official-rtab-map-forum.206.s1.nabble.com/Loop-closure-registration-strategies-Help-tp6421p6432.html

For example when the robot rotates he create "fake" walls due to laser scan displacement.

Not sure to see the fake walls problem. Normally in gazebo the position between the sensors should be accurate. Is this happening only when rotating fast? Your approx_sync is false too, so it would not be a sync problem either.

In this latter case every time I relocalize myself the objects re-appears.. As for the 3D map I don't think there are already filtering/cleaning methods for previously constructed clouds right?

re-appears on the occupancy grid /rtabmap/grid_map, or MapCloud display or /rtabmap/cloud_map (PointCloud2)?

For g2o marginals, the computeMarginals() function returns a boolean, we would need then to find the function to get the computed marginal for a specified node. The marginals are computed by the solver here, I think the uncertaintyData is modified for each vertex, so we could get the covariance with this.

To propagate the covariances, I think the best would be to modify the API:

rtabmap/corelib/include/rtabmap/core/Optimizer.h

Lines 124 to 131 in f9e818c

	virtual std::map<int, Transform> optimize(
	int rootId,
	const std::map<int, Transform> & poses,
	const std::multimap<int, Link> & constraints,
	cv::Mat & outputCovariance,
	std::list<std::map<int, Transform> > * intermediateGraphes = 0,
	double * finalError = 0,
	int * iterationsDone = 0);

or add this new implementation:

 virtual std::map<int, Transform> optimize( 
 			int rootId, 
 			const std::map<int, Transform> & poses, 
 			const std::multimap<int, Link> & constraints, 
 			const std::map<cv::Mat> & outputCovariances, 
 			std::list<std::map<int, Transform> > * intermediateGraphes = 0, 
 			double * finalError = 0, 
 			int * iterationsDone = 0); 

where outputCovariances contains the marginals.

cheers,
Mathieu

[eliabntt]

Yup I agree with you for the g2o thing.. I'll peek into this I think from monday, no time right now.

As for the walls I mean as in the right side of rviz starting from top between the third and fourth square there is the right wall parallel to the squares side and two "readings" of obliques walls that essentially ruins the 3D scan (while the 2D map is cleared later on). See next pictures

Regarding this do you have any suggestion to clear things from the 3D scan? any parameter I can try? I've seen older issues etc but as long as I understand we have only the 2D map that is cleaning.

re-appears on the occupancy grid /rtabmap/grid_map, or MapCloud display or /rtabmap/cloud_map (PointCloud2)?

I'll check this..

As for odometry topic that is publishing this fixed covariance: [1e-05, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-05, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1000000000000.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1000000000000.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1000000000000.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.001] I can't find a solution. I tried to work a bit with odom_frame_id, odom params but nothing worked..

[matlabbe]

Maybe a synchronization issue between odometry and the sensors. Do you have the database to share?

We can do ray tracing in 3D too. This is done by enabling Grid/FromDepth=true (to use depth images, otherwise set it false to use with 3D lidar), Grid/3D=true and Grid/RayTracing=true. Then subscribe to /rtabmap/octomap_obstacles for a point cloud or /rtabmap/octomap_binary for the OctoMap. See Table 10 and Figures 14 to 17 on the paper RTAB-Map as an Open-Source Lidar and Visual SLAM Library for Large-Scale and Long-Term Online Operation for different configurations.

[eliabntt]

Here the database https://we.tl/t-Nv3DVyEomh
The launch file essentially hadn't change much from the last time apart from the parameters settings.

In rviz odom frame is fixed (where map frame is) while everything else moves.

The parameters you are suggesting are already set as you say but these three problems(walls [that I suppose it's related to the simulation], odom, and obstacles [sorry couldn't find time to do a video for you today]) remains..

Thanks a lot for your patience :)

[matlabbe]

The biggest problem is the synchronization between the odometry, camera and lidar.

While the laser scans are correctly aligned, the 3D clouds from camera are not:

As approx_sync=false in your launch, it seems to be a gazebo problem not giving the right stamps at the right time. Do you have a computer with a discrete GPU? You could try on another computer to see if the problem is also there.

[eliabntt]

Ok. Yes I have a gpu I can enable it I think..
So that's also the reason why covariance of /Odom topic does not change? It sound strange since heading covariance is crazy high while position covariance is crazy low 😅😅

[matlabbe]

For the covariance in odometry topic, it is related to gazebo's odometry plugin you are using, which may just set it to a fixed value.

[eliabntt]

Isn't that the odometry of the visual odometry system?

[matlabbe]

In your TF tree, /gazebo is sending odom->base_link, I assumed that visual odometry is not used, but the odometry plugin from gazebo.

[eliabntt]

Nope I want visual odometry 😅😅

Ok, solved I simply missed the node. Added that and the reset odom after 10 tries everything works.

[eliabntt]

The problem for the camera I think that's related to the gazebo_realsense_plugin that publish every 5/6 Hz (not/compressed data) while the laser is publishing at 20.

[eliabntt]

The problem for the camera I think that's related to the gazebo_realsense_plugin that publish every 5/6 Hz (not/compressed data) while the laser is publishing at 20.

I checked and it's a simulation problem. When the gazebo_realsense is alone it's publishing at 60 Hz when I place the env in gazebo drops down to 5/6

[eliabntt]

So I've peeked into this:

For g2o marginals, the computeMarginals() function returns a boolean, we would need then to find the function to get the computed marginal for a specified node. The marginals are computed by the solver here, I think the uncertaintyData is modified for each vertex, so we could get the covariance with this.

And I've tried to edit both the source code of g2o (block_solver.hpp lines 489-498 that is different from the one you've linked) and rtabmap (OptimizerG2O.cpp).
Essentially as long as I understand correctly you in line 1038 create a vertex with the last pose. Then you compute the marginals. Point is that spinv is increasing the number of rows(spinv.rows()), 7 new rows each time a loop closure is detected (strange actually, should probably be 6). The point is tha blockIndices are just the ones about the last pose so if you try

for (int i = 0; i < blockIndices.size(); i++) 
{
     MatrixXD m = *spinv.block(blockIndices.at(index).first, blockIndices.at(index).second);
     std::cout << m;
}

being blockIndices 1 you have only the last 6x6 matrix.
What do you think? Is there a way to have the previous blocks?

Moreover, sometime computeMarginals is called twice.

blockIndices 1 spinv.rows() 18  
 8.31043e-05  1.26399e-18 -5.82655e-19           -0           -0           -0
 1.26399e-18  8.31043e-05   6.1406e-19           -0           -0           -0
-5.82655e-19   6.1406e-19  8.31043e-05           -0           -0           -0
          -0           -0           -0  8.31043e-05 -6.31994e-19  2.91327e-19
          -0           -0           -0 -6.31994e-19  8.31043e-05  -3.0703e-19
          -0           -0           -0  2.91327e-19  -3.0703e-19  8.31043e-05
blockIndices 1 spinv.rows() 110  
  0.00358579  9.11838e-05 -7.55971e-10   1.2156e-10  1.49195e-09   0.00050024
 9.11838e-05   0.00381645 -6.83577e-11 -1.89299e-09 -8.50372e-11  0.000985436
-7.55971e-10 -6.83577e-11   0.00402816 -0.000487069  -0.00102642  2.41365e-11
  1.2156e-10 -1.89299e-09 -0.000487069   0.00305717 -0.000222169 -1.14008e-09
 1.49195e-09 -8.50372e-11  -0.00102642 -0.000222169   0.00324033 -3.39539e-10
  0.00050024  0.000985436  2.41365e-11 -1.14008e-09 -3.39539e-10   0.00280544

And next loop closure

blockIndices 1 spinv.rows() 117
  0.00358579  9.11838e-05 -7.55971e-10   1.2156e-10  1.49195e-09   0.00050024
 9.11838e-05   0.00381645 -6.83577e-11 -1.89299e-09 -8.50372e-11  0.000985436
-7.55971e-10 -6.83577e-11   0.00402816 -0.000487069  -0.00102642  2.41365e-11
  1.2156e-10 -1.89299e-09 -0.000487069   0.00305717 -0.000222169 -1.14008e-09
 1.49195e-09 -8.50372e-11  -0.00102642 -0.000222169   0.00324033 -3.39539e-10
  0.00050024  0.000985436  2.41365e-11 -1.14008e-09 -3.39539e-10   0.00280544

I haven't looked at that yet but to associate these matrices to the right node is there an easy way?

[eliabntt]

When a new node is added to the graph do you set an uncertainty for it? Or are there any other point where the previous nodes uncertainty is updated?

[eliabntt]

The problem for the camera I think that's related to the gazebo_realsense_plugin that publish every 5/6 Hz (not/compressed data) while the laser is publishing at 20.

I checked and it's a simulation problem. When the gazebo_realsense is alone it's publishing at 60 Hz when I place the env in gazebo drops down to 5/6

Moreover I think that with gazebo there is some delay between the action and the transmission of the tf transformation and this effects especially the rotational movements.