Extract images: added timestamp.jpg/.png filname options (to match RGBD-SLAM pose format). Fixed wrong calibration file when first node is an intermdediate node. Export poses: ignore intermediate nodes when output frame is camera or scan.

corelib/src/CameraModel.cpp

@@ -37,8 +37,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 namespace rtabmap {
 
-CameraModel::CameraModel() :
+CameraModel::CameraModel()
-	localTransform_(0,0,1,0, -1,0,0,0, 0,-1,0,0)
 {
 
 }

corelib/src/Graph.cpp

@@ -125,6 +125,12 @@ bool exportPoses(
 					// Format: stamp x y z qx qy qz qw
 					Eigen::Quaternionf q = pose.getQuaternionf();
 
+					if(iter == poses.begin())
+					{
+						// header
+						fprintf(fout, "# timestamp x y z qx qy qz qw\n");
+					}
+
 					UASSERT(uContains(stamps, iter->first));
 					fprintf(fout, "%f %f %f %f %f %f %f %f\n",
 							stamps.at(iter->first),

guilib/src/DatabaseViewer.cpp

@@ -1428,24 +1428,36 @@ void DatabaseViewer::extractImages()
 	}
 
 	QStringList formats;
-	formats.push_back("jpg");
+	formats.push_back("id.jpg");
-	formats.push_back("png");
+	formats.push_back("id.png");
+	formats.push_back("timestamp.jpg");
+	formats.push_back("timestamp.png");
 	bool ok;
-	QString ext = QInputDialog::getItem(this, tr("Which RGB format?"), tr("Format:"), formats, 0, false, &ok);
+	QString format = QInputDialog::getItem(this, tr("Which RGB format?"), tr("Format:"), formats, 0, false, &ok);
 	if(!ok)
 	{
 		return;
 	}
 
+	QString ext = format.split('.').back();
+	bool useStamp = format.split('.').front().compare("timestamp") == 0;
+
 	QString path = QFileDialog::getExistingDirectory(this, tr("Select directory where to save images..."), QDir::homePath());
 	if(!path.isEmpty())
 	{
-		if(ids_.size())
+		int imagesExported = 0;
+		bool calibrationSaved = false;
+		for(int i=0; i<ids_.size(); ++i)
 		{
-			int id = ids_.at(0);
+			QString id = QString::number(ids_.at(i));
+
 			SensorData data;
-			dbDriver_->getNodeData(id, data);
+			dbDriver_->getNodeData(ids_.at(i), data);
 			data.uncompressData();
+
+			if(!calibrationSaved)
+			{
+				//stereo
 				if(!data.imageRaw().empty() && !data.rightRaw().empty())
 				{
 					QDir dir;
@@ -1477,6 +1489,7 @@ void DatabaseViewer::extractImages()
 								data.stereoCameraModel().left().localTransform());
 						if(model.save(path.toStdString()))
 						{
+							calibrationSaved = true;
 							UINFO("Saved stereo calibration \"%s\"", (path.toStdString()+"/"+cameraName).c_str());
 						}
 						else
@@ -1514,6 +1527,7 @@ void DatabaseViewer::extractImages()
 								data.cameraModels().front().localTransform());
 						if(model.save(path.toStdString()))
 						{
+							calibrationSaved = true;
 							UINFO("Saved calibration \"%s\"", (path.toStdString()+"/"+cameraName).c_str());
 						}
 						else
@@ -1524,34 +1538,59 @@ void DatabaseViewer::extractImages()
 				}
 			}
 
-		int imagesExported = 0;
+			if(!data.imageRaw().empty() && useStamp)
-		for(int i=0; i<ids_.size(); ++i)
 			{
-			int id = ids_.at(i);
+				Transform p,gt;
-			SensorData data;
+				int m,w;
-			dbDriver_->getNodeData(id, data);
+				std::string l;
-			data.uncompressData();
+				double stamp;
+				std::vector<float> v;
+				GPS gps;
+				EnvSensors s;
+				dbDriver_->getNodeInfo(ids_.at(i), p, m, w, l, stamp, gt, v, gps, s);
+				if(stamp == 0.0)
+				{
+					UWARN("Node %d has null timestamp! Using id instead!", ids_.at(i));
+				}
+				else
+				{
+					id = QString::number(stamp, 'f');
+				}
+			}
+
 			if(!data.imageRaw().empty() && !data.rightRaw().empty())
 			{
-				cv::imwrite(QString("%1/left/%2.%3").arg(path).arg(id).arg(ext).toStdString(), data.imageRaw());
+				if(!cv::imwrite(QString("%1/left/%2.%3").arg(path).arg(id).arg(ext).toStdString(), data.imageRaw()))
-				cv::imwrite(QString("%1/right/%2.%3").arg(path).arg(id).arg(ext).toStdString(), data.rightRaw());
+					UWARN("Failed saving \"%s\"", QString("%1/left/%2.%3").arg(path).arg(id).arg(ext).toStdString().c_str());
+				if(!cv::imwrite(QString("%1/right/%2.%3").arg(path).arg(id).arg(ext).toStdString(), data.rightRaw()))
+					UWARN("Failed saving \"%s\"", QString("%1/right/%2.%3").arg(path).arg(id).arg(ext).toStdString().c_str());
 				UINFO(QString("Saved left/%1.%2 and right/%1.%2").arg(id).arg(ext).toStdString().c_str());
 				++imagesExported;
 			}
 			else if(!data.imageRaw().empty() && !data.depthRaw().empty())
 			{
-				cv::imwrite(QString("%1/rgb/%2.%3").arg(path).arg(id).arg(ext).toStdString(), data.imageRaw());
+				if(!cv::imwrite(QString("%1/rgb/%2.%3").arg(path).arg(id).arg(ext).toStdString(), data.imageRaw()))
-				cv::imwrite(QString("%1/depth/%2.png").arg(path).arg(id).toStdString(), data.depthRaw().type()==CV_32FC1?util2d::cvtDepthFromFloat(data.depthRaw()):data.depthRaw());
+					UWARN("Failed saving \"%s\"", QString("%1/rgb/%2.%3").arg(path).arg(id).arg(ext).toStdString().c_str());
+				if(!cv::imwrite(QString("%1/depth/%2.png").arg(path).arg(id).toStdString(), data.depthRaw().type()==CV_32FC1?util2d::cvtDepthFromFloat(data.depthRaw()):data.depthRaw()))
+					UWARN("Failed saving \"%s\"", QString("%1/depth/%2.png").arg(path).arg(id).toStdString().c_str());
 				UINFO(QString("Saved rgb/%1.%2 and depth/%1.png").arg(id).arg(ext).toStdString().c_str());
 				++imagesExported;
 			}
 			else if(!data.imageRaw().empty())
 			{
-				cv::imwrite(QString("%1/%2.%3").arg(path).arg(id).arg(ext).toStdString(), data.imageRaw());
+				if(!cv::imwrite(QString("%1/%2.%3").arg(path).arg(id).arg(ext).toStdString(), data.imageRaw()))
+					UWARN("Failed saving \"%s\"", QString("%1/%2.%3").arg(path).arg(id).arg(ext).toStdString().c_str());
+				else
 					UINFO(QString("Saved %1.%2").arg(id).arg(ext).toStdString().c_str());
 				++imagesExported;
 			}
 		}
+
+		if(!calibrationSaved)
+		{
+			UWARN("no calibration exported...");
+		}
+
 		QMessageBox::information(this, tr("Exporting"), tr("%1 images exported!").arg(imagesExported));
 	}
 }
