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

Updated wifi example

Browse Source
This commit is contained in:
matlabbe
2018-11-09 18:09:12 -05:00
parent 8333677dc6
commit be1532b809
2 changed files with 19 additions and 5 deletions
Download Patch File Download Diff File Expand all files Collapse all files
examples/WifiMapping/MapBuilderWifi.h
+17 -5
View File
@@ -91,20 +91,32 @@ protected Q_SLOTS:
wifiLevels_.insert(std::make_pair(stamp, level));
}
// for the logic below, we should keep only stamps for
// nodes still in the graph (in case nodes are ignored when not moving)
std::map<double, int> nodeStamps;
for(std::map<double, int>::iterator iter=nodeStamps_.begin(); iter!=nodeStamps_.end(); ++iter)
{
std::map<int, Transform>::const_iterator jter = poses.find(iter->second);
if(jter != poses.end())
{
nodeStamps.insert(*iter);
}
}
int id = 0;
for(std::map<double, int>::iterator iter=wifiLevels_.begin(); iter!=wifiLevels_.end(); ++iter, ++id)
{
// The Wifi value may be taken between two nodes, interpolate its position.
double stampWifi = iter->first;
std::map<double, int>::iterator previousNode = nodeStamps_.lower_bound(stampWifi); // lower bound of the stamp
if(previousNode!=nodeStamps_.end() && previousNode->first > stampWifi && previousNode != nodeStamps_.begin())
std::map<double, int>::iterator previousNode = nodeStamps.lower_bound(stampWifi); // lower bound of the stamp
if(previousNode!=nodeStamps.end() && previousNode->first > stampWifi && previousNode != nodeStamps.begin())
{
--previousNode;
}
std::map<double, int>::iterator nextNode = nodeStamps_.upper_bound(stampWifi); // upper bound of the stamp
std::map<double, int>::iterator nextNode = nodeStamps.upper_bound(stampWifi); // upper bound of the stamp
if(previousNode != nodeStamps_.end() &&
nextNode != nodeStamps_.end() &&
if(previousNode != nodeStamps.end() &&
nextNode != nodeStamps.end() &&
previousNode->second != nextNode->second &&
uContains(poses, previousNode->second) && uContains(poses, nextNode->second))
{
examples/WifiMapping/main.cpp
+2
View File
@@ -214,6 +214,8 @@ int main(int argc, char * argv[])
Rtabmap * rtabmap = new Rtabmap();
ParametersMap param;
param.insert(ParametersPair(Parameters::kMemRehearsalSimilarity(), "1.0")); // disable rehearsal (node merging when not moving)
param.insert(ParametersPair(Parameters::kRGBDLinearUpdate(), "0")); // disable node ignored when not moving
param.insert(ParametersPair(Parameters::kRGBDAngularUpdate(), "0")); // disable node ignored when not moving
rtabmap->init(param);
RtabmapThread rtabmapThread(rtabmap); // ownership is transfered