Transformations between Maps

[Kaifei]

Hi Mathieu,

    May I ask some questions about multi-map data manipulations?

    I believe that every map saved in one multi-map database file has its own coordinate system.
    My questions are:
    1. Is it true that two images taken at the same real-world location may have different poses in a database file if they are in two maps?
    2. Is the Link table the only information about the relative transformation between two maps?
    3. When I "Export 3D clouds (*.ply *.pcd *.obj)..." on a multi-map database, RTABMap must know the relative transformations between two maps to assemble all small clouds. Can you point me where the code figure out this relative transformation?
        In other words, if I have an image A in map A, and an image B in map B, how do I know Image B's pose in map A's coordinate system? (Assume there was at least one success loop closure detection)

    Thanks a lot!

Best,
Kaifei

[matlabbe]

Hi Kaifei,

Sure, it is the best place to ask them!

1. Yes, and even if they are in the same map (unless odometry is perfect).
2. The Link table contains all transformations between nodes in a map and between maps (on loop closures).
3. The relative transformation between two maps is implicitly saved in loop closure links. There could be many loop closures between two maps. On graph optimization, all nodes are transformed in the coordinate system of the first map if "RGBD/OptimizeFromGraphEnd=false" or the last map if "RGBD/OptimizeFromGraphEnd=true". In "rtabmap-databaseViewer", you can specify exactly the root node of the optimization under Graph View panel so that all maps/nodes are transformed accordingly to this node.

cheers,
Mathieu

[Kaifei]

Thanks!

One more question. What is the best practice to pick the parameters for "Optimizer::optimize(rootId, poses, constraints)"?

In particular, I did

std::map<int, int> ids = _memory->getNeighborsId(_memory->getLastSignatureId(), 0, 0);
 _memory->getMetricConstraints(uKeysSet(ids), poses, links);
rtabmap::Optimizer *graphOptimizer = rtabmap::Optimizer::create(rtabmap::Optimizer::kTypeTORO);
_optimizedPoses = graphOptimizer->optimize(poses.begin()->first, poses, links);

But it turns out the getLastSignatureId() doesn't have any neighbor in the graph extracted from the link table. So the ids contains only one id (the one returned by getLastSignatureId).

I then do

std::list<int> idList = uKeysList(_memory->getWorkingMem());
std::set<int> idSet(idList.begin(), idList.end());
_memory->getMetricConstraints(idSet, poses, links, lookInDatabase);
rtabmap::Optimizer *graphOptimizer = rtabmap::Optimizer::create(rtabmap::Optimizer::kTypeTORO);
_optimizedPoses = graphOptimizer->optimize(poses.begin()->first, poses, links);

which solves my particular problem, but it cannot guarantee that the poses.begin()->first will contain any neighbor on the TORO graph. So I'm wondering whether there is a better way to do this.

Thanks!

Kaifei

[matlabbe]

Hi,

You should use getLastWorkingSignature() instead of getLastSignatureId(). The second one gives the last signature ID in the database, not necessary the ID of the last working signature in the graph.

Example:

#include <rtabmap/core/Memory.h>
#include <rtabmap/core/Optimizer.h>

int main(int argc, char * argv[])
{
	rtabmap::Memory memory;
	memory.init(argv[1]);

	if(memory.getLastWorkingSignature())
	{
		// Get all IDs linked to last signature (including those in Long-Term Memory)
		std::map<int, int> ids = memory.getNeighborsId(memory.getLastWorkingSignature()->id(), 0, -1);

		// Get all metric constraints (the graph)
		std::map<int, rtabmap::Transform> poses;
		std::multimap<int, rtabmap::Link> links;
		memory.getMetricConstraints(uKeysSet(ids), poses, links, true);

		// Optimize the graph
		std::map<int, rtabmap::Transform> optimizedPoses;
		rtabmap::Optimizer * graphOptimizer = rtabmap::Optimizer::create(rtabmap::Optimizer::kTypeTORO);
		optimizedPoses = graphOptimizer->optimize(poses.begin()->first, poses, links);
		delete graphOptimizer;

		for(std::map<int, rtabmap::Transform>::iterator iter=optimizedPoses.begin();
				iter!=optimizedPoses.end();
				++iter)
		{
			printf("%d Old=%s New=%s\n",
					iter->first,
					poses.at(iter->first).prettyPrint().c_str(),
					iter->second.prettyPrint().c_str());
		}
	}
	return 0;
}

