Rtabmap: when optimizing graph, removed guess for rootid to avoid global rotation drift over time. CameraStereoImages: set BayerMode also to right image. GraphView: fixed 0 width for line inside NodeItem. Reprocess: added -loc_null and -gt options.

corelib/src/Odometry.cpp

@@ -303,9 +303,6 @@ Transform Odometry::process(SensorData & data, const Transform & guessIn, Odomet
 					orientation*
 					data.imu().localTransform().rotation().inverse();
 
-			IMU imu2 = data.imu();
-			imu2.convertToBaseFrame();
-
 			if(	this->getPose().r11() == 1.0f && this->getPose().r22() == 1.0f && this->getPose().r33() == 1.0f &&
 				this->framesProcessed() == 0)
 			{

corelib/src/Rtabmap.cpp

@@ -4418,9 +4418,9 @@ std::map<int, Transform> Rtabmap::optimizeGraph(
 	{
 		for(std::map<int, Transform>::iterator iter=poses.begin(); iter!=poses.end(); ++iter)
 		{
-			// Apply guess poses (if some)
+			// Apply guess poses (if some), ignore for rootid to avoid origin drifting
 			std::map<int, Transform>::const_iterator foundGuess = guessPoses.find(iter->first);
-			if(foundGuess!=guessPoses.end())
+			if(foundGuess!=guessPoses.end() && iter->first != fromId)
 			{
 				iter->second = foundGuess->second;
 			}

corelib/src/camera/CameraImages.cpp

@@ -862,7 +862,7 @@ SensorData CameraImages::captureImage(CameraInfo * info)
 						cv::cvtColor(img, out, CV_BGRA2BGR);
 						img = out;
 					}
-					else if(_bayerMode >= 0 && _bayerMode <=3)
+					else if(!img.empty() && _bayerMode >= 0 && _bayerMode <=3)
 					{
 						cv::Mat debayeredImg;
 						try

corelib/src/camera/CameraStereoImages.cpp

@@ -166,6 +166,7 @@ SensorData CameraStereoImages::captureImage(CameraInfo * info)
 	{
 		if(camera2_)
 		{
+			camera2_->setBayerMode(this->getBayerMode());
 			right = camera2_->takeImage(info);
 		}
 		else

guilib/src/GraphViewer.cpp

@@ -67,7 +67,7 @@ class NodeItem: public QGraphicsEllipseItem
 {
 public:
 	// in meter
-	NodeItem(int id, int mapId, const Transform & pose, float radius, int weight, GraphViewer::ViewPlane plane) :
+	NodeItem(int id, int mapId, const Transform & pose, float radius, int weight, GraphViewer::ViewPlane plane, float linkWidth) :
 		QGraphicsEllipseItem(QRectF(-radius*100.0f,-radius*100.0f,radius*100.0f*2.0f,radius*100.0f*2.0f)),
 		_id(id),
 		_mapId(mapId),
@@ -82,6 +82,9 @@ public:
 		pose.getEulerAngles(r, p, yaw);
 		radius*=100.0f;
 		_line = new QGraphicsLineItem(0,0,-radius*sin(yaw),-radius*cos(yaw), this);
+		QPen pen = _line->pen();
+		pen.setWidth(linkWidth*100.0f);
+		_line->setPen(pen);
 	}
 	virtual ~NodeItem() {}
 
@@ -94,7 +97,9 @@ public:
 		b.setColor(color);
 		this->setBrush(b);
 
-		_line->setPen(QPen(QColor(255-color.red(), 255-color.green(), 255-color.blue())));
+		QPen pen = _line->pen();
+		pen.setColor(QColor(255-color.red(), 255-color.green(), 255-color.blue()));
+		_line->setPen(pen);
 	}
 
 	void setRadius(float radius)
@@ -157,8 +162,8 @@ private:
 class NodeGPSItem: public NodeItem
 {
 public:
-	NodeGPSItem(int id, int mapId, const Transform & pose, float radius, const GPS & gps, GraphViewer::ViewPlane plane) :
+	NodeGPSItem(int id, int mapId, const Transform & pose, float radius, const GPS & gps, GraphViewer::ViewPlane plane, float linkWidth) :
-		NodeItem(id, mapId, pose, radius, -1, plane),
+		NodeItem(id, mapId, pose, radius, -1, plane, linkWidth),
 		_gps(gps)
 	{
 	}
@@ -490,7 +495,7 @@ void GraphViewer::updateGraph(const std::map<int, Transform> & poses,
 			{
 				// create node item
 				const Transform & pose = iter->second;
-				NodeItem * item = new NodeItem(iter->first, uContains(mapIds, iter->first)?mapIds.at(iter->first):-1, pose, _nodeRadius, uContains(weights, iter->first)?weights.at(iter->first):-1, _viewPlane);
+				NodeItem * item = new NodeItem(iter->first, uContains(mapIds, iter->first)?mapIds.at(iter->first):-1, pose, _nodeRadius, uContains(weights, iter->first)?weights.at(iter->first):-1, _viewPlane, _linkWidth);
 				this->scene()->addItem(item);
 				item->setZValue(iter->first<0?21:20);
 				item->setColor(iter->first<0?QColor(255-_nodeColor.red(), 255-_nodeColor.green(), 255-_nodeColor.blue()):_nodeColor);
@@ -710,7 +715,7 @@ void GraphViewer::updateGTGraph(const std::map<int, Transform> & poses)
 			{
 				// create node item
 				const Transform & pose = iter->second;
-				NodeItem * item = new NodeItem(iter->first, -1, pose, _nodeRadius, -1, _viewPlane);
+				NodeItem * item = new NodeItem(iter->first, -1, pose, _nodeRadius, -1, _viewPlane, _linkWidth);
 				this->scene()->addItem(item);
 				item->setZValue(20);
 				item->setColor(_gtPathColor);
@@ -745,15 +750,29 @@ void GraphViewer::updateGTGraph(const std::map<int, Transform> & poses)
 				}
 				if(linkItem == 0)
 				{
-					//create a link item
+					bool linkFound = iter->first - iterPrevious->first == 1; // if consecutive, add link
-					linkItem = new LinkItem(iterPrevious->first, iter->first, previousPose, currentPose, Link(), 1, _viewPlane);
+					for(QMultiMap<int, LinkItem*>::iterator kter = _linkItems.find(iterPrevious->first);
-					QPen p = linkItem->pen();
+							kter!=_linkItems.end() && kter.key()==iterPrevious->first && !linkFound;
-					p.setWidthF(_linkWidth*100.0f);
+							++kter)
-					linkItem->setPen(p);
+					{
-					linkItem->setZValue(10);
+						if(kter.value()->from() == iterPrevious->first && kter.value()->to() == iter->first)
-					this->scene()->addItem(linkItem);
+						{
-					linkItem->setParentItem(_gtGraphRoot);
+							linkFound = true;
-					_gtLinkItems.insert(iterPrevious->first, linkItem);
+						}
+					}
+
+					if(linkFound)
+					{
+						//create a link item
+						linkItem = new LinkItem(iterPrevious->first, iter->first, previousPose, currentPose, Link(), 1, _viewPlane);
+						QPen p = linkItem->pen();
+						p.setWidthF(_linkWidth*100.0f);
+						linkItem->setPen(p);
+						linkItem->setZValue(10);
+						this->scene()->addItem(linkItem);
+						linkItem->setParentItem(_gtGraphRoot);
+						_gtLinkItems.insert(iterPrevious->first, linkItem);
+					}
 				}
 				if(linkItem)
 				{
@@ -840,7 +859,7 @@ void GraphViewer::updateGPSGraph(
 				// create node item
 				const Transform & pose = iter->second;
 				UASSERT(gpsValues.find(iter->first) != gpsValues.end());
-				NodeItem * item = new NodeGPSItem(iter->first, -1, pose, _nodeRadius, gpsValues.at(iter->first), _viewPlane);
+				NodeItem * item = new NodeGPSItem(iter->first, -1, pose, _nodeRadius, gpsValues.at(iter->first), _viewPlane, _linkWidth);
 				this->scene()->addItem(item);
 				item->setZValue(20);
 				item->setColor(_gpsPathColor);

tools/Reprocess/main.cpp

@@ -55,7 +55,8 @@ void showUsage()
 			"   rtabmap-reprocess [options] \"input1.db;input2.db;input3.db\" \"output.db\"\n"
 			"\n"
 			"   For the second example, only parameters from the first database are used.\n"
-			"   If Mem/IncrementalMemory is false, RTAB-Map is initialized with the first input database.\n"
+			"   If Mem/IncrementalMemory is false, RTAB-Map is initialized with the first input database,\n"
+			"   then localization-only is done with next databases against the first one.\n"
 			"   To see warnings when loop closures are rejected, add \"--uwarn\" argument.\n"
 			"   To upgrade version of an old database to newest version:\n"
 			"      rtabmap-reprocess --Db/TargetVersion \"\" \"input.db\" \"output.db\"\n"
@@ -68,6 +69,9 @@ void showUsage()
 			"                       arguments, they overwrite those in config file and the database.\n"
 			"     -start #    Start from this node ID.\n"
 			"     -stop #     Last node to process.\n"
+			"     -loc_null   On localization mode, reset localization pose to null and map correction to identity between sessions.\n"
+			"     -gt         When reprocessing a single database, load its original optimized graph, then \n"
+			"                 set it as ground truth for output database. If there was a ground truth in the input database, it will be ignored.\n"
 			"     -g2         Assemble 2D occupancy grid map and save it to \"[output]_map.pgm\". Use with -db to save in database.\n"
 			"     -g3         Assemble 3D cloud map and save it to \"[output]_map.pcd\".\n"
 			"     -o2         Assemble OctoMap 2D projection and save it to \"[output]_octomap.pgm\". Use with -db to save in database.\n"
@@ -221,6 +225,8 @@ int main(int argc, char * argv[])
 	int startId = 0;
 	int stopId = 0;
 	int framesToSkip = 0;
+	bool locNull = false;
+	bool originalGraphAsGT = false;
 	bool scanFromDepth = false;
 	int scanDecimation = 1;
 	float scanRangeMin = 0.0f;
@@ -297,6 +303,16 @@ int main(int argc, char * argv[])
 				showUsage();
 			}
 		}
+		else if(strcmp(argv[i], "-loc_null") == 0 || strcmp(argv[i], "--loc_null") == 0)
+		{
+			locNull = true;
+			printf("In localization mode, when restarting a new session, the current localization pose is set to null (-loc_null option).\n");
+		}
+		else if(strcmp(argv[i], "-gt") == 0 || strcmp(argv[i], "--gt") == 0)
+		{
+			originalGraphAsGT = true;
+			printf("Original graph is used as ground truth for output database (-gt option).\n");
+		}
 		else if(strcmp(argv[i], "-p") == 0 || strcmp(argv[i], "--p") == 0)
 		{
 			exportPoses = true;
@@ -529,6 +545,13 @@ int main(int argc, char * argv[])
 	{
 		totalIds = ids.size();
 	}
+
+	std::map<int, Transform> gt;
+	if(databases.size() == 1 && originalGraphAsGT)
+	{
+		gt = dbDriver->loadOptimizedPoses();
+	}
+
 	dbDriver->closeConnection(false);
 
 	// Count remaining ids in the other databases
@@ -573,6 +596,11 @@ int main(int argc, char * argv[])
 	Rtabmap rtabmap;
 	rtabmap.init(parameters, outputDatabasePath);
 
+	if(!incrementalMemory && locNull)
+	{
+		rtabmap.setInitialPose(Transform());
+	}
+
 	bool rgbdEnabled = Parameters::defaultRGBDEnabled();
 	Parameters::parse(parameters, Parameters::kRGBDEnabled(), rgbdEnabled);
 	bool odometryIgnored = !rgbdEnabled;
@@ -623,8 +651,18 @@ int main(int argc, char * argv[])
 					lastLocalizationOdomPose = info.odomPose;
 				}
 				rtabmap.triggerNewMap();
+				if(!incrementalMemory && locNull)
+				{
+					rtabmap.setInitialPose(Transform());
+				}
 				inMotion = true;
 			}
+
+			if(originalGraphAsGT)
+			{
+				data.setGroundTruth(gt.find(data.id()) != gt.end()?gt.at(data.id()):Transform());
+			}
+
 			UTimer t;
 			if(!rtabmap.process(data, info.odomPose, info.odomCovariance, info.odomVelocity, globalMapStats))
 			{