i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-09-02 01:20:25 +08:00
Code Issues Packages Projects Releases Wiki Activity

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

Browse Source
This commit is contained in:
matlabbe
2017-05-10 16:47:29 -04:00
parent 104c1e6945
commit 7f8ca64f64
6 changed files with 231 additions and 28 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
corelib/include/rtabmap/core/DBDriver.h
View File

@@ -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;

24
corelib/src/DBDriver.cpp
View File

@@ -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,

64
corelib/src/DBDriverSqlite3.cpp
View File

@@ -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,

1
corelib/src/DBDriverSqlite3.h
View File

@@ -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;

113
guilib/src/DatabaseViewer.cpp
View File

@@ -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)
{

55
guilib/src/MainWindow.cpp
View File

@@ -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);
if(item.isEmpty())
bool ok;
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)