Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Include gps metadata to extracted image #920

Open
wants to merge 3 commits into
base: master
Choose a base branch
from
Open

Include gps metadata to extracted image #920

Show file tree
Hide file tree
Changes from 1 commit
Commits
Show all changes
3 commits
Select commit Hold shift click to select a range
5643010
add gps info to exported images
Nov 1, 2022
f6d4684
export image with posed based gps.
Nov 1, 2022
2060e57
act better without gps data
Nov 1, 2022
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
Prev Previous commit
Next Next commit
export image with posed based gps. 
Previously it used raw gps info.
  • Loading branch information
Charles Ellison authored and steven.l.bunkley committed Nov 3, 2022
commit f6d468461e8bce308c62e2f35620c14cfcd12c20
2 changes: 2 additions & 0 deletions guilib/include/rtabmap/gui/DatabaseViewer.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -106,6 106,7 @@ private Q_SLOTS:
void updateOptimizedMesh();
void exportDatabase();
void extractImages();
std::string selectGraph();
void exportPosesRaw();
void exportPosesRGBDSLAMMotionCapture();
void exportPosesRGBDSLAM();
Expand Down Expand Up @@ -199,6 200,7 @@ private Q_SLOTS:
void exportPoses(int format);
void exportGPS(int format);

std::map<int, GPS> graphToGPS(std::string graphSource);
void writeExiv2Data(Exiv2::ExifData &exifData, std::string keyStr, std::string str);

std::string toExifTimeStamp(std::string& t)
Expand Down
198 changes: 115 additions & 83 deletions guilib/src/DatabaseViewer.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -1478,6 1478,10 @@ void DatabaseViewer::extractImages()
progressDialog->appendText(tr("Saving %1 images to %2...").arg(ids_.size()).arg(path));
progressDialog->show();

// Determine source of gps info for images
std::string graphSource = DatabaseViewer::selectGraph();
std::map<int, GPS> gpsValues = graphToGPS(graphSource);

int imagesExported = 0;
for(int i=0; i<ids_.size(); i)
{
Expand Down Expand Up @@ -1531,6 1535,8 @@ void DatabaseViewer::extractImages()
id = QString::number(stamp, 'f');
}

gps = gpsValues[ids_.at(i)];

//fill out image metadata
std::time_t time = std::chrono::system_clock::to_time_t(std::chrono::system_clock::time_point(std::chrono::duration_cast<std::chrono::seconds>(std::chrono::duration<double>(gps.stamp()))));
std::tm timestamp = *std::localtime(&time);
Expand Down Expand Up @@ -2431,125 2437,151 @@ void DatabaseViewer::exportPosesKML()
exportPoses(5);
}

void DatabaseViewer::exportPoses(int format)
std::string DatabaseViewer::selectGraph()
{
QStringList types;
QStringList types;
types.push_back("Map's graph (see Graph View)");
types.push_back("Odometry");
if(!groundTruthPoses_.empty())
{
types.push_back("Ground Truth");
}
if(!gpsPoses_.empty())
{
types.push_back("GPS");
}
bool ok;
QString type = QInputDialog::getItem(this, tr("Which poses?"), tr("Poses:"), types, 0, false, &ok);
if(!ok)
{
return;
return "Cancel";
}
bool odometry = type.compare("Odometry") == 0;
bool groundTruth = type.compare("Ground Truth") == 0;

if(groundTruth && groundTruthPoses_.empty())

if(type == "Ground Truth" && groundTruthPoses_.empty())
{
QMessageBox::warning(this, tr("Cannot export poses"), tr("No ground truth poses in database?!"));
return;
return "None";
}
else if(!odometry && graphes_.empty())
else if(type == "Odometry" && odomPoses_.empty())
{
QMessageBox::warning(this, tr("Cannot export poses"), tr("No odometry poses in database?!"));
return "None";
}
else if(type == "GPS" && gpsPoses_.empty())
{
QMessageBox::warning(this, tr("Cannot export poses"), tr("No GPS in database?!"));
return "None";
}
else if(graphes_.empty())
{
this->updateGraphView();
if(graphes_.empty() || ui_->horizontalSlider_iterations->maximum() != (int)graphes_.size()-1)
{
QMessageBox::warning(this, tr("Cannot export poses"), tr("No graph in database?!"));
return;
return "None";
}
}
else if(odometry && odomPoses_.empty())

return type.toStdString();
}