@@ -4660,6 +4699,11 @@ void DatabaseViewer::update(int value,
 						else if(laserScanRaw.hasRGB())
 						{
 							pcl::PointCloud<pcl::PointXYZRGB>::Ptr scan = util3d::laserScanToPointCloudRGB(laserScanRaw, laserScanRaw.localTransform());
+							cloudViewer_->addCloud("scan", scan, pose, Qt::yellow);
+						}
+						else if(laserScanRaw.hasIntensity())
+						{
+							pcl::PointCloud<pcl::PointXYZI>::Ptr scan = util3d::laserScanToPointCloudI(laserScanRaw, laserScanRaw.localTransform());
 							cloudViewer_->addCloud("scan", scan, pose, Qt::yellow);
 						}
 						else

guilib/src/MainWindow.cpp

@@ -7335,15 +7335,20 @@ void MainWindow::exportImages()
 	}
 
 	QStringList formats;
-	formats.push_back("jpg");
+	formats.push_back("id.jpg");
-	formats.push_back("png");
+	formats.push_back("id.png");
+	formats.push_back("timestamp.jpg");
+	formats.push_back("timestamp.png");
 	bool ok;
-	QString ext = QInputDialog::getItem(this, tr("Which RGB format?"), tr("Format:"), formats, 0, false, &ok);
+	QString format = QInputDialog::getItem(this, tr("Which RGB format?"), tr("Format:"), formats, 0, false, &ok);
 	if(!ok)
 	{
 		return;
 	}
