Signature.cpp
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 #include "rtabmap/core/Signature.h"
30 #include "rtabmap/core/Memory.h"
32 #include <opencv2/highgui/highgui.hpp>
33 
35 
36 namespace rtabmap
37 {
38 
40  _id(0), // invalid id
41  _mapId(-1),
42  _stamp(0.0),
43  _weight(0),
44  _saved(false),
45  _modified(true),
46  _linksModified(true),
47  _enabled(false),
48  _invalidWordsCount(0)
49 {
50 }
51 
53  int id,
54  int mapId,
55  int weight,
56  double stamp,
57  const std::string & label,
58  const Transform & pose,
59  const Transform & groundTruthPose,
60  const SensorData & sensorData):
61  _id(id),
62  _mapId(mapId),
63  _stamp(stamp),
64  _weight(weight),
65  _label(label),
66  _saved(false),
67  _modified(true),
69  _enabled(false),
71  _pose(pose),
72  _groundTruthPose(groundTruthPose),
73  _sensorData(sensorData)
74 {
75  if(_sensorData.id() == 0)
76  {
77  _sensorData.setId(id);
78  }
79  UASSERT(_sensorData.id() == _id);
80 }
81 
83  _id(data.id()),
84  _mapId(-1),
85  _stamp(data.stamp()),
86  _weight(0),
87  _label(""),
88  _saved(false),
89  _modified(true),
91  _enabled(false),
93  _pose(Transform::getIdentity()),
94  _groundTruthPose(data.groundTruth()),
95  _sensorData(data)
96 {
97 
98 }
99 
101 {
102  //UDEBUG("id=%d", _id);
103 }
104 
105 void Signature::addLinks(const std::list<Link> & links)
106 {
107  for(std::list<Link>::const_iterator iter = links.begin(); iter!=links.end(); ++iter)
108  {
109  addLink(*iter);
110  }
111 }
112 void Signature::addLinks(const std::map<int, Link> & links)
113 {
114  for(std::map<int, Link>::const_iterator iter = links.begin(); iter!=links.end(); ++iter)
115  {
116  addLink(iter->second);
117  }
118 }
119 void Signature::addLink(const Link & link)
120 {
121  UDEBUG("Add link %d to %d (type=%d/%s var=%f,%f)", link.to(), this->id(), (int)link.type(), link.typeName().c_str(), link.transVariance(), link.rotVariance());
122  UASSERT_MSG(link.from() == this->id(), uFormat("%d->%d for signature %d (type=%d)", link.from(), link.to(), this->id(), link.type()).c_str());
123  UASSERT_MSG((link.to() != this->id()) || link.type()==Link::kPosePrior || link.type()==Link::kGravity, uFormat("%d->%d for signature %d (type=%d)", link.from(), link.to(), this->id(), link.type()).c_str());
124  UASSERT_MSG(link.to() == this->id() || _links.find(link.to()) == _links.end(), uFormat("Link %d (type=%d) already added to signature %d!", link.to(), link.type(), this->id()).c_str());
125  _links.insert(std::make_pair(link.to(), link));
126  _linksModified = true;
127 }
128 
129 bool Signature::hasLink(int idTo, Link::Type type) const
130 {
131  if(type == Link::kUndef)
132  {
133  return _links.find(idTo) != _links.end();
134  }
135  if(idTo==0)
136  {
137  for(std::multimap<int, Link>::const_iterator iter=_links.begin(); iter!=_links.end(); ++iter)
138  {
139  if(type == iter->second.type())
140  {
141  return true;
142  }
143  }
144  }
145  else
146  {
147  for(std::multimap<int, Link>::const_iterator iter=_links.find(idTo); iter!=_links.end() && iter->first == idTo; ++iter)
148  {
149  if(type == iter->second.type())
150  {
151  return true;
152  }
153  }
154  }
155  return false;
156 }
157 
158 void Signature::changeLinkIds(int idFrom, int idTo)
159 {
160  std::multimap<int, Link>::iterator iter = _links.find(idFrom);
161  while(iter != _links.end() && iter->first == idFrom)
162  {
163  Link link = iter->second;
164  _links.erase(iter++);
165  link.setTo(idTo);
166  _links.insert(std::make_pair(idTo, link));
167  _linksModified = true;
168  UDEBUG("(%d) neighbor ids changed from %d to %d", _id, idFrom, idTo);
169  }
170 }
171 
172 void Signature::removeLinks(bool keepSelfReferringLinks)
173 {
174  size_t sizeBefore = _links.size();
175  if(keepSelfReferringLinks)
176  {
177  for(std::multimap<int, Link>::iterator iter = _links.begin(); iter != _links.end();)
178  {
179  if(iter->second.from() == iter->second.to())
180  {
181  ++iter;
182  }
183  else
184  {
185  _links.erase(iter++);
186  }
187  }
188  }
189  else
190  {
191  _links.clear();
192  }
193  if(_links.size() != sizeBefore)
194  _linksModified = true;
195 }
196 
197 void Signature::removeLink(int idTo)
198 {
199  int count = (int)_links.erase(idTo);
200  if(count)
201  {
202  UDEBUG("Removed link %d from %d", idTo, this->id());
203  _linksModified = true;
204  }
205 }
206 
208 {
209  for(std::multimap<int, Link>::iterator iter=_links.begin(); iter!=_links.end();)
210  {
211  if(iter->second.type() == Link::kVirtualClosure)
212  {
213  _links.erase(iter++);
214  }
215  else
216  {
217  ++iter;
218  }
219  }
220 }
221 
222 float Signature::compareTo(const Signature & s) const
223 {
224  float similarity = 0.0f;
225  const std::multimap<int, int> & words = s.getWords();
226 
227  if(!s.isBadSignature() && !this->isBadSignature())
228  {
229  std::list<std::pair<int, std::pair<int, int> > > pairs;
230  int totalWords = ((int)_words.size()-_invalidWordsCount)>((int)words.size()-s.getInvalidWordsCount())?((int)_words.size()-_invalidWordsCount):((int)words.size()-s.getInvalidWordsCount());
231  UASSERT(totalWords > 0);
232  EpipolarGeometry::findPairs(words, _words, pairs);
233 
234  similarity = float(pairs.size()) / float(totalWords);
235  }
236  return similarity;
237 }
238 
239 void Signature::changeWordsRef(int oldWordId, int activeWordId)
240 {
241  std::list<int> words = uValues(_words, oldWordId);
242  if(words.size())
243  {
244  if(oldWordId<=0)
245  {
246  _invalidWordsCount-=(int)_words.erase(oldWordId);
248  }
249  else
250  {
251  _words.erase(oldWordId);
252  }
253 
254  _wordsChanged.insert(std::make_pair(oldWordId, activeWordId));
255  for(std::list<int>::const_iterator iter=words.begin(); iter!=words.end(); ++iter)
256  {
257  _words.insert(std::pair<int, int>(activeWordId, (*iter)));
258  }
259  }
260 }
261 
262 void Signature::setWords(const std::multimap<int, int> & words,
263  const std::vector<cv::KeyPoint> & keypoints,
264  const std::vector<cv::Point3f> & points,
265  const cv::Mat & descriptors)
266 {
267  UASSERT_MSG(descriptors.empty() || descriptors.rows == (int)words.size(), uFormat("words=%d, descriptors=%d", (int)words.size(), descriptors.rows).c_str());
268  UASSERT_MSG(points.empty() || points.size() == words.size(), uFormat("words=%d, points=%d", (int)words.size(), (int)points.size()).c_str());
269  UASSERT_MSG(keypoints.empty() || keypoints.size() == words.size(), uFormat("words=%d, descriptors=%d", (int)words.size(), (int)keypoints.size()).c_str());
270  UASSERT(words.empty() || !keypoints.empty() || !points.empty() || !descriptors.empty());
271 
272  _invalidWordsCount = 0;
273  for(std::multimap<int, int>::const_iterator iter=words.begin(); iter!=words.end(); ++iter)
274  {
275  if(iter->first<=0)
276  {
278  }
279  // make sure indexes are all valid!
280  UASSERT_MSG(iter->second >=0 && iter->second < (int)words.size(), uFormat("iter->second=%d words.size()=%d", iter->second, (int)words.size()).c_str());
281  }
282 
283  _enabled = false;
284  _words = words;
285  _wordsKpts = keypoints;
286  _words3 = points;
287  _wordsDescriptors = descriptors.clone();
288 }
289 
291 {
292  return _words.size()-_invalidWordsCount <= 0;
293 }
294 
296 {
297  _words.clear();
298  _wordsKpts.clear();
299  _words3.clear();
300  _wordsDescriptors = cv::Mat();
301  _invalidWordsCount = 0;
302 }
303 
304 void Signature::setWordsDescriptors(const cv::Mat & descriptors)
305 {
306  if(descriptors.empty())
307  {
308  if(_wordsKpts.empty() && _words3.empty())
309  {
310  removeAllWords();
311  }
312  else
313  {
314  _wordsDescriptors = cv::Mat();
315  }
316  }
317  else
318  {
319  UASSERT(descriptors.rows == (int)_words.size());
320  _wordsDescriptors = descriptors.clone();
321  }
322 }
323 
325 {
326  cv::Mat covariance = cv::Mat::eye(6,6,CV_64FC1);
327  if(_links.size())
328  {
329  for(std::multimap<int, Link>::const_iterator iter = _links.begin(); iter!=_links.end(); ++iter)
330  {
331  if(iter->second.kNeighbor)
332  {
333  //Assume the first neighbor to be the backward neighbor link
334  if(iter->second.to() < iter->second.from())
335  {
336  covariance = iter->second.infMatrix().inv();
337  break;
338  }
339  }
340  }
341  }
342  return covariance;
343 }
344 
345 unsigned long Signature::getMemoryUsed(bool withSensorData) const // Return memory usage in Bytes
346 {
347  unsigned long total = sizeof(Signature);
348  total += _words.size() * (sizeof(int)*2+sizeof(std::multimap<int, cv::KeyPoint>::iterator)) + sizeof(std::multimap<int, cv::KeyPoint>);
349  total += _wordsKpts.size() * sizeof(cv::KeyPoint) + sizeof(std::vector<cv::KeyPoint>);
350  total += _words3.size() * sizeof(cv::Point3f) + sizeof(std::vector<cv::Point3f>);
351  total += _wordsDescriptors.total() * _wordsDescriptors.elemSize() + sizeof(cv::Mat);
352  total += _wordsChanged.size() * (sizeof(int)*2+sizeof(std::map<int, int>::iterator)) + sizeof(std::map<int, int>);
353  if(withSensorData)
354  {
355  total+=_sensorData.getMemoryUsed();
356  }
357  total += _pose.size() * (sizeof(Transform) + sizeof(float)*12);
358  total += _groundTruthPose.size() * (sizeof(Transform) + sizeof(float)*12);
359  total += _velocity.size() * sizeof(float);
360  total += _links.size() * (sizeof(int) + sizeof(Transform) + 12 * sizeof(float) + sizeof(cv::Mat) + 36 * sizeof(double)+sizeof(std::multimap<int, Link>::iterator)) + sizeof(std::multimap<int, Link>);
361  total += _landmarks.size() * (sizeof(int) + sizeof(Transform) + 12 * sizeof(float) + sizeof(cv::Mat) + 36 * sizeof(double)+sizeof(std::map<int, Link>::iterator)) + sizeof(std::map<int, Link>);
362  return total;
363 }
364 
365 } //namespace rtabmap
void changeWordsRef(int oldWordId, int activeWordId)
Definition: Signature.cpp:239
int getInvalidWordsCount() const
Definition: Signature.h:113
virtual ~Signature()
Definition: Signature.cpp:100
cv::Mat _wordsDescriptors
Definition: Signature.h:160
int mapId() const
Definition: Signature.h:71
void removeVirtualLinks()
Definition: Signature.cpp:207
std::string _label
Definition: Signature.h:149
std::map< int, Link > _landmarks
Definition: Signature.h:147
void setId(int id)
Definition: SensorData.h:175
float compareTo(const Signature &signature) const
Definition: Signature.cpp:222
SensorData _sensorData
Definition: Signature.h:169
std::vector< float > _velocity
Definition: Signature.h:167
bool isBadSignature() const
Definition: Signature.cpp:290
#define UASSERT(condition)
std::vector< cv::Point3f > _words3
Definition: Signature.h:159
int id() const
Definition: SensorData.h:174
Transform _groundTruthPose
Definition: Signature.h:166
std::vector< cv::KeyPoint > _wordsKpts
Definition: Signature.h:158
#define true
Definition: ConvertUTF.c:57
void removeLinks(bool keepSelfReferringLinks=false)
Definition: Signature.cpp:172
#define UASSERT_MSG(condition, msg_str)
Definition: ULogger.h:67
std::multimap< int, int > _words
Definition: Signature.h:157
static int findPairs(const std::map< int, T > &wordsA, const std::map< int, T > &wordsB, std::list< std::pair< int, std::pair< T, T > > > &pairs, bool ignoreNegativeIds=true)
bool hasLink(int idTo, Link::Type type=Link::kUndef) const
Definition: Signature.cpp:129
void setWordsDescriptors(const cv::Mat &descriptors)
Definition: Signature.cpp:304
unsigned long getMemoryUsed(bool withSensorData=true) const
Definition: Signature.cpp:345
std::multimap< int, Link > _links
Definition: Signature.h:146
unsigned long getMemoryUsed() const
Definition: SensorData.cpp:810
#define false
Definition: ConvertUTF.c:56
cv::Mat getPoseCovariance() const
Definition: Signature.cpp:324
void addLinks(const std::list< Link > &links)
Definition: Signature.cpp:105
std::vector< V > uValues(const std::multimap< K, V > &mm)
Definition: UStl.h:100
#define UDEBUG(...)
SensorData & sensorData()
Definition: Signature.h:137
void setWords(const std::multimap< int, int > &words, const std::vector< cv::KeyPoint > &keypoints, const std::vector< cv::Point3f > &words3, const cv::Mat &descriptors)
Definition: Signature.cpp:262
int id() const
Definition: Signature.h:70
void addLink(const Link &link)
Definition: Signature.cpp:119
int size() const
Definition: Transform.h:90
void removeLink(int idTo)
Definition: Signature.cpp:197
Transform _pose
Definition: Signature.h:165
void changeLinkIds(int idFrom, int idTo)
Definition: Signature.cpp:158
std::string UTILITE_EXP uFormat(const char *fmt,...)
std::map< int, int > _wordsChanged
Definition: Signature.h:161
const std::multimap< int, int > & getWords() const
Definition: Signature.h:111


rtabmap
Author(s): Mathieu Labbe
autogenerated on Mon Jan 23 2023 03:37:30