OctoMap rectification

[eliabntt]

Hey, sorry to bother you. I am planning to generate mine map data in addition to your octomap. To do so obviously I'll have to rectify the map when a loop closure is detected. I was wondering if you can point out where you are doing that for your maps, especially for the 3D ones.

Moreover, I'd like to have a look at where you project point clouds to these maps for visualization. I would like to have a representation for example of an open door that was previously closed or a moved chair. I know if that the background is feature rich this should be represented in the octomap and that there are some options to filter points out in your plugin. Maybe, a way to solve this would be to use only the last node in a nearby position for visualization and weight it much more in the maps building.

I've found this but without loosing hours maybe you can just point that out in a blink of an eye ;)

Thank for your patience
Best,
Elia

[matlabbe]

The code you linked is where the OctoMap is updated. In 3D Map view of the GUI, point clouds are added as new frames are coming. When a loop closure is detected, only the pose of the individual clouds are modified (the clouds are not regenerated, just moved) in MainWindow::updateMapCloud().

If you refer to ROS, for other other point cloud outputs, they are reassembled here in MapsManager.cpp when a loop closure happens.

Not sure to understand what you are trying to do though. Do you want to modify the resulting 2D occupancy grid map, the OctoMap or the 3D point clouds?

cheers,
Mathieu

[eliabntt]

Hey, thanks for your fast response. I'm trying to figure out how to save some additional information and keep it consistent with the loop closures/rectification.

