Added Map color overlay on 3D cloud (press key '2' on keyboard)

Fixed loading all node ids from database, ignoring children

git-svn-id: http://rtabmap.googlecode.com/svn/trunk/rtabmap@1813 f169173b-cf89-36c8-b27e-44dbe73f0c83

corelib/include/rtabmap/core/DBDriver.h

@@ -102,7 +102,7 @@ public:
 	void loadNeighbors(int signatureId, std::map<int, Transform> & neighbors) const;
 	void loadLoopClosures(int signatureId, std::map<int, Transform> & loopIds, std::map<int, Transform> & childIds) const;
 	void getWeight(int signatureId, int & weight) const;
-	void getAllNodeIds(std::set<int> & ids) const;
+	void getAllNodeIds(std::set<int> & ids, bool ignoreChildren = false) const;
 	void getLastNodeId(int & id) const;
 	void getLastWordId(int & id) const;
 	void getInvertedIndexNi(int signatureId, int & ni) const;
@@ -138,7 +138,7 @@ private:
 	virtual void getNodeDataQuery(int signatureId, std::vector<unsigned char> & image, std::vector<unsigned char> & depth, std::vector<unsigned char> & depth2d, float & fx, float & fy, float & cx, float & cy, Transform & localTransform) const = 0;
 	virtual void getNodeDataQuery(int signatureId, std::vector<unsigned char> & image) const = 0;
 	virtual void getPoseQuery(int signatureId, Transform & pose, int & mapId) const = 0;
-	virtual void getAllNodeIdsQuery(std::set<int> & ids) const = 0;
+	virtual void getAllNodeIdsQuery(std::set<int> & ids, bool ignoreChildren) const = 0;
 	virtual void getLastIdQuery(const std::string & tableName, int & id) const = 0;
 	virtual void getInvertedIndexNiQuery(int signatureId, int & ni) const = 0;
 

corelib/src/DBDriver.cpp

@@ -453,10 +453,10 @@ void DBDriver::loadLoopClosures(int signatureId, std::map<int, Transform> & loop
 }
 
 //TODO Check also in the trash ?
-void DBDriver::getAllNodeIds(std::set<int> & ids) const
+void DBDriver::getAllNodeIds(std::set<int> & ids, bool ignoreChildren) const
 {
 	_dbSafeAccessMutex.lock();
-	this->getAllNodeIdsQuery(ids);
+	this->getAllNodeIdsQuery(ids, ignoreChildren);
 	_dbSafeAccessMutex.unlock();
 }
 

corelib/src/DBDriverSqlite3.cpp

@@ -806,7 +806,7 @@ void DBDriverSqlite3::getPoseQuery(int signatureId, Transform & pose, int & mapI
 }
 
 
-void DBDriverSqlite3::getAllNodeIdsQuery(std::set<int> & ids) const
+void DBDriverSqlite3::getAllNodeIdsQuery(std::set<int> & ids, bool ignoreChildren) const
 {
 	if(_ppDb)
 	{
@@ -816,9 +816,21 @@ void DBDriverSqlite3::getAllNodeIdsQuery(std::set<int> & ids) const
 		sqlite3_stmt * ppStmt = 0;
 		std::stringstream query;
 
-		query << "SELECT id "
+		if(!ignoreChildren)
-			  << "FROM Node "
+		{
-			  << "ORDER BY id";
+			query << "SELECT id "
+				  << "FROM Node "
+				  << "ORDER BY id";
+		}
+		else
+		{
+			query << "SELECT id "
+				  << "FROM Node "
+				  << "LEFT OUTER JOIN Link "
+				  << "ON id = from_id "
+				  << "WHERE type!=1 "
+				  << "ORDER BY id";
+		}
 
 		rc = sqlite3_prepare_v2(_ppDb, query.str().c_str(), -1, &ppStmt, 0);
 		UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());

corelib/src/DBDriverSqlite3.h

@@ -87,7 +87,7 @@ private:
 			Transform & localTransform) const;
 	virtual void getNodeDataQuery(int signatureId, std::vector<unsigned char> & image) const;
 	virtual void getPoseQuery(int signatureId, Transform & pose, int & mapId) const;