+	QString ext = format.split('.').back();
+	bool useStamp = format.split('.').front().compare("timestamp") == 0;
 
+	QMap<int, double> stamps;
 	QString path = QFileDialog::getExistingDirectory(this, tr("Select directory where to save images..."), this->getWorkingDirectory());
 	if(!path.isEmpty())
 	{
@@ -7353,6 +7358,25 @@ void MainWindow::exportImages()
 			data = _cachedSignatures.value(poses.rbegin()->first).sensorData();
 			data.uncompressData();
 		}
+
+		_progressDialog->resetProgress();
+		_progressDialog->show();
+		_progressDialog->setMaximumSteps(_cachedSignatures.size());
+
+		unsigned int saved = 0;
+		bool calibrationSaved = false;
+		for(std::map<int, Transform>::iterator iter=poses.begin(); iter!=poses.end(); ++iter)
+		{
+			QString id = QString::number(iter->first);
+
+			SensorData data;
+			if(_cachedSignatures.contains(iter->first))
+			{
+				data = _cachedSignatures.value(iter->first).sensorData();
+				data.uncompressData();
+
+				if(!calibrationSaved)
+				{
 					if(!data.imageRaw().empty() && !data.rightRaw().empty())
 					{
 						QDir dir;
@@ -7380,6 +7404,7 @@ void MainWindow::exportImages()
 									data.stereoCameraModel().left().localTransform());
 							if(model.save(path.toStdString()))
 							{
+								calibrationSaved = true;
 								UINFO("Saved stereo calibration \"%s\"", (path.toStdString()+"/"+cameraName).c_str());
 							}
 							else
@@ -7413,6 +7438,7 @@ void MainWindow::exportImages()
 									data.cameraModels().front().localTransform());
 							if(model.save(path.toStdString()))
 							{
+								calibrationSaved = true;
 								UINFO("Saved calibration \"%s\"", (path.toStdString()+"/"+cameraName).c_str());
 							}
 							else
@@ -7421,44 +7447,44 @@ void MainWindow::exportImages()
 							}
 						}
 					}
-		else
-		{
-			QMessageBox::warning(this,
-					tr("Export images..."),
-					tr("Data in the cache don't seem to have images (tested node %1). Calibration file will not be saved. Try refreshing the cache (with clouds).").arg(poses.rbegin()->first));
 				}
 