Usage (after saving a first mapping session with loop closures):

./test_optimize ~/Documents/RTAB-Map/160316-115530.db 
1 Old=xyz=0.000000,0.000000,0.000000 rpy=0.000000,-0.000000,0.000000 New=xyz=0.000000,0.000000,0.000000 rpy=0.000000,-0.000000,0.000000
3 Old=xyz=0.009583,0.038811,0.004814 rpy=0.044280,0.250965,-0.037186 New=xyz=0.009220,0.046719,0.006062 rpy=0.050656,0.251603,-0.040074
4 Old=xyz=0.054049,0.009059,0.190762 rpy=0.048593,0.216478,-0.098301 New=xyz=0.050297,0.014064,0.194745 rpy=0.055070,0.220339,-0.100015
5 Old=xyz=0.104435,0.073432,0.524227 rpy=-0.089166,0.116275,-0.279714 New=xyz=0.100829,0.074727,0.523914 rpy=-0.084367,0.120863,-0.281611
6 Old=xyz=-0.098225,0.130635,0.637186 rpy=-0.198295,0.096965,-0.749776 New=xyz=-0.065503,0.179841,0.664653 rpy=-0.195057,0.102473,-0.749617
7 Old=xyz=-0.239622,0.055265,0.705393 rpy=-0.248537,0.210517,-1.108849 New=xyz=-0.178571,0.141945,0.753733 rpy=-0.246415,0.216553,-1.106783
8 Old=xyz=-0.315898,0.037073,0.809141 rpy=-0.212447,0.176366,-1.658437 New=xyz=-0.234357,0.150315,0.872595 rpy=-0.213128,0.182820,-1.655681
9 Old=xyz=-0.359645,0.059857,0.814867 rpy=-0.047815,0.382038,-2.032077 New=xyz=-0.276804,0.174771,0.879470 rpy=-0.050987,0.387838,-2.030311
10 Old=xyz=-0.416682,0.087166,0.833266 rpy=0.063322,0.363883,-2.284481 New=xyz=-0.333292,0.202637,0.898508 rpy=0.058737,0.368785,-2.283131
11 Old=xyz=-0.482870,0.199329,0.864908 rpy=0.182388,0.261932,-2.847941 New=xyz=-0.398467,0.316204,0.931865 rpy=0.175931,0.263845,-2.846549
12 Old=xyz=-0.465834,0.285009,0.839882 rpy=0.303791,0.350022,2.913938 New=xyz=-0.380522,0.403673,0.908253 rpy=0.296999,0.348617,2.914751
13 Old=xyz=-0.373509,0.312933,0.793595 rpy=0.398224,0.488842,2.371866 New=xyz=-0.273315,0.452465,0.873313 rpy=0.390946,0.485395,2.374398
14 Old=xyz=-0.337808,0.312556,0.776407 rpy=0.350787,0.563007,1.955879 New=xyz=-0.234037,0.457195,0.858802 rpy=0.345319,0.557276,1.959118
15 Old=xyz=-0.287325,0.247122,0.787802 rpy=0.246143,0.650560,1.397846 New=xyz=-0.168680,0.411464,0.880613 rpy=0.244385,0.643040,1.403808
16 Old=xyz=-0.231045,0.136714,0.729107 rpy=0.019377,0.813038,0.866136 New=xyz=-0.106991,0.307872,0.824555 rpy=0.022980,0.805697,0.876023
17 Old=xyz=-0.204746,0.079393,0.732094 rpy=-0.124173,0.700042,0.479095 New=xyz=-0.079866,0.251285,0.827508 rpy=-0.117568,0.694161,0.490639
18 Old=xyz=-0.194332,0.054904,0.714525 rpy=-0.126338,0.456595,0.235491 New=xyz=-0.068586,0.227858,0.810256 rpy=-0.119287,0.452086,0.245925
19 Old=xyz=-0.150904,0.038342,0.664988 rpy=-0.124679,0.354286,-0.009269 New=xyz=-0.028765,0.208895,0.752597 rpy=-0.115207,0.349421,0.003330
20 Old=xyz=-0.115797,-0.020118,0.548667 rpy=-0.190354,0.101407,-0.110893 New=xyz=0.007303,0.151753,0.632916 rpy=-0.180027,0.095762,-0.100418
21 Old=xyz=-0.110419,-0.046596,0.225344 rpy=-0.123067,-0.037278,-0.232000 New=xyz=0.015289,0.128546,0.306483 rpy=-0.111086,-0.043155,-0.222906
22 Old=xyz=-0.110438,-0.082432,-0.053128 rpy=-0.027026,-0.029700,-0.229217 New=xyz=0.016357,0.095053,0.028916 rpy=-0.015095,-0.034485,-0.219533