In short, while moving, I'd like to save in memory, let's say for each added node, a feature "intensity" value (like #of features in that region or something similar) projected where the camera is looking at.
To do so I've thought either to save the nodeID (so to have updated x,y and yaw) and the # of features for that node, to create a new map (like an heatmap) or to add some info to an already existing map.

Right now, I'm starting to think that the best way is to save it through the nodeID but then I've to find a way to access the "nearby" nodes given the robot x,y position and keeping this operation computationally efficient and keep my list updated if old nodes get deleted.

[eliabntt]

Something like and "occupancy grid of features" or a "graph of # of features".

[matlabbe]

If the features are sparse (and unique), you may use the landmark mechanism in rtabmap.

If it is more semantic segmentation of the occupancy grid (those cells are a door, those a wall, those table's or chair's legs, those a stair, those a carpet, those are ceramic... ), then a dense layer like the costmap layer in move_base would be created. I tried to draw something (left is the standard obstacle map, on right a semantic map):

Indeed, when a loop closure happens, the semantic layer would have to be updated. For the costmaps in move_base, the obstacle and inflation layers are re-created from the updated occupancy grid from rtabmap, thus flushing updates from sensor that would have been done by move_base. If I understand, you want to save somekind of semantic for all points in the local occpuancy grids. The local occupancy grids are created here for each node. The resulting points in obstacle and ground can have colors, the color could be used to tell which cells are what.

Maybe not what you want exactly, but some ideas to look at: http://wustl.probablydavid.com/publications/IROS2014.pdf
http://wiki.ros.org/social_navigation_layers

[eliabntt]

Ok, I got your point but maybe I haven't explained myself.
This is my predicted workflow:

1. Start RTABMap with my RGBD cam
2. Move around, for each position added to the graph I can easily have a given number of features in that image [I already know where to catch this information]
3. Build some structure (list, map...) that contains such information

Thanks to that I'll be able to know "good" positions and "bad" positions. Good positions are ones where my number of features is high, or dense. Bad positions instead are for example white walls.
I don't know if in your database you already save this information somewhere (for each Node the relative number of features seen).
If not, I see three possible ways to solve this.

The first one is keeping some map <NodeID, numberOfFeatures>, go here or somewhere else, and while you are filtering out poses I can prune my eventually deleted Nodes from my list.

The second one is to save this information directly in the database. For each new node, I think you save x,y,yaw. If I can add the number of features would be straightforward to "navigate" nodes through links and get back this information.

The third one (but I think it's much more "difficult"), is to create some sort of 2D-local-heatmap where for each new node I generate a local "density" based on the number of features. Something like:

(don't mind the background)
But to do so I need either to rectify my map manually or to use in some way the first method. Then I can go to a high-density-region and query nearby nodes and get the orientation.

Still, I believe that the first or second approaches would be the best one.

[eliabntt]

I thought about this in the last week and I think we'll go with the first approach. This means we're going to save the couples <nodeID, number-of-features of the image associated with that node> somewhere.

The thing I'm interested into is what's in the GUI is called "keypoints count in the last signature" (that should be the number of total features that I have in it). I think I can get it directly with a listener on the topic /MapData -> keypoints -> and by counting them, or, with some (easy) coding, in the CoreWrapper.cpp (both in rtabmap_ros).

I'm now starting to think about what's the best way to do that in order to keep the save and load memory efficient. Are you already saving this information in the database? Or maybe, if for each image you are already saving the position of the features, I could directly use that.

As far as I understand for each node the system add you update the database with pictures, node transform with respect to the previous one and another bunch of information. I think that maybe the easiest and most efficient way would be to add this information in the database directly and then access it through a SELECT and the NODEID. Am I making myself clear? What do you think?

Surely I could simply create a dictionary in Python, checking if old nodes are being deleted updating the list and save that at the end of the run but I'd like to keep it consistent with your code.

[eliabntt]

I've another question for you. I've found this and it seems that in the field information you are saving the covariance of the transformation between two ids. Are these variances updated when a loop closure is detected? If so, in theory, I could use this as a real-time metric of the quality of my graph (including previous poses) and so I would have not only my current robot-covariance (through odometry) but also something map-related that is not given only when a loop closure is detected.

I know that in graph slam methods there is no such thing as "Map covariance" but I'm trying to find something similar.

I've seen also that by using GTSAM optimizer Marginals covariances should be updated for the whole graph. So for each pose of the graph we have an updated covariance when a loop closure is detected. Am I interpreting this rightly? (the same happens with g2o with the addition of a small for loop code)

[matlabbe]

The number of features in a node can be simply get from:

rtabmap/corelib/include/rtabmap/core/Signature.h

Line 112 in 0902ccc

const std::multimap<int, cv::KeyPoint> & getWords() const {return _words;}

This information is saved in the database, we can get them with:

$ sqlite3 rtabmap.db "SELECT count(word_id)  FROM Feature WHERE node_id = 1"

But from code, it is easier to use high level DBDriver interface by loading a Signature, then call Signature::getWords() as above. If you do your code in MainWindow, all feature information is kept in cache in:

rtabmap/guilib/include/rtabmap/gui/MainWindow.h

Line 280 in 0902ccc

const QMap<int, Signature> & cachedSignatures() const { return _cachedSignatures;}

To get covariance of each pose, you would have to use the marginals. I didn't with them a lot, currently only the last marginal is outputted. The Optimizer interface would need to be modified to get all marginals.

[eliabntt]

Yes, perfect. I've identified also a couple of other places where to retrieve this information but either the cached or database will be the fastest ones I think. As for the marginal computation following #98 and rtabmap_ros/#405 I've worked a bit on them.
What I've found is that with GTSAM it's already there so you can easily print them(hence expose them) with a simple for loop:

	gtsam::Marginals marginals(graph, optimizer->values());
	gtsam::Matrix info = marginals.marginalCovariance(poses.rbegin()->first);
	UDEBUG("Computed marginals = %fs (key=%d)", t.ticks(), poses.rbegin()->first);

	for(const std::pair<const int, Transform>& i: poses) {
                info = marginals.marginalCovariance(i.first);

                std::cout << "Computed marginals of node " << i.first << ":{\n"
                        << info(0,0) << " " << info(1,1) << " " << info(2,2) << " " 
			<< info(3,3) << " " << info(4,4) << " " << info(5,5) << "\n"
                        << "}\n";
		}

while for g2o it needs some more coding but I think that something like this

for (const std::pair<const int, Transform> &i: poses) {
     g2o::VertexSE3 *v = (g2o::VertexSE3 *) optimizer.vertex(i.first);
     g2o::SparseBlockMatrix<g2o::MatrixXD> spinv;
     optimizer.computeMarginals(spinv, v);

     if (v->hessianIndex() >= 0 && v->hessianIndex() < (int) spinv.blockCols().size()) {
          g2o::SparseBlockMatrix<g2o::MatrixXD>::SparseMatrixBlock *block = spinv.blockCols()[v->hessianIndex()].begin()->second;
          std::cout << "Computed marginals = " << i.first << " " << (*block)(0, 0) << "\n";
          } 
     else { std::cout << "negative index " << std::endl; }
}

should work even if probably I'd need to do that from the last pose backward and only if the last pose hessian index is not negative.
Do you agree?
By having this information should be straightforward to add that/update the one in the database.