Fixed export RGB-D SLAM format

corelib/src/Graph.cpp

@@ -108,19 +108,29 @@ bool exportPoses(
 			{
 				if(format == 1) // rgbd-slam format
 				{
-					// Format: stamp x y z qw qx qy qz
+					// put the pose in rgbd-slam world reference
-					Eigen::Quaternionf q = (*iter).second.getQuaternionf();
+					Transform t( 0, 0, 1, 0,
+								 0, -1, 0, 0,
+								 1, 0, 0, 0);
+					Transform pose = t.inverse() * iter->second;
+					t = Transform( 0, 0, 1, 0,
+								  -1, 0, 0, 0,
+								   0,-1, 0, 0);
+					pose = t.inverse() * pose * t;
+
+					// Format: stamp x y z qx qy qz qw
+					Eigen::Quaternionf q = pose.getQuaternionf();
 
 					UASSERT(uContains(stamps, iter->first));
 					fprintf(fout, "%f %f %f %f %f %f %f %f\n",
 							stamps.at(iter->first),
-							(*iter).second.x(),
+							pose.x(),
-							(*iter).second.y(),
+							pose.y(),
-							(*iter).second.z(),
+							pose.z(),
-							q.w(),
 							q.x(),
 							q.y(),
-							q.z());
+							q.z(),
+							q.w());
 				}
 				else // default / KITTI format
 				{

corelib/src/Rtabmap.cpp

@@ -1153,7 +1153,7 @@ bool Rtabmap::process(
 		if(_proximityByTime &&
 		   rehearsedId == 0 && // don't do it if rehearsal happened
 		   signature->getWords3().size() &&
-		   _memory->isIncremental(), // don't do it in localization mode
+		   _memory->isIncremental() && // don't do it in localization mode
 		   !signature->isBadSignature() &&
 		   signature->getWeight()>=0)
 		{