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

DBReader: publish global pose if prior link is detected, negative image rate means now a ratio of the database stamps

Browse Source
This commit is contained in:
matlabbe
2018-10-24 09:26:41 +12:00
parent 8e99291e13
commit 299bec15ff
2 changed files with 23 additions and 5 deletions
Download Patch File Download Diff File Expand all files Collapse all files

4
corelib/src/DBDriverSqlite3.cpp
View File

@@ -2300,7 +2300,7 @@ bool DBDriverSqlite3::getNodeInfoQuery(int signatureId,
int sensorsNum = (dataSize/sizeof(double))/3; int sensorsNum = (dataSize/sizeof(double))/3;
for(int i=0; i<sensorsNum;++i) for(int i=0; i<sensorsNum;++i)
{ {
EnvSensor::Type type = (EnvSensor::Type)dataDouble[i*3]; EnvSensor::Type type = (EnvSensor::Type)(int)dataDouble[i*3];
sensors.insert(std::make_pair(type, EnvSensor(type, dataDouble[i*3+1], dataDouble[i*3+2]))); sensors.insert(std::make_pair(type, EnvSensor(type, dataDouble[i*3+1], dataDouble[i*3+2])));
} }
} }
@@ -2844,7 +2844,7 @@ void DBDriverSqlite3::loadSignaturesQuery(const std::list<int> & ids, std::list<
int sensorsNum = (dataSize/sizeof(double))/3; int sensorsNum = (dataSize/sizeof(double))/3;
for(int i=0; i<sensorsNum;++i) for(int i=0; i<sensorsNum;++i)
{ {
EnvSensor::Type type = (EnvSensor::Type)dataDouble[i*3]; EnvSensor::Type type = (EnvSensor::Type)(int)dataDouble[i*3];
sensors.insert(std::make_pair(type, EnvSensor(type, dataDouble[i*3+1], dataDouble[i*3+2]))); sensors.insert(std::make_pair(type, EnvSensor(type, dataDouble[i*3+1], dataDouble[i*3+2])));
} }
} }

24
corelib/src/DBReader.cpp
View File

@@ -325,8 +325,21 @@ SensorData DBReader::getNextData(CameraInfo * info)
std::vector<float> velocity; std::vector<float> velocity;
GPS gps; GPS gps;
EnvSensors sensors; EnvSensors sensors;
Transform globalPose;
cv::Mat globalPoseCov;
_dbDriver->getNodeInfo(*_currentId, pose, mapId, weight, label, stamp, groundTruth, velocity, gps, sensors); _dbDriver->getNodeInfo(*_currentId, pose, mapId, weight, label, stamp, groundTruth, velocity, gps, sensors);
std::map<int, Link> priorLinks;
_dbDriver->loadLinks(*_currentId, priorLinks, Link::kPosePrior);
if( priorLinks.size() &&
!priorLinks.begin()->second.transform().isNull() &&
priorLinks.begin()->second.infMatrix().cols == 6 &&
priorLinks.begin()->second.infMatrix().rows == 6)
{
globalPose = priorLinks.begin()->second.transform();
globalPoseCov = priorLinks.begin()->second.infMatrix().inv();
}
cv::Mat infMatrix = cv::Mat::eye(6,6,CV_64FC1); cv::Mat infMatrix = cv::Mat::eye(6,6,CV_64FC1);
if(!_odometryIgnored) if(!_odometryIgnored)
{ {
@@ -377,7 +390,8 @@ SensorData DBReader::getNextData(CameraInfo * info)
} }
else if(_previousMapID == mapId && _previousStamp > 0) else if(_previousMapID == mapId && _previousStamp > 0)
{ {
int sleepTime = 1000.0*(stamp-_previousStamp) - 1000.0*_timer.getElapsedTime(); float ratio = -this->getImageRate();
int sleepTime = 1000.0*(stamp-_previousStamp)/ratio - 1000.0*_timer.getElapsedTime();
if(sleepTime > 10000) if(sleepTime > 10000)
{ {
UWARN("Detected long delay (%d sec, stamps = %f vs %f). Waiting a maximum of 10 seconds.", UWARN("Detected long delay (%d sec, stamps = %f vs %f). Waiting a maximum of 10 seconds.",
@@ -390,14 +404,14 @@ SensorData DBReader::getNextData(CameraInfo * info)
} }
// Add precision at the cost of a small overhead // Add precision at the cost of a small overhead
while(_timer.getElapsedTime() < (stamp-_previousStamp)-0.000001) while(_timer.getElapsedTime() < (stamp-_previousStamp)/ratio-0.000001)
{ {
// //
} }
double slept = _timer.getElapsedTime(); double slept = _timer.getElapsedTime();
_timer.start(); _timer.start();
UDEBUG("slept=%fs vs target=%fs", slept, stamp-_previousStamp); UDEBUG("slept=%fs vs target=%fs (ratio=%f)", slept, (stamp-_previousStamp)/ratio, ratio);
} }
_previousStamp = stamp; _previousStamp = stamp;
_previousMapID = mapId; _previousMapID = mapId;
@@ -438,6 +452,10 @@ SensorData DBReader::getNextData(CameraInfo * info)
data.setId(seq); data.setId(seq);
data.setStamp(stamp); data.setStamp(stamp);
data.setGroundTruth(groundTruth); data.setGroundTruth(groundTruth);
if(globalPose.isNull())
{
data.setGlobalPose(globalPose, globalPoseCov);
}
data.setGPS(gps); data.setGPS(gps);
data.setEnvSensors(sensors); data.setEnvSensors(sensors);
UDEBUG("Laser=%d RGB/Left=%d Depth/Right=%d, UserData=%d", UDEBUG("Laser=%d RGB/Left=%d Depth/Right=%d, UserData=%d",