Re: Transformations between Maps

Posted by matlabbe on
URL: http://official-rtab-map-forum.206.s1.nabble.com/Transformations-between-Maps-tp1126p1147.html

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