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

0.14: added gps field to Node table in database (#226). Tango: saving gps if enabled, added Rename/Remove/Share on long click in Open dialog (fixed #233)

Browse Source
This commit is contained in:
matlabbe
2017-09-21 20:59:45 -04:00
parent 114490f01e
commit bfc393a090
22 changed files with 516 additions and 103 deletions
Download Patch File Download Diff File Expand all files Collapse all files

6
corelib/src/DBDriver.cpp
View File

@@ -712,7 +712,8 @@ bool DBDriver::getNodeInfo(
std::string & label,
double & stamp,
Transform & groundTruthPose,
std::vector<float> & velocity) const
std::vector<float> & velocity,
std::vector<double> & gps) const
{
bool found = false;
// look in the trash
@@ -725,6 +726,7 @@ bool DBDriver::getNodeInfo(
label = _trashSignatures.at(signatureId)->getLabel();
stamp = _trashSignatures.at(signatureId)->getStamp();
groundTruthPose = _trashSignatures.at(signatureId)->getGroundTruthPose();
gps = _trashSignatures.at(signatureId)->sensorData().gps();
found = true;
}
_trashesMutex.unlock();
@@ -732,7 +734,7 @@ bool DBDriver::getNodeInfo(
if(!found)
{
_dbSafeAccessMutex.lock();
found = this->getNodeInfoQuery(signatureId, pose, mapId, weight, label, stamp, groundTruthPose, velocity);
found = this->getNodeInfoQuery(signatureId, pose, mapId, weight, label, stamp, groundTruthPose, velocity, gps);
_dbSafeAccessMutex.unlock();
}
return found;

73
corelib/src/DBDriverSqlite3.cpp
View File

@@ -497,7 +497,11 @@ long DBDriverSqlite3::getNodesMemoryUsedQuery() const
if(_ppDb)
{
std::string query;
if(uStrNumCmp(_version, "0.13.0") >= 0)
if(uStrNumCmp(_version, "0.14.0") >= 0)
{
query = "SELECT sum(length(id) + length(map_id) + length(weight) + length(pose) + length(stamp) + ifnull(length(label),0) + length(ground_truth_pose) + ifnull(length(velocity),0) + ifnull(length(gps),0) + length(time_enter)) from Node;";
}
else if(uStrNumCmp(_version, "0.13.0") >= 0)
{
query = "SELECT sum(length(id) + length(map_id) + length(weight) + length(pose) + length(stamp) + ifnull(length(label),0) + length(ground_truth_pose) + ifnull(length(velocity),0) + length(time_enter)) from Node;";
}
@@ -1751,7 +1755,8 @@ bool DBDriverSqlite3::getNodeInfoQuery(int signatureId,
std::string & label,
double & stamp,
Transform & groundTruthPose,
std::vector<float> & velocity) const
std::vector<float> & velocity,
std::vector<double> & gps) const
{
bool found = false;
if(_ppDb && signatureId)
@@ -1760,7 +1765,14 @@ bool DBDriverSqlite3::getNodeInfoQuery(int signatureId,
sqlite3_stmt * ppStmt = 0;
std::stringstream query;
if(uStrNumCmp(_version, "0.13.0") >= 0)
if(uStrNumCmp(_version, "0.14.0") >= 0)
{
query << "SELECT pose, map_id, weight, label, stamp, ground_truth_pose, velocity, gps "
"FROM Node "
"WHERE id = " << signatureId <<
";";
}
else if(uStrNumCmp(_version, "0.13.0") >= 0)
{
query << "SELECT pose, map_id, weight, label, stamp, ground_truth_pose, velocity "
"FROM Node "
@@ -1839,6 +1851,17 @@ bool DBDriverSqlite3::getNodeInfoQuery(int signatureId,
memcpy(velocity.data(), data, dataSize);
}
}
if(uStrNumCmp(_version, "0.14.0") >= 0)
{
gps.resize(6,0);
data = sqlite3_column_blob(ppStmt, index); // velocity
dataSize = sqlite3_column_bytes(ppStmt, index++);
if((unsigned int)dataSize == gps.size()*sizeof(double) && data)
{
memcpy(gps.data(), data, dataSize);
}
}
}
}
@@ -2239,7 +2262,13 @@ void DBDriverSqlite3::loadSignaturesQuery(const std::list<int> & ids, std::list<
unsigned int loaded = 0;
// Load nodes information
if(uStrNumCmp(_version, "0.13.0") >= 0)
if(uStrNumCmp(_version, "0.14.0") >= 0)
{
query << "SELECT id, map_id, weight, pose, stamp, label, ground_truth_pose, velocity, gps "
<< "FROM Node "
<< "WHERE id=?;";
}
else if(uStrNumCmp(_version, "0.13.0") >= 0)
{
query << "SELECT id, map_id, weight, pose, stamp, label, ground_truth_pose, velocity "
<< "FROM Node "
@@ -2281,6 +2310,7 @@ void DBDriverSqlite3::loadSignaturesQuery(const std::list<int> & ids, std::list<
Transform pose;
Transform groundTruthPose;
std::vector<float> velocity;
std::vector<double> gps;
const void * data = 0;
int dataSize = 0;
std::string label;
@@ -2329,6 +2359,17 @@ void DBDriverSqlite3::loadSignaturesQuery(const std::list<int> & ids, std::list<
memcpy(velocity.data(), data, dataSize);
}
}
if(uStrNumCmp(_version, "0.14.0") >= 0)
{
gps.resize(6,0);
data = sqlite3_column_blob(ppStmt, index); // gps
dataSize = sqlite3_column_bytes(ppStmt, index++);
if((unsigned int)dataSize == gps.size()*sizeof(double) && data)
{
memcpy(gps.data(), data, dataSize);
}
}
}
}
@@ -2352,6 +2393,10 @@ void DBDriverSqlite3::loadSignaturesQuery(const std::list<int> & ids, std::list<
{
s->setVelocity(velocity[0], velocity[1], velocity[2], velocity[3], velocity[4], velocity[5]);
}
if(gps.size() == 6)
{
s->sensorData().setGPS(gps[0], gps[1], gps[2], gps[3], gps[4], gps[5]);
}
s->setSaved(true);
nodes.push_back(s);
++loaded;
@@ -4213,7 +4258,11 @@ cv::Mat DBDriverSqlite3::loadOptimizedMeshQuery(
std::string DBDriverSqlite3::queryStepNode() const
{
if(uStrNumCmp(_version, "0.13.0") >= 0)
if(uStrNumCmp(_version, "0.14.0") >= 0)
{
return "INSERT INTO Node(id, map_id, weight, pose, stamp, label, ground_truth_pose, velocity, gps) VALUES(?,?,?,?,?,?,?,?,?);";
}
else if(uStrNumCmp(_version, "0.13.0") >= 0)
{
return "INSERT INTO Node(id, map_id, weight, pose, stamp, label, ground_truth_pose, velocity) VALUES(?,?,?,?,?,?,?,?);";
}
@@ -4293,6 +4342,20 @@ void DBDriverSqlite3::stepNode(sqlite3_stmt * ppStmt, const Signature * s) const
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
}
}
if(uStrNumCmp(_version, "0.14.0") >= 0)
{
if(s->sensorData().gps().empty())
{
rc = sqlite3_bind_null(ppStmt, index++);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
}
else
{
rc = sqlite3_bind_blob(ppStmt, index++, s->sensorData().gps().data(), s->sensorData().gps().size()*sizeof(double), SQLITE_STATIC);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
}
}
}
}
else if(uStrNumCmp(_version, "0.8.8") >= 0)

2
corelib/src/DBDriverSqlite3.h
View File

@@ -127,7 +127,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 bool getNodeInfoQuery(int signatureId, Transform & pose, int & mapId, int & weight, std::string & label, double & stamp, Transform & groundTruthPose, std::vector<float> & velocity, std::vector<double> & gps) const;
virtual void getAllNodeIdsQuery(std::set<int> & ids, bool ignoreChildren, bool ignoreBadSignatures) const;
virtual void getAllLinksQuery(std::multimap<int, Link> & links, bool ignoreNullLinks) const;
virtual void getLastIdQuery(const std::string & tableName, int & id) const;

6
corelib/src/DBReader.cpp
View File

@@ -268,7 +268,8 @@ SensorData DBReader::captureImage(CameraInfo * info)
int mapId;
Transform localTransform, pose, groundTruth;
std::vector<float> velocity;
_dbDriver->getNodeInfo(*_currentId, pose, mapId, weight, label, stamp, groundTruth, velocity);
std::vector<double> gps;
_dbDriver->getNodeInfo(*_currentId, pose, mapId, weight, label, stamp, groundTruth, velocity, gps);
if(previousStamp && stamp && stamp > previousStamp)
{
delay = stamp - previousStamp;
@@ -322,7 +323,8 @@ SensorData DBReader::getNextData(CameraInfo * info)
double stamp;
Transform groundTruth;
std::vector<float> velocity;
_dbDriver->getNodeInfo(*_currentId, pose, mapId, weight, label, stamp, groundTruth, velocity);
std::vector<double> gps;
_dbDriver->getNodeInfo(*_currentId, pose, mapId, weight, label, stamp, groundTruth, velocity, gps);
cv::Mat infMatrix = cv::Mat::eye(6,6,CV_64FC1);
if(!_odometryIgnored)

14
corelib/src/Memory.cpp
View File

@@ -3051,7 +3051,8 @@ Transform Memory::getOdomPose(int signatureId, bool lookInDatabase) const
std::string label;
double stamp;
std::vector<float> velocity;
getNodeInfo(signatureId, pose, mapId, weight, label, stamp, groundTruth, velocity, lookInDatabase);
std::vector<double> gps;
getNodeInfo(signatureId, pose, mapId, weight, label, stamp, groundTruth, velocity, gps, lookInDatabase);
return pose;
}
@@ -3062,7 +3063,8 @@ Transform Memory::getGroundTruthPose(int signatureId, bool lookInDatabase) const
std::string label;
double stamp;
std::vector<float> velocity;
getNodeInfo(signatureId, pose, mapId, weight, label, stamp, groundTruth, velocity, lookInDatabase);
std::vector<double> gps;
getNodeInfo(signatureId, pose, mapId, weight, label, stamp, groundTruth, velocity, gps, lookInDatabase);
return groundTruth;
}
@@ -3074,6 +3076,7 @@ bool Memory::getNodeInfo(int signatureId,
double & stamp,
Transform & groundTruth,
std::vector<float> & velocity,
std::vector<double> & gps,
bool lookInDatabase) const
{
const Signature * s = this->getSignature(signatureId);
@@ -3086,11 +3089,12 @@ bool Memory::getNodeInfo(int signatureId,
stamp = s->getStamp();
groundTruth = s->getGroundTruthPose();
velocity = s->getVelocity();
gps = s->sensorData().gps();
return true;
}
else if(lookInDatabase && _dbDriver)
{
return _dbDriver->getNodeInfo(signatureId, odomPose, mapId, weight, label, stamp, groundTruth, velocity);
return _dbDriver->getNodeInfo(signatureId, odomPose, mapId, weight, label, stamp, groundTruth, velocity, gps);
}
return false;
}
@@ -3963,6 +3967,10 @@ Signature * Memory::createSignature(const SensorData & data, const Transform & p
s->sensorData().setUserDataRaw(data.userDataRaw());
s->sensorData().setGroundTruth(data.groundTruth());
if(!data.gps().empty())
{
s->sensorData().setGPS(data.gps()[0], data.gps()[1], data.gps()[2], data.gps()[3], data.gps()[4], data.gps()[5]);
}
t = timer.ticks();
if(stats) stats->addStatistic(Statistics::kTimingMemCompressing_data(), t*1000.0f);

24
corelib/src/Rtabmap.cpp
View File

@@ -761,7 +761,8 @@ void Rtabmap::exportPoses(const std::string & path, bool optimized, bool global,
std::string l;
double stamp = 0.0;
std::vector<float> v;
_memory->getNodeInfo(iter->first, o, m, w, l, stamp, g, v, true);
std::vector<double> gps;
_memory->getNodeInfo(iter->first, o, m, w, l, stamp, g, v, gps, true);
stamps.insert(std::make_pair(iter->first, stamp));
}
}
@@ -2650,7 +2651,8 @@ bool Rtabmap::process(
double stamp = 0;
Transform groundTruth;
std::vector<float> velocity;
_memory->getNodeInfo(iter->first, odomPoseLocal, mapId, weight, label, stamp, groundTruth, velocity, false);
std::vector<double> gps;
_memory->getNodeInfo(iter->first, odomPoseLocal, mapId, weight, label, stamp, groundTruth, velocity, gps, false);
signatures.insert(std::make_pair(iter->first,
Signature(iter->first,
mapId,
@@ -2663,6 +2665,10 @@ bool Rtabmap::process(
{
signatures.at(iter->first).setVelocity(velocity[0], velocity[1], velocity[2], velocity[3], velocity[4], velocity[5]);
}
if(!gps.empty())
{
signatures.at(iter->first).sensorData().setGPS(gps[0], gps[1], gps[2], gps[3], gps[4], gps[5]);
}
}
localGraphSize = (int)poses.size();
if(!lastSignatureLocalizedPose.isNull())
@@ -3340,7 +3346,8 @@ void Rtabmap::get3DMap(
double stamp = 0;
Transform groundTruth;
std::vector<float> velocity;
_memory->getNodeInfo(*iter, odomPoseLocal, mapId, weight, label, stamp, groundTruth, velocity, true);
std::vector<double> gps;
_memory->getNodeInfo(*iter, odomPoseLocal, mapId, weight, label, stamp, groundTruth, velocity, gps, true);
SensorData data = _memory->getNodeData(*iter);
data.setId(*iter);
std::multimap<int, cv::KeyPoint> words;
@@ -3363,6 +3370,10 @@ void Rtabmap::get3DMap(
{
signatures.at(*iter).setVelocity(velocity[0], velocity[1], velocity[2], velocity[3], velocity[4], velocity[5]);
}
if(!gps.empty())
{
signatures.at(*iter).sensorData().setGPS(gps[0], gps[1], gps[2], gps[3], gps[4], gps[5]);
}
}
}
else if(_memory && (_memory->getStMem().size() || _memory->getWorkingMem().size() > 1))
@@ -3415,7 +3426,8 @@ void Rtabmap::getGraph(
double stamp = 0;
Transform groundTruth;
std::vector<float> velocity;
_memory->getNodeInfo(iter->first, odomPoseLocal, mapId, weight, label, stamp, groundTruth, velocity, global);
std::vector<double> gps;
_memory->getNodeInfo(iter->first, odomPoseLocal, mapId, weight, label, stamp, groundTruth, velocity, gps, global);
signatures->insert(std::make_pair(iter->first,
Signature(iter->first,
mapId,
@@ -3443,6 +3455,10 @@ void Rtabmap::getGraph(
{
signatures->at(iter->first).setVelocity(velocity[0], velocity[1], velocity[2], velocity[3], velocity[4], velocity[5]);
}
if(!gps.empty())
{
signatures->at(iter->first).sensorData().setGPS(gps[0], gps[1], gps[2], gps[3], gps[4], gps[5]);
}
}
}
}

1
corelib/src/resources/DatabaseSchema.sql.in
View File

@@ -22,6 +22,7 @@ CREATE TABLE Node (
ground_truth_pose BLOB, -- 3x4 float
velocity BLOB, -- 6 float (vx,vy,vz,vroll,vpitch,vyaw) m/s and rad/s
label TEXT,
gps BLOB, -- 1x6 double: stamp, longitude (DD), latitude (DD), altitude (m), accuracy (m), bearing (North 0->360 deg clockwise)
time_enter DATE,
PRIMARY KEY (id)