cheers

[Kaifei]

Thanks!

I found that ids in your code doesn't always contain ALL IDs I have in the database. Does that mean I'd better NOT use the poses whose IDs are NOT in ids? ( because their poses cannot be optimized )

Kaifei

[matlabbe]

Nodes merged on Weight Update (or Rehearsal) or deleted because of small movements are still saved in the database for debugging purpose (Mem/NotLinkedNodesKept=true) but not linked to graph anymore. These nodes can be safely ignored for graph optimization.

If you want to save some space in the resulting database, you may want to set parameter "Mem/NotLinkedNodesKept" to false. In Preferences dialog, it is under Database panel.

cheers

[Kaifei]

I have another question.

After I call _memory->update(sensorData), _memory->getLastWorkingSignature()->id() is not contained in the keys of _memory->getWorkingMem(). Based on the function names, shouldn't the last working signature's ID be contained in the IDs of working memory?
I do notice that the last working signature, which is what I added, was labelled map1, and I think all other signatures are in map0. And I added an arbitrary sensorData, which won't have a Link with nodes on map0.

Thanks,
Kaifei

[matlabbe]

Hi Kaifei,

The newly added node should be contained in STM (as well as the previous 9 added by default). So use Memory::getStMem() instead to get the latest added IDs to memory.

In general, nodes in the local map are in WM+STM (RAM). Other nodes are in LTM (database). On start, RTAB-Map always increments the map ID. If a loop closure can be detected on start, the map1 will be linked to map0 so that the local map will contain nodes of both sessions.

cheers

[Kaifei]

Hi Mathieu,

     If I do graph optimization on a database with two maps (map 1 and map 2).
     But there is no link between these two maps,
     what would be the coordinate system for the nodes in map 2 after optimization?
     Is it the first node's coordinate system in map 2?
     Thanks!

Best,
Kaifei

[matlabbe]

Hi,

If the graph is optimized from a node of map 1, then nodes from map 2 will not be in the global map, and vice-versa. If you want to show both maps at the same time (with an offset), you may want to add a fake link (e.g., with 50 meters offset on x) between a node from map 1 and a node from map2.

Adding a link with python following the database schema for Link table:

# Transformation Matrix (3x4): [r11 r12 r13 o1; r21 r22 r23 o2; r31 r32 r33 o3] 
# Set high variance 9999
# Add link between 1 of map 1 and 77 of map 2
# Type of the link is "user link"=4

t = [ 1, 0, 0, 50, 0, 1, 0, 0, 0, 0, 1, 0]
hexStr = ''.join(float_hex4(x) for x in t)
call(["sqlite3", database_path, 'insert into Link (from_id, to_id, type, rot_variance, trans_variance, transform) values(1, 77, 4, 9999, 9999, X\'' + hexStr + '\';'])

# inverse
t = [ 1, 0, 0, -50, 0, 1, 0, 0, 0, 0, 1, 0]
hexStr = ''.join(float_hex4(x) for x in t)
call(["sqlite3", database_path, 'insert into Link (from_id, to_id, type, rot_variance, trans_variance, transform) values(77, 1, 4, 9999, 9999, X\'' + hexStr + '\';'])

To remove a link, you can do that with the database viewer or using sqlite3 as well:

$ sqlite3 "database_path" "DELETE FROM Link WHERE from_id=1 and to_id=77"
$ sqlite3 "database_path" "DELETE FROM Link WHERE from_id=77 and to_id=1"

In database viewer under Graph view, we can set the root of the graph so that corresponding map will be optimized.

cheers,
Mathieu