DbViewer: added frame selection when exporting poses. DBDriver: added getLaserScanInfo().

corelib/include/rtabmap/core/DBDriver.h

@@ -131,6 +131,7 @@ public:
 	void loadNodeData(std::list<Signature *> & signatures, bool images = true, bool scan = true, bool userData = true, bool occupancyGrid = true) const;
 	void getNodeData(int signatureId, SensorData & data, bool images = true, bool scan = true, bool userData = true, bool occupancyGrid = true) const;
 	bool getCalibration(int signatureId, std::vector<CameraModel> & models, StereoCameraModel & stereoModel) const;
+	bool getLaserScanInfo(int signatureId, LaserScanInfo & info) const;
 	bool getNodeInfo(int signatureId, Transform & pose, int & mapId, int & weight, std::string & label, double & stamp, Transform & groundTruthPose, std::vector<float> & velocity) const;
 	void loadLinks(int signatureId, std::map<int, Link> & links, Link::Type type = Link::kUndef) const;
 	void getWeight(int signatureId, int & weight) const;
@@ -197,6 +198,7 @@ private:
 
 	virtual void loadNodeDataQuery(std::list<Signature *> & signatures, bool images=true, bool scan=true, bool userData=true, bool occupancyGrid=true) const = 0;
 	virtual bool getCalibrationQuery(int signatureId, std::vector<CameraModel> & models, StereoCameraModel & stereoModel) const = 0;
+	virtual bool getLaserScanInfoQuery(int signatureId, LaserScanInfo & info) const = 0;
 	virtual bool getNodeInfoQuery(int signatureId, Transform & pose, int & mapId, int & weight, std::string & label, double & stamp, Transform & groundTruthPose, std::vector<float> & velocity) const = 0;
 	virtual void getAllNodeIdsQuery(std::set<int> & ids, bool ignoreChildren, bool ignoreBadSignatures) const = 0;
 	virtual void getAllLinksQuery(std::multimap<int, Link> & links, bool ignoreNullLinks) const = 0;

corelib/src/DBDriver.cpp

@@ -632,6 +632,30 @@ bool DBDriver::getCalibration(
 	return found;
 }
 
