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),
68  _linksModified(true),
69  _enabled(false),
70  _invalidWordsCount(0),
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),
90  _linksModified(true),
91  _enabled(false),
92  _invalidWordsCount(0),
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 void Signature::addLandmark(const Link & landmark)
223 {
224  UDEBUG("Add landmark %d to %d (type=%d/%s var=%f,%f)", landmark.to(), this->id(), (int)landmark.type(), landmark.typeName().c_str(), landmark.transVariance(), landmark.rotVariance());
225  UASSERT_MSG(landmark.from() == this->id(), uFormat("%d->%d for signature %d (type=%d)", landmark.from(), landmark.to(), this->id(), landmark.type()).c_str());
226  UASSERT_MSG(landmark.to() < 0, uFormat("%d->%d for signature %d (type=%d)", landmark.from(), landmark.to(), this->id(), landmark.type()).c_str());
227  UASSERT_MSG(_landmarks.find(landmark.to()) == _landmarks.end(), uFormat("Landmark %d (type=%d) already added to signature %d!", landmark.to(), landmark.type(), this->id()).c_str());
228  _landmarks.insert(std::make_pair(landmark.to(), landmark));
229  _linksModified = true;
230 }
231 
233 {
234  size_t sizeBefore = _landmarks.size();
235  _landmarks.clear();
236  if(_landmarks.size() != sizeBefore)
237  _linksModified = true;
238 }
239 
240 void Signature::removeLandmark(int landmarkId)
241 {
242  int count = (int)_landmarks.erase(landmarkId);
243  if(count)
244  {
245  UDEBUG("Removed landmark %d from %d", landmarkId, this->id());
246  _linksModified = true;
247  }
248 }
249 
250 float Signature::compareTo(const Signature & s) const
251 {
252  UASSERT(this->sensorData().globalDescriptors().size() == s.sensorData().globalDescriptors().size());
253 
254  float similarity = 0.0f;
255  int totalDescs = 0;
256 
257  for(size_t i=0; i<this->sensorData().globalDescriptors().size(); ++i)
258  {
259  if(this->sensorData().globalDescriptors()[i].type()==1 && s.sensorData().globalDescriptors()[i].type()==1)
260  {
261  // rescale dot product from -1<->1 to 0<->1 (we assume normalized vectors!)
262  float dotProd = (this->sensorData().globalDescriptors()[i].data().dot(s.sensorData().globalDescriptors()[i].data()) + 1.0f) / 2.0f;
263  UASSERT_MSG(dotProd>=0, "Global descriptors should be normalized!");
264  similarity += dotProd;
265  totalDescs += 1;
266  }
267  }
268 
269  if(totalDescs)
270  {
271  similarity /= totalDescs;
272  }
273  else
274  {
275  const std::multimap<int, int> & words = s.getWords();
276 
277  if(!s.isBadSignature() && !this->isBadSignature())
278  {
279  std::list<std::pair<int, std::pair<int, int> > > pairs;
280  int totalWords = ((int)_words.size()-_invalidWordsCount)>((int)words.size()-s.getInvalidWordsCount())?((int)_words.size()-_invalidWordsCount):((int)words.size()-s.getInvalidWordsCount());
281  UASSERT(totalWords > 0);
282  EpipolarGeometry::findPairs(words, _words, pairs);
283 
284  similarity = float(pairs.size()) / float(totalWords);
285  }
286  }
287  return similarity;
288 }
289 
290 void Signature::changeWordsRef(int oldWordId, int activeWordId)
291 {
292  std::list<int> words = uValues(_words, oldWordId);
293  if(words.size())
294  {
295  if(oldWordId<=0)
296  {
297  _invalidWordsCount-=(int)_words.erase(oldWordId);
299  }
300  else
301  {
302  _words.erase(oldWordId);
303  }
304 
305  _wordsChanged.insert(std::make_pair(oldWordId, activeWordId));
306  for(std::list<int>::const_iterator iter=words.begin(); iter!=words.end(); ++iter)
307  {
308  _words.insert(std::pair<int, int>(activeWordId, (*iter)));
309  }
310  }
311 }
312 
313 void Signature::setWords(const std::multimap<int, int> & words,
314  const std::vector<cv::KeyPoint> & keypoints,
315  const std::vector<cv::Point3f> & points,
316  const cv::Mat & descriptors)
317 {
318  UASSERT_MSG(descriptors.empty() || descriptors.rows == (int)words.size(), uFormat("words=%d, descriptors=%d", (int)words.size(), descriptors.rows).c_str());
319  UASSERT_MSG(points.empty() || points.size() == words.size(), uFormat("words=%d, points=%d", (int)words.size(), (int)points.size()).c_str());
320  UASSERT_MSG(keypoints.empty() || keypoints.size() == words.size(), uFormat("words=%d, descriptors=%d", (int)words.size(), (int)keypoints.size()).c_str());
321  UASSERT(words.empty() || !keypoints.empty() || !points.empty() || !descriptors.empty());
322 
323  _invalidWordsCount = 0;
324  for(std::multimap<int, int>::const_iterator iter=words.begin(); iter!=words.end(); ++iter)
325  {
326  if(iter->first<=0)
327  {
329  }
330  // make sure indexes are all valid!
331  UASSERT_MSG(iter->second >=0 && iter->second < (int)words.size(), uFormat("iter->second=%d words.size()=%d", iter->second, (int)words.size()).c_str());
332  }
333 
334  _enabled = false;
335  _words = words;
336  _wordsKpts = keypoints;
337  _words3 = points;
338  _wordsDescriptors = descriptors.clone();
339 }
340 
342 {
343  return _words.size()-_invalidWordsCount <= 0;
344 }
345 
347 {
348  _words.clear();
349  _wordsKpts.clear();
350  _words3.clear();
351  _wordsDescriptors = cv::Mat();
352  _invalidWordsCount = 0;
353 }
354 
355 void Signature::setWordsDescriptors(const cv::Mat & descriptors)
356 {
357  if(descriptors.empty())
358  {
359  if(_wordsKpts.empty() && _words3.empty())
360  {
361  removeAllWords();
362  }
363  else
364  {
365  _wordsDescriptors = cv::Mat();
366  }
367  }
368  else
369  {
370  UASSERT(descriptors.rows == (int)_words.size());
371  _wordsDescriptors = descriptors.clone();
372  }
373 }
374 
376 {
377  cv::Mat covariance = cv::Mat::eye(6,6,CV_64FC1);
378  if(_links.size())
379  {
380  for(std::multimap<int, Link>::const_iterator iter = _links.begin(); iter!=_links.end(); ++iter)
381  {
382  if(iter->second.kNeighbor)
383  {
384  //Assume the first neighbor to be the backward neighbor link
385  if(iter->second.to() < iter->second.from())
386  {
387  covariance = iter->second.infMatrix().inv();
388  break;
389  }
390  }
391  }
392  }
393  return covariance;
394 }
395 
396 unsigned long Signature::getMemoryUsed(bool withSensorData) const // Return memory usage in Bytes
397 {
398  unsigned long total = sizeof(Signature);
399  total += _words.size() * (sizeof(int)*2+sizeof(std::multimap<int, cv::KeyPoint>::iterator)) + sizeof(std::multimap<int, cv::KeyPoint>);
400  total += _wordsKpts.size() * sizeof(cv::KeyPoint) + sizeof(std::vector<cv::KeyPoint>);
401  total += _words3.size() * sizeof(cv::Point3f) + sizeof(std::vector<cv::Point3f>);
402  total += _wordsDescriptors.empty()?0:_wordsDescriptors.total() * _wordsDescriptors.elemSize() + sizeof(cv::Mat);
403  total += _wordsChanged.size() * (sizeof(int)*2+sizeof(std::map<int, int>::iterator)) + sizeof(std::map<int, int>);
404  if(withSensorData)
405  {
406  total+=_sensorData.getMemoryUsed();
407  }
408  total += _pose.size() * (sizeof(Transform) + sizeof(float)*12);
409  total += _groundTruthPose.size() * (sizeof(Transform) + sizeof(float)*12);
410  total += _velocity.size() * sizeof(float);
411  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>);
412  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>);
413  return total;
414 }
415 
416 } //namespace rtabmap
rtabmap::SensorData
Definition: SensorData.h:51
int
int
UtiLite.h
Compression.h
rtabmap::Signature::changeLinkIds
void changeLinkIds(int idFrom, int idTo)
Definition: Signature.cpp:158
rtabmap::Signature::_id
int _id
Definition: Signature.h:144
rtabmap::Signature::_words3
std::vector< cv::Point3f > _words3
Definition: Signature.h:160
rtabmap::EpipolarGeometry::findPairs
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)
Definition: EpipolarGeometry.h:96
rtabmap::Signature::removeVirtualLinks
void removeVirtualLinks()
Definition: Signature.cpp:207
rtabmap::Signature::removeLandmark
void removeLandmark(int landmarkId)
Definition: Signature.cpp:240
s
RealScalar s
size
Index size
this
this
count
Index count
type
true
#define true
Definition: ConvertUTF.c:57
rtabmap::Signature::removeLinks
void removeLinks(bool keepSelfReferringLinks=false)
Definition: Signature.cpp:172
EpipolarGeometry.h
rtabmap::Signature::hasLink
bool hasLink(int idTo, Link::Type type=Link::kUndef) const
Definition: Signature.cpp:129
rtabmap::Signature::_sensorData
SensorData _sensorData
Definition: Signature.h:170
rtabmap::SensorData::globalDescriptors
const std::vector< GlobalDescriptor > & globalDescriptors() const
Definition: SensorData.h:281
rtabmap::Signature::compareTo
float compareTo(const Signature &signature) const
Definition: Signature.cpp:250
rtabmap::Signature::getMemoryUsed
unsigned long getMemoryUsed(bool withSensorData=true) const
Definition: Signature.cpp:396
rtabmap::Signature::Signature
Signature()
Definition: Signature.cpp:39
data
int data[]
rtabmap::SensorData::id
int id() const
Definition: SensorData.h:174
Signature.h
rtabmap::Signature::isBadSignature
bool isBadSignature() const
Definition: Signature.cpp:341
rtabmap::Signature::_links
std::multimap< int, Link > _links
Definition: Signature.h:147
rtabmap::Signature::_linksModified
bool _linksModified
Definition: Signature.h:153
rtabmap::Signature::removeLink
void removeLink(int idTo)
Definition: Signature.cpp:197
rtabmap::SensorData::setId
void setId(int id)
Definition: SensorData.h:175
UASSERT
#define UASSERT(condition)
rtabmap::Signature::_wordsDescriptors
cv::Mat _wordsDescriptors
Definition: Signature.h:161
rtabmap::Signature::addLinks
void addLinks(const std::list< Link > &links)
Definition: Signature.cpp:105
rtabmap::Signature::removeLandmarks
void removeLandmarks()
Definition: Signature.cpp:232
rtabmap::Signature::_landmarks
std::map< int, Link > _landmarks
Definition: Signature.h:148
rtabmap::Signature::addLandmark
void addLandmark(const Link &landmark)
Definition: Signature.cpp:222
UASSERT_MSG
#define UASSERT_MSG(condition, msg_str)
Definition: ULogger.h:67
f
Point2(* f)(const Point3 &, OptionalJacobian< 2, 3 >)
uFormat
std::string UTILITE_EXPORT uFormat(const char *fmt,...)
Definition: UConversion.cpp:365
rtabmap::Signature::_wordsKpts
std::vector< cv::KeyPoint > _wordsKpts
Definition: Signature.h:159
rtabmap::Transform
Definition: Transform.h:41
Memory.h
rtabmap::Signature::setWordsDescriptors
void setWordsDescriptors(const cv::Mat &descriptors)
Definition: Signature.cpp:355
iter
iterator iter(handle obj)
id
id
rtabmap::Signature::_words
std::multimap< int, int > _words
Definition: Signature.h:158
UDEBUG
#define UDEBUG(...)
rtabmap::Signature::_enabled
bool _enabled
Definition: Signature.h:163
rtabmap::Signature::changeWordsRef
void changeWordsRef(int oldWordId, int activeWordId)
Definition: Signature.cpp:290
rtabmap::Signature::~Signature
virtual ~Signature()
Definition: Signature.cpp:100
float
float
false
#define false
Definition: ConvertUTF.c:56
uValues
std::vector< V > uValues(const std::multimap< K, V > &mm)
Definition: UStl.h:100
rtabmap::Signature::sensorData
SensorData & sensorData()
Definition: Signature.h:138
rtabmap::Signature::getPoseCovariance
cv::Mat getPoseCovariance() const
Definition: Signature.cpp:375
rtabmap
Definition: CameraARCore.cpp:35
rtabmap::Signature::removeAllWords
void removeAllWords()
Definition: Signature.cpp:346
i
int i
rtabmap::Signature::addLink
void addLink(const Link &link)
Definition: Signature.cpp:119
rtabmap::Signature::_invalidWordsCount
int _invalidWordsCount
Definition: Signature.h:164
rtabmap::Signature::_wordsChanged
std::map< int, int > _wordsChanged
Definition: Signature.h:162
rtabmap::Signature
Definition: Signature.h:48
rtabmap::Signature::setWords
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:313


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