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,130 +1428,169 @@ 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 id = ids_.at(0);
-			SensorData data;
-			dbDriver_->getNodeData(id, data);
-			data.uncompressData();
-			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 if(data.stereoCameraModel().isValidForProjection())
-				{
-					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()))
-					{
-						UINFO("Saved stereo calibration \"%s\"", (path.toStdString()+"/"+cameraName).c_str());
-					}
-					else
-					{
-						UERROR("Failed saving calibration \"%s\"", (path.toStdString()+"/"+cameraName).c_str());
-					}
-				}
-			}
-			else if(!data.imageRaw().empty())
-			{
-				if(!data.depthRaw().empty())
-				{
-					QDir dir;
-					dir.mkdir(QString("%1/rgb").arg(path));
-					dir.mkdir(QString("%1/depth").arg(path));
-				}
-
-				if(databaseFileName_.empty())
-				{
-					UERROR("Cannot save calibration file, database name is empty!");
-				}
-				else if(data.cameraModels().size() > 1)
-				{
-					UERROR("Only one camera calibration can be saved at this time (%d detected)", (int)data.cameraModels().size());
-				}
-				else if(data.cameraModels().size() == 1 && data.cameraModels().front().isValidForProjection())
-				{
-					std::string cameraName = uSplit(databaseFileName_, '.').front();
-					CameraModel model(cameraName,
-							data.imageRaw().size(),
-							data.cameraModels().front().K(),
-							data.cameraModels().front().D(),
-							data.cameraModels().front().R(),
-							data.cameraModels().front().P(),
-							data.cameraModels().front().localTransform());
-					if(model.save(path.toStdString()))
-					{
-						UINFO("Saved calibration \"%s\"", (path.toStdString()+"/"+cameraName).c_str());
-					}
-					else
-					{
-						UERROR("Failed saving calibration \"%s\"", (path.toStdString()+"/"+cameraName).c_str());
-					}
-				}
-			}
-		}
-
 		int imagesExported = 0;
+		bool calibrationSaved = false;
 		for(int i=0; i<ids_.size(); ++i)
 		{
-			int id = ids_.at(i);
+			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;
+					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 if(data.stereoCameraModel().isValidForProjection())
+					{
+						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()))
+						{
+							calibrationSaved = true;
+							UINFO("Saved stereo calibration \"%s\"", (path.toStdString()+"/"+cameraName).c_str());
+						}
+						else
+						{
+							UERROR("Failed saving calibration \"%s\"", (path.toStdString()+"/"+cameraName).c_str());
+						}
+					}
+				}
+				else if(!data.imageRaw().empty())
+				{
+					if(!data.depthRaw().empty())
+					{
+						QDir dir;
+						dir.mkdir(QString("%1/rgb").arg(path));
+						dir.mkdir(QString("%1/depth").arg(path));
+					}
+
+					if(databaseFileName_.empty())
+					{
+						UERROR("Cannot save calibration file, database name is empty!");
+					}
+					else if(data.cameraModels().size() > 1)
+					{
+						UERROR("Only one camera calibration can be saved at this time (%d detected)", (int)data.cameraModels().size());
+					}
+					else if(data.cameraModels().size() == 1 && data.cameraModels().front().isValidForProjection())
+					{
+						std::string cameraName = uSplit(databaseFileName_, '.').front();
+						CameraModel model(cameraName,
+								data.imageRaw().size(),
+								data.cameraModels().front().K(),
+								data.cameraModels().front().D(),
+								data.cameraModels().front().R(),
+								data.cameraModels().front().P(),
+								data.cameraModels().front().localTransform());
+						if(model.save(path.toStdString()))
+						{
+							calibrationSaved = true;
+							UINFO("Saved calibration \"%s\"", (path.toStdString()+"/"+cameraName).c_str());
+						}
+						else
+						{
+							UERROR("Failed saving calibration \"%s\"", (path.toStdString()+"/"+cameraName).c_str());
+						}
+					}
+				}
+			}
+
+			if(!data.imageRaw().empty() && useStamp)
+			{
+				Transform p,gt;
+				int m,w;
+				std::string l;
+				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()))
-				UINFO(QString("Saved %1.%2").arg(id).arg(ext).toStdString().c_str());
+					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,113 +7358,134 @@ void MainWindow::exportImages()
 			data = _cachedSignatures.value(poses.rbegin()->first).sensorData();
 			data.uncompressData();
 		}