-		_progressDialog->resetProgress();
+				if(!data.imageRaw().empty() && useStamp)
-		_progressDialog->show();
-		_progressDialog->setMaximumSteps(_cachedSignatures.size());
-
-		unsigned int saved = 0;
-		for(std::map<int, Transform>::iterator iter=poses.begin(); iter!=poses.end(); ++iter)
 				{
-			int id = iter->first;
+					double stamp = _cachedSignatures.value(iter->first).getStamp();
-			SensorData data;
+					if(stamp == 0.0)
-			if(_cachedSignatures.contains(iter->first))
 					{
-				data = _cachedSignatures.value(iter->first).sensorData();
+						UWARN("Node %d has null timestamp! Using id instead!", iter->first);
-				data.uncompressData();
+					}
+					else
+					{
+						id = QString::number(stamp, 'f');
+					}
+				}
 			}
 			QString info;
 			bool warn = false;
 			if(!data.imageRaw().empty() && !data.rightRaw().empty())
 			{
-				cv::imwrite(QString("%1/left/%2.%3").arg(path).arg(id).arg(ext).toStdString(), data.imageRaw());
+				if(!cv::imwrite(QString("%1/left/%2.%3").arg(path).arg(id).arg(ext).toStdString(), data.imageRaw()))
-				cv::imwrite(QString("%1/right/%2.%3").arg(path).arg(id).arg(ext).toStdString(), data.rightRaw());
+					UWARN("Failed saving \"%s\"", QString("%1/left/%2.%3").arg(path).arg(id).arg(ext).toStdString().c_str());
+				if(!cv::imwrite(QString("%1/right/%2.%3").arg(path).arg(id).arg(ext).toStdString(), data.rightRaw()))
+					UWARN("Failed saving \"%s\"", QString("%1/right/%2.%3").arg(path).arg(id).arg(ext).toStdString().c_str());
 				info = tr("Saved left/%1.%2 and right/%1.%2.").arg(id).arg(ext);
 			}
 			else if(!data.imageRaw().empty() && !data.depthRaw().empty())
 			{
-				cv::imwrite(QString("%1/rgb/%2.%3").arg(path).arg(id).arg(ext).toStdString(), data.imageRaw());
+				if(!cv::imwrite(QString("%1/rgb/%2.%3").arg(path).arg(id).arg(ext).toStdString(), data.imageRaw()))
-				cv::imwrite(QString("%1/depth/%2.png").arg(path).arg(id).toStdString(), data.depthRaw().type()==CV_32FC1?util2d::cvtDepthFromFloat(data.depthRaw()):data.depthRaw());
+					UWARN("Failed saving \"%s\"", QString("%1/rgb/%2.%3").arg(path).arg(id).arg(ext).toStdString().c_str());
+				if(!cv::imwrite(QString("%1/depth/%2.png").arg(path).arg(id).toStdString(), data.depthRaw().type()==CV_32FC1?util2d::cvtDepthFromFloat(data.depthRaw()):data.depthRaw()))
+					UWARN("Failed saving \"%s\"", QString("%1/depth/%2.png").arg(path).arg(id).toStdString().c_str());
 				info = tr("Saved rgb/%1.%2 and depth/%1.png.").arg(id).arg(ext);
 			}
 			else if(!data.imageRaw().empty())
 			{
-				cv::imwrite(QString("%1/%2.%3").arg(path).arg(id).arg(ext).toStdString(), data.imageRaw());
+				if(!cv::imwrite(QString("%1/%2.%3").arg(path).arg(id).arg(ext).toStdString(), data.imageRaw()))
+					UWARN("Failed saving \"%s\"", QString("%1/%2.%3").arg(path).arg(id).arg(ext).toStdString().c_str());
+				else
 					info = tr("Saved %1.%2.").arg(id).arg(ext);
 			}
 			else
@@ -7482,6 +7508,13 @@ void MainWindow::exportImages()
 			_progressDialog->appendText(tr("%1 images saved to \"%2\".").arg(saved).arg(path));
 		}
 
+		if(!calibrationSaved)
+		{
+			QMessageBox::warning(this,
+					tr("Export images..."),
+					tr("Data in the cache don't seem to have valid calibration. Calibration file will not be saved. Try refreshing the cache (with clouds)."));
+		}
+
 		_progressDialog->setValue(_progressDialog->maximumSteps());
 	}
 }