-	virtual void getAllNodeIdsQuery(std::set<int> & ids) const;
+	virtual void getAllNodeIdsQuery(std::set<int> & ids, bool ignoreChildren) const;
 	virtual void getLastIdQuery(const std::string & tableName, int & id) const;
 	virtual void getInvertedIndexNiQuery(int signatureId, int & ni) const;
 

corelib/src/Memory.cpp

@@ -160,7 +160,7 @@ bool Memory::init(const std::string & dbUrl, bool dbOverwritten, const Parameter
 			{
 				if(postInitEvents) UEventsManager::post(new RtabmapEventInit(std::string("Loading all nodes to WM...")));
 				std::set<int> ids;
-				_dbDriver->getAllNodeIds(ids);
+				_dbDriver->getAllNodeIds(ids, true);
 				_dbDriver->loadSignatures(std::list<int>(ids.begin(), ids.end()), dbSignatures);
 			}
 			else

guilib/include/rtabmap/gui/CloudViewer.h

@@ -80,28 +80,33 @@ public:
 	bool addOrUpdateCloud(
 		const std::string & id,
 		const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
-		const Transform & pose = Transform::getIdentity());
+		const Transform & pose = Transform::getIdentity(),
+		const QColor & color = Qt::gray);
 
 	bool addOrUpdateCloud(
 			const std::string & id,
 			const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
-			const Transform & pose = Transform::getIdentity());
+			const Transform & pose = Transform::getIdentity(),
+			const QColor & color = Qt::gray);
 
 	bool addCloud(
 			const std::string & id,
 			const pcl::PCLPointCloud2Ptr & binaryCloud,
 			const Transform & pose,
-			bool rgb);
+			bool rgb,
+			const QColor & color = Qt::gray);
 
 	bool addCloud(
 		const std::string & id,
 		const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
-		const Transform & pose = Transform::getIdentity());
+		const Transform & pose = Transform::getIdentity(),
+		const QColor & color = Qt::gray);
 
 	bool addCloud(
 		const std::string & id,
 		const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
-		const Transform & pose = Transform::getIdentity());
+		const Transform & pose = Transform::getIdentity(),
+		const QColor & color = Qt::gray);
 
 	bool addCloudMesh(
 		const std::string & id,

guilib/include/rtabmap/gui/MainWindow.h

@@ -255,6 +255,7 @@ private:
 	QMap<int, float> _depthFysMap;
 	QMap<int, float> _depthCxsMap;
 	QMap<int, float> _depthCysMap;
+	QMap<int, int> _mapIds;
 	QMap<int, Transform> _localTransformsMap;
 	std::map<int, Transform> _currentPosesMap;
 	QMap<int, pcl::PointCloud<pcl::PointXYZRGB>::Ptr > _createdClouds;

guilib/src/CloudViewer.cpp

@@ -209,11 +209,12 @@ bool CloudViewer::updateCloud(
 bool CloudViewer::addOrUpdateCloud(
 		const std::string & id,
 		const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
-		const Transform & pose)
+		const Transform & pose,
+		const QColor & color)
 {
 	if(!updateCloud(id, cloud, pose))
 	{
-		return addCloud(id, cloud, pose);
+		return addCloud(id, cloud, pose, color);
 	}
 	return true;
 }
@@ -221,11 +222,12 @@ bool CloudViewer::addOrUpdateCloud(
 bool CloudViewer::addOrUpdateCloud(
 		const std::string & id,
 		const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
-		const Transform & pose)
+		const Transform & pose,
+		const QColor & color)
 {
 	if(!updateCloud(id, cloud, pose))
 	{
-		return addCloud(id, cloud, pose);
+		return addCloud(id, cloud, pose, color);
 	}
 	return true;
 }
@@ -234,7 +236,8 @@ bool CloudViewer::addCloud(
 		const std::string & id,
 		const pcl::PCLPointCloud2Ptr & binaryCloud,
 		const Transform & pose,
-		bool rgb)
+		bool rgb,
+		const QColor & color)
 {
 	if(!_addedClouds.contains(id))
 	{
@@ -247,7 +250,7 @@ bool CloudViewer::addCloud(
 		 if(_visualizer->addPointCloud (binaryCloud, colorHandler, origin, orientation, id))
 		{
 			 // white
-			colorHandler.reset (new pcl::visualization::PointCloudColorHandlerCustom<pcl::PCLPointCloud2> (binaryCloud, 255, 255, 255));
+			colorHandler.reset (new pcl::visualization::PointCloudColorHandlerCustom<pcl::PCLPointCloud2> (binaryCloud, color.red(), color.green(), color.blue()));
 			_visualizer->addPointCloud (binaryCloud, colorHandler, origin, orientation, id);
 
 			// x,y,z
@@ -277,7 +280,8 @@ bool CloudViewer::addCloud(
 bool CloudViewer::addCloud(
 		const std::string & id,
 		const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
-		const Transform & pose)
+		const Transform & pose,
+		const QColor & color)
 {
 	if(!_addedClouds.contains(id))
 	{
@@ -285,7 +289,7 @@ bool CloudViewer::addCloud(
 
 		pcl::PCLPointCloud2Ptr binaryCloud(new pcl::PCLPointCloud2);
 		pcl::toPCLPointCloud2(*cloud, *binaryCloud);
-		return addCloud(id, binaryCloud, pose, true);
+		return addCloud(id, binaryCloud, pose, true, color);
 	}
 	return false;
 }
@@ -293,7 +297,8 @@ bool CloudViewer::addCloud(
 bool CloudViewer::addCloud(
 		const std::string & id,
 		const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
-		const Transform & pose)
+		const Transform & pose,
+		const QColor & color)
 {
 	if(!_addedClouds.contains(id))
 	{
@@ -301,7 +306,7 @@ bool CloudViewer::addCloud(
 
 		pcl::PCLPointCloud2Ptr binaryCloud(new pcl::PCLPointCloud2);
 		pcl::toPCLPointCloud2(*cloud, *binaryCloud);
-		return addCloud(id, binaryCloud, pose, false);
+		return addCloud(id, binaryCloud, pose, false, color);
 	}
 	return false;
 }

guilib/src/DatabaseViewer.cpp

@@ -781,7 +781,13 @@ void DatabaseViewer::view3DMap()
 
 							cloud = rtabmap::util3d::transformPointCloud(cloud, localTransform);
 
-							viewer->addCloud(uFormat("cloud%d", iter->first), cloud, pose);
+							QColor color = Qt::red;
+							int mapId = memory_->getMapId(iter->first);
+							if(mapId >= 0)
+							{
+								color = (Qt::GlobalColor)(mapId % 12 + 7 );
+							}
+							viewer->addCloud(uFormat("cloud%d", iter->first), cloud, pose, color);
 
 							UINFO("Generated %d (%d points)", iter->first, cloud->size());
 							progressDialog.appendText(QString("Generated %1 (%2 points)").arg(iter->first).arg(cloud->size()));

guilib/src/MainWindow.cpp

@@ -747,6 +747,14 @@ void MainWindow::processStats(const rtabmap::Statistics & stat)
 					_depths2DMap.insert(iter->first, iter->second);
 				}
 			}
+
+			// map ids
+			for(std::map<int, int>::const_iterator iter = stat.getMapIds().begin();
+				iter != stat.getMapIds().end();
+				++iter)
+			{
+				_mapIds.insert(iter->first, iter->second);
+			}
 		}
 
 		int rehearsed = (int)uValue(stat.data(), Statistics::kMemoryRehearsal_merged(), 0.0f);
@@ -1335,7 +1343,13 @@ void MainWindow::createAndAddCloudToMap(int nodeId, const Transform & pose)
 			cloud->clear();
 			pcl::copyPointCloud(*cloudWithNormals, *cloud);
 		}
-		if(!_ui->widget_cloudViewer->addOrUpdateCloud(cloudName, cloud, pose))
+		QColor color = Qt::gray;
+		int mapId = _mapIds.value(nodeId, -1);
+		if(mapId >= 0)
+		{
+			color = (Qt::GlobalColor)(mapId % 12 + 7 );
+		}
+		if(!_ui->widget_cloudViewer->addOrUpdateCloud(cloudName, cloud, pose, color))
 		{
 			UERROR("Adding cloud %d to viewer failed!", nodeId);
 		}
@@ -1360,6 +1374,12 @@ void MainWindow::createAndAddScanToMap(int nodeId, const Transform & pose)
 	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud;
 	cv::Mat depth2d = util3d::uncompressData(_depths2DMap.value(nodeId));
 	cloud = util3d::depth2DToPointCloud(depth2d);
+	QColor color = Qt::red;
+	int mapId = _mapIds.value(nodeId, -1);
+	if(mapId >= 0)
+	{
+		color = (Qt::GlobalColor)(mapId % 12 + 7 );
+	}
 	if(!_ui->widget_cloudViewer->addOrUpdateCloud(scanName, cloud, pose))
 	{
 		UERROR("Adding cloud %d to viewer failed!", nodeId);
@@ -1463,6 +1483,7 @@ void MainWindow::processRtabmapEvent3DMap(const rtabmap::RtabmapEvent3DMap & eve
 		UINFO(" depthFys = %d", event.getDepthFys().size());
 		UINFO(" depthCxs = %d", event.getDepthCxs().size());
 		UINFO(" depthCys = %d", event.getDepthCys().size());
+		UINFO(" map ids = %d", event.getMapIds().size());
 		UINFO(" localTransforms = %d", event.getLocalTransforms().size());
 		UINFO(" poses = %d", event.getPoses().size());
 		UINFO(" constraints = %d", event.getConstraints().size());
@@ -1523,6 +1544,15 @@ void MainWindow::processRtabmapEvent3DMap(const rtabmap::RtabmapEvent3DMap & eve
 		_initProgressDialog->appendText(tr("Inserted %1 depth cy parameters.").arg(_depthCysMap.size()));
 		_initProgressDialog->incrementStep();
 
+		for(std::map<int, int>::const_iterator iter = event.getMapIds().begin();
+			iter!=event.getMapIds().end();
+			++iter)
+		{
+			_mapIds.insert(iter->first, iter->second);
+		}
+		_initProgressDialog->appendText(tr("Inserted %1 map ids").arg(_mapIds.size()));
+		_initProgressDialog->incrementStep();
+
 		for(std::map<int, std::vector<unsigned char> >::const_iterator iter = event.getDepths2d().begin();
 			iter!=event.getDepths2d().end();
 			++iter)
@@ -2715,6 +2745,7 @@ void MainWindow::clearTheCache()
 	_depthFysMap.clear();
 	_depthCxsMap.clear();
 	_depthCysMap.clear();
+	_mapIds.clear();
 	_localTransformsMap.clear();
 	_createdClouds.clear();
 	_createdScans.clear();
@@ -3047,6 +3078,13 @@ void MainWindow::viewScans()
 		{
 			_initProgressDialog->appendText(tr("Viewing the scan %1 (%2 points)...").arg(iter->first).arg(iter->second->size()));
 			_initProgressDialog->incrementStep();
+
+			QColor color = Qt::red;
+			int mapId = _mapIds.value(iter->first, -1);
+			if(mapId >= 0)
+			{
+				color = (Qt::GlobalColor)(mapId % 12 + 7 );
+			}
 			viewer->addCloud(uFormat("cloud%d",iter->first), iter->second, iter->first>0?_currentPosesMap.at(iter->first):Transform::getIdentity());
 			_initProgressDialog->appendText(tr("Viewing the scan %1 (%2 points)... done.").arg(iter->first).arg(iter->second->size()));
 		}
@@ -3213,6 +3251,13 @@ void MainWindow::viewClouds()
 			{
 				_initProgressDialog->appendText(tr("Viewing the cloud %1 (%2 points)...").arg(iter->first).arg(iter->second->size()));
 				_initProgressDialog->incrementStep();
+
+				QColor color = Qt::gray;
+				int mapId = _mapIds.value(iter->first, -1);
+				if(mapId >= 0)
+				{
+					color = (Qt::GlobalColor)(mapId % 12 + 7 );
+				}
 				viewer->addCloud(uFormat("cloud%d",iter->first), iter->second, iter->first>0?_currentPosesMap.at(iter->first):Transform::getIdentity());
 				_initProgressDialog->appendText(tr("Viewing the cloud %1 (%2 points)... done.").arg(iter->first).arg(iter->second->size()));
 			}