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

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;