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

Version 0.10.1: user_data is now a cv::Mat to avoid a deep copy when SensorData is copied

Browse Source
This commit is contained in:
matlabbe
2015-06-28 19:22:18 -04:00
parent 7d3a3317b7
commit 4f96fd3530
22 changed files with 516 additions and 640 deletions
Download Patch File Download Diff File Expand all files Collapse all files

63
examples/WifiMapping/MapBuilderWifi.h
View File

@@ -29,6 +29,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#define MAPBUILDERWIFI_H_
#include "../RGBDMapping/MapBuilder.h"
#include "rtabmap/core/UserDataEvent.h"
using namespace rtabmap;
@@ -64,6 +65,28 @@ public:
this->unregisterFromEventsManager();
}
protected:
virtual void handleEvent(UEvent * event)
{
if(event->getClassName().compare("UserDataEvent") == 0)
{
UserDataEvent * rtabmapEvent = (UserDataEvent *)event;
// convert userData to wifi levels
if(!rtabmapEvent->data().empty())
{
UASSERT(rtabmapEvent->data().type() == CV_64FC1 &&
rtabmapEvent->data().cols == 2 &&
rtabmapEvent->data().rows == 1);
// format [int level, double stamp]
int level = rtabmapEvent->data().at<double>(0);
double stamp = rtabmapEvent->data().at<double>(1);
wifiLevels_.insert(std::make_pair(stamp, level));
}
}
MapBuilder::handleEvent(event);
}
protected slots:
virtual void processStatistics(const rtabmap::Statistics & stats)
{
@@ -76,7 +99,6 @@ protected slots:
// Add WIFI symbols
//============================
std::map<double, int> nodeStamps; // <stamp, id>
std::map<int, std::pair<int, double> > wifiLevels;
for(std::map<int, Signature>::const_iterator iter=stats.getSignatures().begin();
iter!=stats.getSignatures().end();
@@ -84,34 +106,22 @@ protected slots:
{
// Sort stamps by stamps->id
nodeStamps.insert(std::make_pair(iter->second.getStamp(), iter->first));
// convert userData to wifi levels
if(iter->second.getUserData().size())
{
UASSERT(iter->second.getUserData().size() == sizeof(int)+sizeof(double));
// format [int level, double stamp]
int level;
double stamp;
memcpy(&level, iter->second.getUserData().data(), sizeof(int));
memcpy(&stamp, iter->second.getUserData().data()+sizeof(int), sizeof(double));
wifiLevels.insert(std::make_pair(iter->first, std::make_pair(level, stamp)));
}
}
for(std::map<int, std::pair<int, double> >::iterator iter=wifiLevels.begin(); iter!=wifiLevels.end(); ++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->second.second;
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())
{
--previousNode;
}
std::map<double, int>::iterator nextNode = nodeStamps.upper_bound(iter->second.second); // 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))
{
@@ -130,18 +140,18 @@ protected slots:
Transform wifiPose = (poseA*v).translation(); // rip off the rotation
std::string cloudName = uFormat("level%d", iter->first);
std::string cloudName = uFormat("level%d", id);
if(clouds.contains(cloudName))
{
if(!cloudViewer_->updateCloudPose(cloudName, wifiPose))
{
UERROR("Updating pose cloud %d failed!", iter->first);
UERROR("Updating pose cloud %d failed!", id);
}
}
else
{
// Make a line with points
int quality = dBm2Quality(iter->second.first)/10;
int quality = dBm2Quality(iter->second)/10;
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
for(int i=0; i<10; ++i)
{
@@ -167,7 +177,7 @@ protected slots:
//UWARN("level %d -> %d pose=%s size=%d", level, iter->second.first, wifiPose.prettyPrint().c_str(), (int)cloud->size());
if(!cloudViewer_->addOrUpdateCloud(cloudName, cloud, wifiPose, Qt::yellow))
{
UERROR("Adding cloud %d to viewer failed!", iter->first);
UERROR("Adding cloud %d to viewer failed!", id);
}
else
{
@@ -175,10 +185,6 @@ protected slots:
}
}
}
else
{
UWARN("Bounds not found!");
}
}
//============================
@@ -186,6 +192,9 @@ protected slots:
//============================
MapBuilder::processStatistics(stats);
}
private:
std::map<double, int> wifiLevels_;
};