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(SensorCaptureThread * 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  // Sort stamps by stamps->id
80  nodeStamps_.insert(std::make_pair(stats.getLastSignatureData().getStamp(), stats.getLastSignatureData().id()));
81 
82  if(!stats.getLastSignatureData().sensorData().userDataRaw().empty())
83  {
84  UASSERT(stats.getLastSignatureData().sensorData().userDataRaw().type() == CV_64FC1 &&
85  stats.getLastSignatureData().sensorData().userDataRaw().cols == 2 &&
86  stats.getLastSignatureData().sensorData().userDataRaw().rows == 1);
87 
88  // format [int level, double stamp]
89  int level = stats.getLastSignatureData().sensorData().userDataRaw().at<double>(0);
90  double stamp = stats.getLastSignatureData().sensorData().userDataRaw().at<double>(1);
91  wifiLevels_.insert(std::make_pair(stamp, level));
92  }
93 
94  // for the logic below, we should keep only stamps for
95  // nodes still in the graph (in case nodes are ignored when not moving)
96  std::map<double, int> nodeStamps;
97  for(std::map<double, int>::iterator iter=nodeStamps_.begin(); iter!=nodeStamps_.end(); ++iter)
98  {
99  std::map<int, Transform>::const_iterator jter = poses.find(iter->second);
100  if(jter != poses.end())
101  {
102  nodeStamps.insert(*iter);
103  }
104  }
105 
106  int id = 0;
107  for(std::map<double, int>::iterator iter=wifiLevels_.begin(); iter!=wifiLevels_.end(); ++iter, ++id)
108  {
109  // The Wifi value may be taken between two nodes, interpolate its position.
110  double stampWifi = iter->first;
111  std::map<double, int>::iterator previousNode = nodeStamps.lower_bound(stampWifi); // lower bound of the stamp
112  if(previousNode!=nodeStamps.end() && previousNode->first > stampWifi && previousNode != nodeStamps.begin())
113  {
114  --previousNode;
115  }
116  std::map<double, int>::iterator nextNode = nodeStamps.upper_bound(stampWifi); // upper bound of the stamp
117 
118  if(previousNode != nodeStamps.end() &&
119  nextNode != nodeStamps.end() &&
120  previousNode->second != nextNode->second &&
121  uContains(poses, previousNode->second) && uContains(poses, nextNode->second))
122  {
123  Transform poseA = poses.at(previousNode->second);
124  Transform poseB = poses.at(nextNode->second);
125  double stampA = previousNode->first;
126  double stampB = nextNode->first;
127  UASSERT(stampWifi>=stampA && stampWifi <=stampB);
128 
129  Transform v = poseA.inverse() * poseB;
130  double ratio = (stampWifi-stampA)/(stampB-stampA);
131 
132  v.x()*=ratio;
133  v.y()*=ratio;
134  v.z()*=ratio;
135 
136  Transform wifiPose = (poseA*v).translation(); // rip off the rotation
137 
138  std::string cloudName = uFormat("level%d", id);
139  if(clouds.contains(cloudName))
140  {
141  if(!cloudViewer_->updateCloudPose(cloudName, wifiPose))
142  {
143  UERROR("Updating pose cloud %d failed!", id);
144  }
145  }
146  else
147  {
148  // Make a line with points
149  int quality = dBm2Quality(iter->second)/10;
150  pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
151  for(int i=0; i<10; ++i)
152  {
153  // 2 cm between each points
154  // the number of points depends on the dBm (which varies from -30 (near) to -80 (far))
155  pcl::PointXYZRGB pt;
156  pt.z = float(i+1)*0.02f;
157  if(i<quality)
158  {
159  // yellow
160  pt.r = 255;
161  pt.g = 255;
162  }
163  else
164  {
165  // gray
166  pt.r = pt.g = pt.b = 100;
167  }
168  cloud->push_back(pt);
169  }
170  pcl::PointXYZRGB anchor;
171  anchor.r = 255;
172  cloud->push_back(anchor);
173  //UWARN("level %d -> %d pose=%s size=%d", level, iter->second.first, wifiPose.prettyPrint().c_str(), (int)cloud->size());
174  if(!cloudViewer_->addCloud(cloudName, cloud, wifiPose, Qt::yellow))
175  {
176  UERROR("Adding cloud %d to viewer failed!", id);
177  }
178  else
179  {
180  cloudViewer_->setCloudPointSize(cloudName, 5);
181  }
182  }
183  }
184  }
185 
186  //============================
187  // Add RGB-D clouds
188  //============================
189  MapBuilder::processStatistics(stats);
190  }
191 
192 private:
193  std::map<double, int> wifiLevels_;
194  std::map<double, int> nodeStamps_; // <stamp, id>
195 };
196 
197 
198 #endif /* MAPBUILDERWIFI_H_ */
rtabmap::SensorCaptureThread
Definition: SensorCaptureThread.h:58
dBm2Quality
int dBm2Quality(int dBm)
Definition: MapBuilderWifi.h:43
rtabmap::Statistics
Definition: Statistics.h:53
MapBuilderWifi
Definition: MapBuilderWifi.h:54
MapBuilderWifi::nodeStamps_
std::map< double, int > nodeStamps_
Definition: MapBuilderWifi.h:194
MapBuilder.h
MapBuilderWifi::~MapBuilderWifi
virtual ~MapBuilderWifi()
Definition: MapBuilderWifi.h:63
MapBuilderWifi::wifiLevels_
std::map< double, int > wifiLevels_
Definition: MapBuilderWifi.h:193
uContains
bool uContains(const std::list< V > &list, const V &value)
Definition: UStl.h:407
MapBuilder::processStatistics
virtual void processStatistics(const rtabmap::Statistics &stats)
Definition: LidarMapping/MapBuilder.h:201
translation
translation
stats
bool stats
MapBuilderWifi::processStatistics
virtual void processStatistics(const rtabmap::Statistics &stats)
Definition: MapBuilderWifi.h:69
UASSERT
#define UASSERT(condition)
level
level
UserDataEvent.h
rtabmap::Transform::inverse
Transform inverse() const
Definition: Transform.cpp:178
uFormat
std::string UTILITE_EXPORT uFormat(const char *fmt,...)
Definition: UConversion.cpp:365
rtabmap::Transform
Definition: Transform.h:41
iter
iterator iter(handle obj)
camera
Camera camera(Pose3(Rot3().retract(Vector3(0.1, 0.2, 0.3)), Point3(0, 5, 0)), Cal3Bundler0(1, 0, 0))
id
id
ratio
set noclip points set clip one set noclip two set bar set border lt lw set xdata set ydata set zdata set x2data set y2data set boxwidth set dummy y set format x g set format y g set format x2 g set format y2 g set format z g set angles radians set nogrid set key title set key left top Right noreverse box linetype linewidth samplen spacing width set nolabel set noarrow set nologscale set logscale x set set pointsize set encoding default set nopolar set noparametric set set set set surface set nocontour set clabel set mapping cartesian set nohidden3d set cntrparam order set cntrparam linear set cntrparam levels auto set cntrparam points set size ratio
v
Array< int, Dynamic, 1 > v
float
float
rtabmap
Definition: CameraARCore.cpp:35
UERROR
#define UERROR(...)
MapBuilder
Definition: LidarMapping/MapBuilder.h:52
MapBuilderWifi::MapBuilderWifi
MapBuilderWifi(SensorCaptureThread *camera=0)
Definition: MapBuilderWifi.h:59
i
int i

rtabmap
Author(s): Mathieu Labbe
autogenerated on Thu Jul 25 2024 02:50:12