MapBuilderWifi.h
Go to the documentation of this file.
1 /*
2 Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
3 All rights reserved.
4 
5 Redistribution and use in source and binary forms, with or without
6 modification, are permitted provided that the following conditions are met:
7  * Redistributions of source code must retain the above copyright
8  notice, this list of conditions and the following disclaimer.
9  * Redistributions in binary form must reproduce the above copyright
10  notice, this list of conditions and the following disclaimer in the
11  documentation and/or other materials provided with the distribution.
12  * Neither the name of the Universite de Sherbrooke nor the
13  names of its contributors may be used to endorse or promote products
14  derived from this software without specific prior written permission.
15 
16 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
17 ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
18 WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
19 DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
20 DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
21 (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
22 LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
23 ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
24 (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
25 SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
26 */
27 
28 #ifndef MAPBUILDERWIFI_H_
29 #define MAPBUILDERWIFI_H_
30 
31 #include "MapBuilder.h"
32 #include "rtabmap/core/UserDataEvent.h"
33 
34 using namespace rtabmap;
35 
36 // A percentage value that represents the signal quality
37 // of the network. WLAN_SIGNAL_QUALITY is of type ULONG.
38 // This member contains a value between 0 and 100. A value
39 // of 0 implies an actual RSSI signal strength of -100 dbm.
40 // A value of 100 implies an actual RSSI signal strength of -50 dbm.
41 // You can calculate the RSSI signal strength value for wlanSignalQuality
42 // values between 1 and 99 using linear interpolation.
43 inline int dBm2Quality(int dBm)
44 {
45  // dBm to Quality:
46  if(dBm <= -100)
47  return 0;
48  else if(dBm >= -50)
49  return 100;
50  else
51  return 2 * (dBm + 100);
52 }
53 
54 class MapBuilderWifi : public MapBuilder
55 {
56  Q_OBJECT
57 public:
58  // Camera ownership is not transferred!
59  MapBuilderWifi(CameraThread * camera = 0) :
60  MapBuilder(camera)
61  {}
62 
63  virtual ~MapBuilderWifi()
64  {
65  this->unregisterFromEventsManager();
66  }
67 
68 protected Q_SLOTS:
69  virtual void processStatistics(const rtabmap::Statistics & stats)
70  {
71  processingStatistics_ = true;
72 
73  const std::map<int, Transform> & poses = stats.poses();
74  QMap<std::string, Transform> clouds = cloudViewer_->getAddedClouds();
75 
76  //============================
77  // Add WIFI symbols
78  //============================
79  std::map<double, int> nodeStamps; // <stamp, id>
80 
81  for(std::map<int, Signature>::const_iterator iter=stats.getSignatures().begin();
82  iter!=stats.getSignatures().end();
83  ++iter)
84  {
85  // Sort stamps by stamps->id
86  nodeStamps.insert(std::make_pair(iter->second.getStamp(), iter->first));
87 
88  if(!iter->second.sensorData().userDataRaw().empty())
89  {
90  UASSERT(iter->second.sensorData().userDataRaw().type() == CV_64FC1 &&
91  iter->second.sensorData().userDataRaw().cols == 2 &&
92  iter->second.sensorData().userDataRaw().rows == 1);
93 
94  // format [int level, double stamp]
95  int level = iter->second.sensorData().userDataRaw().at<double>(0);
96  double stamp = iter->second.sensorData().userDataRaw().at<double>(1);
97  wifiLevels_.insert(std::make_pair(stamp, level));
98  }
99  }
100 
101  int id = 0;
102  for(std::map<double, int>::iterator iter=wifiLevels_.begin(); iter!=wifiLevels_.end(); ++iter, ++id)
103  {
104  // The Wifi value may be taken between two nodes, interpolate its position.
105  double stampWifi = iter->first;
106  std::map<double, int>::iterator previousNode = nodeStamps.lower_bound(stampWifi); // lower bound of the stamp
107  if(previousNode!=nodeStamps.end() && previousNode->first > stampWifi && previousNode != nodeStamps.begin())
108  {
109  --previousNode;
110  }
111  std::map<double, int>::iterator nextNode = nodeStamps.upper_bound(stampWifi); // upper bound of the stamp
112 
113  if(previousNode != nodeStamps.end() &&
114  nextNode != nodeStamps.end() &&
115  previousNode->second != nextNode->second &&
116  uContains(poses, previousNode->second) && uContains(poses, nextNode->second))
117  {
118  Transform poseA = poses.at(previousNode->second);
119  Transform poseB = poses.at(nextNode->second);
120  double stampA = previousNode->first;
121  double stampB = nextNode->first;
122  UASSERT(stampWifi>=stampA && stampWifi <=stampB);
123 
124  Transform v = poseA.inverse() * poseB;
125  double ratio = (stampWifi-stampA)/(stampB-stampA);
126 
127  v.x()*=ratio;
128  v.y()*=ratio;
129  v.z()*=ratio;
130 
131  Transform wifiPose = (poseA*v).translation(); // rip off the rotation
132 
133  std::string cloudName = uFormat("level%d", id);
134  if(clouds.contains(cloudName))
135  {
136  if(!cloudViewer_->updateCloudPose(cloudName, wifiPose))
137  {
138  UERROR("Updating pose cloud %d failed!", id);
139  }
140  }
141  else
142  {
143  // Make a line with points
144  int quality = dBm2Quality(iter->second)/10;
145  pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
146  for(int i=0; i<10; ++i)
147  {
148  // 2 cm between each points
149  // the number of points depends on the dBm (which varies from -30 (near) to -80 (far))
150  pcl::PointXYZRGB pt;
151  pt.z = float(i+1)*0.02f;
152  if(i<quality)
153  {
154  // yellow
155  pt.r = 255;
156  pt.g = 255;
157  }
158  else
159  {
160  // gray
161  pt.r = pt.g = pt.b = 100;
162  }
163  cloud->push_back(pt);
164  }
165  pcl::PointXYZRGB anchor(255, 0, 0);
166  cloud->push_back(anchor);
167  //UWARN("level %d -> %d pose=%s size=%d", level, iter->second.first, wifiPose.prettyPrint().c_str(), (int)cloud->size());
168  if(!cloudViewer_->addCloud(cloudName, cloud, wifiPose, Qt::yellow))
169  {
170  UERROR("Adding cloud %d to viewer failed!", id);
171  }
172  else
173  {
174  cloudViewer_->setCloudPointSize(cloudName, 5);
175  }
176  }
177  }
178  }
179 
180  //============================
181  // Add RGB-D clouds
182  //============================
183  MapBuilder::processStatistics(stats);
184  }
185 
186 private:
187  std::map<double, int> wifiLevels_;
188 };
189 
190 
191 #endif /* MAPBUILDERWIFI_H_ */
MapBuilderWifi::MapBuilderWifi
MapBuilderWifi(CameraThread *camera=0)
Definition: MapBuilderWifi.h:59
MapBuilderWifi::~MapBuilderWifi
virtual ~MapBuilderWifi()
Definition: MapBuilderWifi.h:63
rtabmap::Statistics::getSignatures
const std::map< int, Signature > & getSignatures() const
Definition: Statistics.h:207
MapBuilder::processStatistics
void processStatistics(const rtabmap::Statistics &stats)
Definition: NoEventsExample/MapBuilder.h:142
MapBuilderWifi::wifiLevels_
std::map< double, int > wifiLevels_
Definition: MapBuilderWifi.h:187
UASSERT
#define UASSERT(condition)
MapBuilderWifi::processStatistics
virtual void processStatistics(const rtabmap::Statistics &stats)
Definition: MapBuilderWifi.h:69
uContains
bool uContains(const std::list< V > &list, const V &value)
Definition: UStl.h:409
rtabmap::Statistics::poses
const std::map< int, Transform > & poses() const
Definition: Statistics.h:209
UERROR
#define UERROR(...)
rtabmap::Transform::inverse
Transform inverse() const
Definition: Transform.cpp:169
dBm2Quality
int dBm2Quality(int dBm)
Definition: MapBuilderWifi.h:43
uFormat
std::string UTILITE_EXP uFormat(const char *fmt,...)
Definition: UConversion.cpp:350

rtabmap
Author(s): Mathieu Labbe
autogenerated on Wed Jun 5 2019 22:41:31