+bool DBDriver::getLaserScanInfo(
+		int signatureId,
+		LaserScanInfo & info) const
+{
+	UDEBUG("");
+	bool found = false;
+	// look in the trash
+	_trashesMutex.lock();
+	if(uContains(_trashSignatures, signatureId))
+	{
+		info = _trashSignatures.at(signatureId)->sensorData().laserScanInfo();
+		found = true;
+	}
+	_trashesMutex.unlock();
+
+	if(!found)
+	{
+		_dbSafeAccessMutex.lock();
+		found = this->getLaserScanInfoQuery(signatureId, info);
+		_dbSafeAccessMutex.unlock();
+	}
+	return found;
+}
+
 bool DBDriver::getNodeInfo(
 		int signatureId,
 		Transform & pose,

corelib/src/DBDriverSqlite3.cpp

@@ -1437,6 +1437,70 @@ bool DBDriverSqlite3::getCalibrationQuery(
 	return found;
 }
 
+bool DBDriverSqlite3::getLaserScanInfoQuery(
+		int signatureId,
+		LaserScanInfo & info) const
+{
+	bool found = false;
+	if(_ppDb && signatureId)
+	{
+		int rc = SQLITE_OK;
+		sqlite3_stmt * ppStmt = 0;
+		std::stringstream query;
+
+		if(uStrNumCmp(_version, "0.11.10") >= 0)
+		{
+			query << "SELECT scan_info "
+				  << "FROM Data "
+				  << "WHERE id = " << signatureId
+				  <<";";
+		}
+		else
+		{
+			return false;
+		}
+
+		rc = sqlite3_prepare_v2(_ppDb, query.str().c_str(), -1, &ppStmt, 0);
+		UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
+
+		const void * data = 0;
+		int dataSize = 0;
+		Transform localTransform = Transform::getIdentity();
+		int maxPts = 0;
+		float maxRange = 0.0f;
+
+		// Process the result if one
+		rc = sqlite3_step(ppStmt);
+		if(rc == SQLITE_ROW)
+		{
+			found = true;
+			int index = 0;
+
+			// scan_info
+			data = sqlite3_column_blob(ppStmt, index);
+			dataSize = sqlite3_column_bytes(ppStmt, index++);
+
+			if(dataSize > 0 && data)
+			{
+				float * dataFloat = (float*)data;
+				memcpy(localTransform.data(), dataFloat+2, localTransform.size()*sizeof(float));
+				maxPts = (int)dataFloat[0];
+				maxRange = dataFloat[1];
+
+				info = LaserScanInfo(maxPts, maxRange, localTransform);
+			}
+
+			rc = sqlite3_step(ppStmt); // next result...
+		}
+		UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
+
+		// Finalize (delete) the statement
+		rc = sqlite3_finalize(ppStmt);
+		UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
+	}
+	return found;
+}
+
 bool DBDriverSqlite3::getNodeInfoQuery(int signatureId,
 		Transform & pose,
 		int & mapId,

corelib/src/DBDriverSqlite3.h

@@ -99,6 +99,7 @@ private:
 
 	virtual void loadNodeDataQuery(std::list<Signature *> & signatures, bool images=true, bool scan=true, bool userData=true, bool occupancyGrid=true) const;
 	virtual bool getCalibrationQuery(int signatureId, std::vector<CameraModel> & models, StereoCameraModel & stereoModel) const;
+	virtual bool getLaserScanInfoQuery(int signatureId, LaserScanInfo & info) const;
 	virtual bool getNodeInfoQuery(int signatureId, Transform & pose, int & mapId, int & weight, std::string & label, double & stamp, Transform & groundTruthPose, std::vector<float> & velocity) const;
 	virtual void getAllNodeIdsQuery(std::set<int> & ids, bool ignoreChildren, bool ignoreBadSignatures) const;
 	virtual void getAllLinksQuery(std::multimap<int, Link> & links, bool ignoreNullLinks) const;

guilib/src/DatabaseViewer.cpp

@@ -1632,10 +1632,115 @@ void DatabaseViewer::exportPoses(int format)
 
 	if(optimizedPoses.size())
 	{
+		std::map<int, Transform> localTransforms;
+		QStringList items;
+		items.push_back("Robot");
+		items.push_back("Camera");
+		items.push_back("Scan");
+		bool ok;
+		QString item = QInputDialog::getItem(this, tr("Export Poses"), tr("Frame: "), items, 0, false, &ok);
+		if(!ok || item.isEmpty())
+		{
+			return;
+		}
+		if(item.compare("Robot") != 0)
+		{
+			bool cameraFrame = item.compare("Camera") == 0;
+			for(std::map<int, Transform>::iterator iter=optimizedPoses.begin(); iter!=optimizedPoses.end(); ++iter)
+			{
+				Transform localTransform;
+				if(cameraFrame)
+				{
+					std::vector<CameraModel> models;
+					StereoCameraModel stereoModel;
+					if(dbDriver_->getCalibration(iter->first, models, stereoModel))
+					{
+						if((models.size() == 1 &&
+							!models.at(0).localTransform().isNull()))
+						{
+							localTransform = models.at(0).localTransform();
+						}
+						else if(!stereoModel.localTransform().isNull())
+						{
+							localTransform = stereoModel.localTransform();
+						}
+						else if(models.size()>1)
+						{
+							UWARN("Multi-camera is not supported (node %d)", iter->first);
+						}
+						else
+						{
+							UWARN("Calibration not valid for node %d", iter->first);
+						}
+					}
+					else
+					{
+						UWARN("Missing calibration for node %d", iter->first);
+					}
+				}
+				else
+				{
+					LaserScanInfo info;
+					if(dbDriver_->getLaserScanInfo(iter->first, info))
+					{
+						if(!info.localTransform().isNull())
+						{
+							localTransform = info.localTransform();
+						}
+						else
+						{
+							UWARN("Invalid scan info for node %d", iter->first);
+						}
+					}
+					else
+					{
+						UWARN("Missing scan info for node %d", iter->first);
+					}
+
+				}
+				if(!localTransform.isNull())
+				{
+					localTransforms.insert(std::make_pair(iter->first, localTransform));
+				}
+			}
+			if(localTransforms.empty())
+			{
+				QMessageBox::warning(this,
+						tr("Export Poses"),
+						tr("Could not find any \"%1\" frame, exporting in Robot frame instead.").arg(item));
+			}
+		}
+
+		std::map<int, Transform> poses;
+		std::multimap<int, Link> links;
+		if(localTransforms.empty())
+		{
+			poses = optimizedPoses;
+			links = graphLinks_;
+		}
+		else
+		{
+			//adjust poses and links
+			for(std::map<int, Transform>::iterator iter=localTransforms.begin(); iter!=localTransforms.end(); ++iter)
+			{
+				poses.insert(std::make_pair(iter->first, optimizedPoses.at(iter->first) * iter->second));
+			}
+			for(std::multimap<int, Link>::iterator iter=graphLinks_.begin(); iter!=graphLinks_.end(); ++iter)
+			{
+				if(uContains(poses, iter->second.from()) && uContains(poses, iter->second.to()))
+				{
+					std::multimap<int, Link>::iterator inserted = links.insert(*iter);
+					int from = iter->second.from();
+					int to = iter->second.to();
+					inserted->second.setTransform(localTransforms.at(from).inverse()*iter->second.transform()*localTransforms.at(to));
+				}
+			}
+		}
+
 		std::map<int, double> stamps;
 		if(format == 1)
 		{
-			for(std::map<int, Transform>::iterator iter=optimizedPoses.begin(); iter!=optimizedPoses.end(); ++iter)
+			for(std::map<int, Transform>::iterator iter=poses.begin(); iter!=poses.end(); ++iter)
 			{
 				Transform p, g;
 				int w;
@@ -1648,10 +1753,10 @@ void DatabaseViewer::exportPoses(int format)
 					stamps.insert(std::make_pair(iter->first, stamp));
 				}
 			}
-			if(stamps.size()!=optimizedPoses.size())
+			if(stamps.size()!=poses.size())
 			{
 				QMessageBox::warning(this, tr("Export poses..."), tr("Poses (%1) and stamps (%2) have not the same size! Cannot export in RGB-D SLAM format.")
-						.arg(optimizedPoses.size()).arg(stamps.size()));
+						.arg(poses.size()).arg(stamps.size()));
 				return;
 			}
 		}
@@ -1666,7 +1771,7 @@ void DatabaseViewer::exportPoses(int format)
 
 		if(!path.isEmpty())
 		{
-			bool saved = graph::exportPoses(path.toStdString(), format, optimizedPoses, graphLinks_, stamps);
+			bool saved = graph::exportPoses(path.toStdString(), format, poses, links, stamps);
 
 			if(saved)
 			{

guilib/src/MainWindow.cpp

@@ -4740,13 +4740,14 @@ void MainWindow::exportPoses(int format)
 {
 	if(_currentPosesMap.size())
 	{
-		std::map<int, Transform> poses;
+		std::map<int, Transform> localTransforms;
 		QStringList items;
 		items.push_back("Robot");
 		items.push_back("Camera");
 		items.push_back("Scan");
-		QString item = QInputDialog::getItem(this, tr("Export Poses"), tr("Frame: "), items, _exportPosesFrame, false);
+		bool ok;
-		if(item.isEmpty())
+		QString item = QInputDialog::getItem(this, tr("Export Poses"), tr("Frame: "), items, _exportPosesFrame, false, &ok);
+		if(!ok || item.isEmpty())
 		{
 			return;
 		}
@@ -4792,7 +4793,7 @@ void MainWindow::exportPoses(int format)
 					}
 					if(!localTransform.isNull())
 					{
-						poses.insert(std::make_pair(iter->first, iter->second * localTransform));
+						localTransforms.insert(std::make_pair(iter->first, localTransform));
 					}
 				}
 				else
@@ -4800,18 +4801,42 @@ void MainWindow::exportPoses(int format)
 					UWARN("Did not find node %d in cache", iter->first);
 				}
 			}
-			if(poses.empty())
+			if(localTransforms.empty())
 			{
 				QMessageBox::warning(this,
 						tr("Export Poses"),
 						tr("Could not find any \"%1\" frame, exporting in Robot frame instead.").arg(item));
-				poses = _currentPosesMap;
 			}
 		}
 		else
 		{
 			_exportPosesFrame = 0;
+		}
+
+		std::map<int, Transform> poses;
+		std::multimap<int, Link> links;
+		if(localTransforms.empty())
+		{
 			poses = _currentPosesMap;
+			links = _currentLinksMap;
+		}
+		else
+		{
+			//adjust poses and links
+			for(std::map<int, Transform>::iterator iter=localTransforms.begin(); iter!=localTransforms.end(); ++iter)
+			{
+				poses.insert(std::make_pair(iter->first, _currentPosesMap.at(iter->first) * iter->second));
+			}
+			for(std::multimap<int, Link>::iterator iter=_currentLinksMap.begin(); iter!=_currentLinksMap.end(); ++iter)
+			{
+				if(uContains(poses, iter->second.from()) && uContains(poses, iter->second.to()))
+				{
+					std::multimap<int, Link>::iterator inserted = links.insert(*iter);
+					int from = iter->second.from();
+					int to = iter->second.to();
+					inserted->second.setTransform(localTransforms.at(from).inverse()*iter->second.transform()*localTransforms.at(to));
+				}
+			}
 		}
 
 		std::map<int, double> stamps;
@@ -4846,24 +4871,6 @@ void MainWindow::exportPoses(int format)
 		if(!path.isEmpty())
 		{
 			_exportPosesFileName[format] = path;
-
-			std::multimap<int, Link> links;
-			if(poses.size() != _currentPosesMap.size())
-			{
-				for(std::multimap<int, Link>::iterator iter=_currentLinksMap.begin(); iter!=_currentLinksMap.end(); ++iter)
-				{
-					if(uContains(poses, iter->second.from()) && uContains(poses, iter->second.to()))
-					{
-						links.insert(*iter);
-					}
-				}
-			}
-			else
-			{
-				links = _currentLinksMap;
-			}
-
-
 			bool saved = graph::exportPoses(path.toStdString(), format, poses, links, stamps);
 
 			if(saved)