std::map<int, GPS> DatabaseViewer::graphToGPS(std::string graphSource)
{
std::map<int, rtabmap::Transform> graph;
if(graphSource == "Ground Truth")
{
QMessageBox::warning(this, tr("Cannot export poses"), tr("No odometry poses in database?!"));
return;
graph = groundTruthPoses_;
}
else if(graphSource == "Odometry")
{
graph = odomPoses_;
}
else if(graphSource == "GPS")
{
graph = gpsPoses_;
}
else
{
graph = uValueAt(graphes_, ui_->horizontalSlider_iterations->value());
}

if(format == 5)
//align with ground truth for more meaningful results
pcl::PointCloud<pcl::PointXYZ> cloud1, cloud2;
cloud1.resize(graph.size());
cloud2.resize(graph.size());
int oi = 0;
int idFirst = 0;
for(std::map<int, Transform>::const_iterator iter=gpsPoses_.begin(); iter!=gpsPoses_.end(); iter)
{
if(gpsValues_.empty() || gpsPoses_.empty())
{
QMessageBox::warning(this, tr("Cannot export poses"), tr("No GPS in database?!"));
}
else
std::map<int, Transform>::iterator iter2 = graph.find(iter->first);
if(iter2!=graph.end())
{
std::map<int, rtabmap::Transform> graph;
if(groundTruth)
if(oi==0)
{
graph = groundTruthPoses_;
}
else if(odometry)
{
graph = odomPoses_;
}
else
{
graph = uValueAt(graphes_, ui_->horizontalSlider_iterations->value());
idFirst = iter->first;
}
cloud1[oi] = pcl::PointXYZ(iter->second.x(), iter->second.y(), iter->second.z());
cloud2[oi ] = pcl::PointXYZ(iter2->second.x(), iter2->second.y(), iter2->second.z());
}
}

Transform t = Transform::getIdentity();
if(oi>5)
{
cloud1.resize(oi);
cloud2.resize(oi);

//align with ground truth for more meaningful results
pcl::PointCloud<pcl::PointXYZ> cloud1, cloud2;
cloud1.resize(graph.size());
cloud2.resize(graph.size());
int oi = 0;
int idFirst = 0;
for(std::map<int, Transform>::const_iterator iter=gpsPoses_.begin(); iter!=gpsPoses_.end(); iter)
{
std::map<int, Transform>::iterator iter2 = graph.find(iter->first);
if(iter2!=graph.end())
{
if(oi==0)
{
idFirst = iter->first;
}
cloud1[oi] = pcl::PointXYZ(iter->second.x(), iter->second.y(), iter->second.z());
cloud2[oi ] = pcl::PointXYZ(iter2->second.x(), iter2->second.y(), iter2->second.z());
}
}
t = util3d::transformFromXYZCorrespondencesSVD(cloud2, cloud1);
}
else if(idFirst)
{
t = gpsPoses_.at(idFirst) * graph.at(idFirst).inverse();
}

Transform t = Transform::getIdentity();
if(oi>5)
{
cloud1.resize(oi);
cloud2.resize(oi);
std::map<int, GPS> values;
GeodeticCoords origin = gpsValues_.begin()->second.toGeodeticCoords();
for(std::map<int, Transform>::iterator iter=graph.begin(); iter!=graph.end(); iter)
{
iter->second = t * iter->second;

t = util3d::transformFromXYZCorrespondencesSVD(cloud2, cloud1);
}
else if(idFirst)
{
t = gpsPoses_.at(idFirst) * graph.at(idFirst).inverse();
}
GeodeticCoords coord;
coord.fromENU_WGS84(cv::Point3d(iter->second.x(), iter->second.y(), iter->second.z()), origin);
double bearing = -(iter->second.theta()*180.0/M_PI-90.0);
if(bearing < 0)
{
bearing = 360;
}

std::map<int, GPS> values;
GeodeticCoords origin = gpsValues_.begin()->second.toGeodeticCoords();
for(std::map<int, Transform>::iterator iter=graph.begin(); iter!=graph.end(); iter)
{
iter->second = t * iter->second;
Transform p, g;
int w;
std::string l;
double stamp=0.0;
int mapId;
std::vector<float> v;
GPS gps;
EnvSensors sensors;
dbDriver_->getNodeInfo(iter->first, p, mapId, w, l, stamp, g, v, gps, sensors);
values.insert(std::make_pair(iter->first, GPS(stamp, coord.longitude(), coord.latitude(), coord.altitude(), 0, 0)));
}

GeodeticCoords coord;
coord.fromENU_WGS84(cv::Point3d(iter->second.x(), iter->second.y(), iter->second.z()), origin);
double bearing = -(iter->second.theta()*180.0/M_PI-90.0);
if(bearing < 0)
{
bearing = 360;
}
return values;
}

Transform p, g;
int w;
std::string l;
double stamp=0.0;
int mapId;
std::vector<float> v;
GPS gps;
EnvSensors sensors;
dbDriver_->getNodeInfo(iter->first, p, mapId, w, l, stamp, g, v, gps, sensors);
values.insert(std::make_pair(iter->first, GPS(stamp, coord.longitude(), coord.latitude(), coord.altitude(), 0, 0)));
}
void DatabaseViewer::exportPoses(int format)
{
std::string graphSource = DatabaseViewer::selectGraph();
bool groundTruth = (graphSource == "Ground Truth");
bool odometry = (graphSource == "Odometry");

if(format == 5)
{
if(gpsValues_.empty() || gpsPoses_.empty())
{
QMessageBox::warning(this, tr("Cannot export poses"), tr("No GPS in database?!"));
}
else
{
std::map<int, GPS> values = graphToGPS(graphSource);

QString output = pathDatabase_ QDir::separator() "poses.kml";
QString path = QFileDialog::getSaveFileName(
Expand Down