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

Merge branch 'master' of https://github.com/introlab/rtabmap into devel

Browse Source
This commit is contained in:
matlabbe
2017-12-11 17:10:08 -05:00
parent 775b80eff5 d776092b35
commit f973fc3743
6 changed files with 48 additions and 14 deletions
Download Patch File Download Diff File Expand all files Collapse all files

1
corelib/include/rtabmap/core/RtabmapThread.h
View File

@@ -113,6 +113,7 @@ private:
std::queue<ParametersMap> _stateParam;
std::list<OdometryEvent> _dataBuffer;
std::list<double> _newMapEvents;
UMutex _dataMutex;
USemaphore _dataAdded;
unsigned int _dataBufferMaxSize;

4
corelib/src/DBReader.cpp
View File

@@ -401,6 +401,10 @@ SensorData DBReader::getNextData(CameraInfo * info)
_previousStamp = stamp;
_previousMapID = mapId;
}
else
{
stamp = 0;
}
data.uncompressData();
if(data.cameraModels().size() > 1 &&

2
corelib/src/Odometry.cpp
View File

@@ -235,7 +235,7 @@ Transform Odometry::process(SensorData & data, const Transform & guessIn, Odomet
UTimer alignTimer;
pcl::IndicesPtr indices(new std::vector<int>);
pcl::IndicesPtr ground, obstacles;
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud = util3d::cloudFromSensorData(data, 1, 0, 0, indices.get());
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud = util3d::cloudFromSensorData(data, 1, 10, 0, indices.get());
bool success = false;
if(indices->size())
{

22
corelib/src/RtabmapThread.cpp
View File

@@ -85,6 +85,7 @@ void RtabmapThread::clearBufferedData()
_dataMutex.lock();
{
_dataBuffer.clear();
_newMapEvents.clear();
lastPose_.setIdentity();
covariance_ = cv::Mat();
_previousStamp = 0;
@@ -572,13 +573,13 @@ void RtabmapThread::addData(const OdometryEvent & odomEvent)
{
if(odomEvent.pose().isIdentity())
{
UWARN("Odometry is reset (identity pose detected). Increment map id!");
UWARN("Odometry is reset (identity pose detected). Increment map id! (stamp=%fs)", odomEvent.data().stamp());
}
else
{
UWARN("Odometry is reset (high variance (%f >=9999 detected). Increment map id!", odomEvent.info().reg.covariance.at<double>(0,0));
UWARN("Odometry is reset (high variance (%f >=9999 detected, stamp=%fs). Increment map id!", odomEvent.info().reg.covariance.at<double>(0,0), odomEvent.data().stamp());
}
pushNewState(kStateTriggeringMap);
_newMapEvents.push_back(odomEvent.data().stamp());
covariance_ = cv::Mat();
}
@@ -654,7 +655,7 @@ bool RtabmapThread::getData(OdometryEvent & data)
ULOGGER_INFO("wake-up");
bool dataFilled = false;
bool triggerNewMap = false;
_dataMutex.lock();
{
if(_state.empty() && !_dataBuffer.empty())
@@ -672,10 +673,23 @@ bool RtabmapThread::getData(OdometryEvent & data)
}
_userDataMutex.unlock();
while(_newMapEvents.size() && _newMapEvents.front() <= data.data().stamp())
{
UWARN("Triggering new map %f<=%f...", _newMapEvents.front() , data.data().stamp());
triggerNewMap = true;
_newMapEvents.pop_front();
}
dataFilled = true;
}
}
_dataMutex.unlock();
if(triggerNewMap)
{
_rtabmap->triggerNewMap();
}
return dataFilled;
}