DBViewer: added stereo images extraction

corelib/include/rtabmap/core/CameraModel.h

@@ -87,7 +87,7 @@ public:
 	int imageWeight() const {return imageSize_.height;}
 
 	bool load(const std::string & filePath);
-	bool save(const std::string & filePath);
+	bool save(const std::string & filePath) const;
 
 	void scale(double scale);
 
@@ -146,7 +146,7 @@ public:
 	const std::string & name() const {return name_;}
 
 	bool load(const std::string & directory, const std::string & cameraName, bool ignoreStereoTransform = true);
-	bool save(const std::string & directory, const std::string & cameraName, bool ignoreStereoTransform = true);
+	bool save(const std::string & directory, const std::string & cameraName, bool ignoreStereoTransform = true) const;
 
 	double baseline() const {return -right_.Tx()/right_.fx();}
 

corelib/src/CameraModel.cpp

@@ -170,7 +170,7 @@ bool CameraModel::load(const std::string & filePath)
 	return false;
 }
 
-bool CameraModel::save(const std::string & filePath)
+bool CameraModel::save(const std::string & filePath) const
 {
 	if(!filePath.empty() && !name_.empty() && !K_.empty() && !D_.empty() && !R_.empty() && !P_.empty())
 	{
@@ -368,7 +368,7 @@ bool StereoCameraModel::load(const std::string & directory, const std::string &
 	}
 	return false;
 }
-bool StereoCameraModel::save(const std::string & directory, const std::string & cameraName, bool ignoreStereoTransform)
+bool StereoCameraModel::save(const std::string & directory, const std::string & cameraName, bool ignoreStereoTransform) const
 {
 	if(left_.save(directory+"/"+cameraName+"_left.yaml") && right_.save(directory+"/"+cameraName+"_right.yaml"))
 	{

guilib/include/rtabmap/gui/DatabaseViewer.h

@@ -156,6 +156,7 @@ private:
 	QList<rtabmap::Link> loopLinks_;
 	rtabmap::Memory * memory_;
 	QString pathDatabase_;
+	std::string databaseFileName_;
 	std::list<std::map<int, rtabmap::Transform> > graphes_;
 	std::multimap<int, rtabmap::Link> graphLinks_;
 	std::map<int, rtabmap::Transform> poses_;

guilib/src/DatabaseViewer.cpp

@@ -44,6 +44,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <opencv2/core/core_c.h>
 #include <opencv2/highgui/highgui.hpp>
 #include <rtabmap/utilite/UTimer.h>
+#include <rtabmap/utilite/UFile.h>
 #include "rtabmap/core/Memory.h"
 #include "rtabmap/core/DBDriver.h"
 #include "rtabmap/gui/KeypointItem.h"
@@ -557,6 +558,7 @@ bool DatabaseViewer::openDatabase(const QString & path)
 			localMaps_.clear();
 			ui_->actionGenerate_TORO_graph_graph->setEnabled(false);
 			ui_->checkBox_showOptimized->setEnabled(false);
+			databaseFileName_.clear();
 		}
 
 		std::string driverType = "sqlite3";
@@ -576,6 +578,7 @@ bool DatabaseViewer::openDatabase(const QString & path)
 		else
 		{
 			pathDatabase_ = UDirectory::getDir(path.toStdString()).c_str();
+			databaseFileName_ = UFile::getName(path.toStdString());
 			updateIds();
 			return true;
 		}
@@ -880,15 +883,65 @@ void DatabaseViewer::extractImages()
 	QString path = QFileDialog::getExistingDirectory(this, tr("Select directory where to save images..."), QDir::homePath());
 	if(!path.isNull())
 	{
-		for(int i=0; i<ids_.size(); i+=1)
+		if(ids_.size())
+		{
+			int id = ids_.at(0);
+			SensorData data = memory_->getNodeData(id, true);
+			if(!data.imageRaw().empty() && !data.rightRaw().empty())
+			{
+				QDir dir;
+				dir.mkdir(QString("%1/left").arg(path));
+				dir.mkdir(QString("%1/right").arg(path));
+				if(databaseFileName_.empty())
+				{
+					UERROR("Cannot save calibration file, database name is empty!");
+				}
+				else
+				{
+					std::string cameraName = uSplit(databaseFileName_, '.').front();
+					StereoCameraModel model(
+							cameraName,
+							data.imageRaw().size(),
+							data.stereoCameraModel().left().K(),
+							data.stereoCameraModel().left().D(),
+							data.stereoCameraModel().left().R(),
+							data.stereoCameraModel().left().P(),
+							data.rightRaw().size(),
+							data.stereoCameraModel().right().K(),
+							data.stereoCameraModel().right().D(),
+							data.stereoCameraModel().right().R(),
+							data.stereoCameraModel().right().P(),
+							data.stereoCameraModel().R(),
+							data.stereoCameraModel().T(),
+							data.stereoCameraModel().E(),
+							data.stereoCameraModel().F(),
+							data.stereoCameraModel().left().localTransform());
+					if(model.save(path.toStdString(), cameraName))
+					{
+						UINFO("Saved stereo calibration \"%s\"", (path.toStdString()+"/"+cameraName).c_str());
+					}
+					else
+					{
+						UERROR("Failed saving calibration \"%s\"", (path.toStdString()+"/"+cameraName).c_str());
+					}
+				}
+			}
+		}
+
+		for(int i=0; i<ids_.size(); ++i)
 		{
 			int id = ids_.at(i);
-			cv::Mat compressedRgb = memory_->getImageCompressed(id);
+			SensorData data = memory_->getNodeData(id, true);
-			if(!compressedRgb.empty())
+			if(!data.imageRaw().empty() && !data.rightRaw().empty())
 			{
-				cv::Mat imageMat = rtabmap::uncompressImage(compressedRgb);
+				cv::imwrite(QString("%1/left/%2.jpg").arg(path).arg(id).toStdString(), data.imageRaw());
-				cv::imwrite(QString("%1/%2.png").arg(path).arg(id).toStdString(), imageMat);
+				cv::imwrite(QString("%1/right/%2.jpg").arg(path).arg(id).toStdString(), data.rightRaw());
-				UINFO(QString("Saved %1/%2.png").arg(path).arg(id).toStdString().c_str());
+				UINFO(QString("Saved left/%1.jpg and right/%1.jpg").arg(id).toStdString().c_str());
+			}
+			else if(!data.imageRaw().empty())
+			{
+				cv::imwrite(QString("%1/%2.jpg").arg(path).arg(id).toStdString(), data.imageRaw());
+				UINFO(QString("Saved %1.jpg").arg(id).toStdString().c_str());
 			}
 		}
 	}