-		if(!data.imageRaw().empty() && !data.rightRaw().empty())
-		{
-			QDir dir;
-			dir.mkdir(QString("%1/left").arg(path));
-			dir.mkdir(QString("%1/right").arg(path));
-			if(data.stereoCameraModel().isValidForProjection())
-			{
-				std::string cameraName = "calibration";
-				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()))
-				{
-					UINFO("Saved stereo calibration \"%s\"", (path.toStdString()+"/"+cameraName).c_str());
-				}
-				else
-				{
-					UERROR("Failed saving calibration \"%s\"", (path.toStdString()+"/"+cameraName).c_str());
-				}
-			}
-		}
-		else if(!data.imageRaw().empty())
-		{
-			if(!data.depthRaw().empty())
-			{
-				QDir dir;
-				dir.mkdir(QString("%1/rgb").arg(path));
-				dir.mkdir(QString("%1/depth").arg(path));
-			}
-
-			if(data.cameraModels().size() > 1)
-			{
-				UERROR("Only one camera calibration can be saved at this time (%d detected)", (int)data.cameraModels().size());
-			}
-			else if(data.cameraModels().size() == 1 && data.cameraModels().front().isValidForProjection())
-			{
-				std::string cameraName = "calibration";
-				CameraModel model(cameraName,
-						data.imageRaw().size(),
-						data.cameraModels().front().K(),
-						data.cameraModels().front().D(),
-						data.cameraModels().front().R(),
-						data.cameraModels().front().P(),
-						data.cameraModels().front().localTransform());
-				if(model.save(path.toStdString()))
-				{
-					UINFO("Saved calibration \"%s\"", (path.toStdString()+"/"+cameraName).c_str());
-				}
-				else
-				{
-					UERROR("Failed saving calibration \"%s\"", (path.toStdString()+"/"+cameraName).c_str());
-				}
-			}
-		}
-		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();
 		_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)
 		{
-			int id = iter->first;
+			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;
+						dir.mkdir(QString("%1/left").arg(path));
+						dir.mkdir(QString("%1/right").arg(path));
+						if(data.stereoCameraModel().isValidForProjection())
+						{
+							std::string cameraName = "calibration";
+							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()))
+							{
+								calibrationSaved = true;
+								UINFO("Saved stereo calibration \"%s\"", (path.toStdString()+"/"+cameraName).c_str());
+							}
+							else
+							{
+								UERROR("Failed saving calibration \"%s\"", (path.toStdString()+"/"+cameraName).c_str());
+							}
+						}
+					}
+					else if(!data.imageRaw().empty())
+					{
+						if(!data.depthRaw().empty())
+						{
+							QDir dir;
+							dir.mkdir(QString("%1/rgb").arg(path));
+							dir.mkdir(QString("%1/depth").arg(path));
+						}
+
+						if(data.cameraModels().size() > 1)
+						{
+							UERROR("Only one camera calibration can be saved at this time (%d detected)", (int)data.cameraModels().size());
+						}
+						else if(data.cameraModels().size() == 1 && data.cameraModels().front().isValidForProjection())
+						{
+							std::string cameraName = "calibration";
+							CameraModel model(cameraName,
+									data.imageRaw().size(),
+									data.cameraModels().front().K(),
+									data.cameraModels().front().D(),
+									data.cameraModels().front().R(),
+									data.cameraModels().front().P(),
+									data.cameraModels().front().localTransform());
+							if(model.save(path.toStdString()))
+							{
+								calibrationSaved = true;
+								UINFO("Saved calibration \"%s\"", (path.toStdString()+"/"+cameraName).c_str());
+							}
+							else
+							{
+								UERROR("Failed saving calibration \"%s\"", (path.toStdString()+"/"+cameraName).c_str());
+							}
+						}
+					}
+				}
+
+				if(!data.imageRaw().empty() && useStamp)
+				{
+					double stamp = _cachedSignatures.value(iter->first).getStamp();
+					if(stamp == 0.0)
+					{
+						UWARN("Node %d has null timestamp! Using id instead!", iter->first);
+					}
+					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()))
-				info = tr("Saved %1.%2.").arg(id).arg(ext);
+					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());
 	}
 }