Signature.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 #pragma once
29 
30 #include "rtabmap/core/rtabmap_core_export.h" // DLL export/import defines
31 
32 #include <pcl/point_types.h>
33 #include <opencv2/core/core.hpp>
34 #include <opencv2/features2d/features2d.hpp>
35 #include <opencv2/imgproc/imgproc.hpp>
36 #include <map>
37 #include <list>
38 #include <vector>
39 #include <set>
40 
41 #include <rtabmap/core/Transform.h>
43 #include <rtabmap/core/Link.h>
44 
45 namespace rtabmap
46 {
47 
48 class RTABMAP_CORE_EXPORT Signature
49 {
50 
51 public:
52  Signature();
53  Signature(int id,
54  int mapId = -1,
55  int weight = 0,
56  double stamp = 0.0,
57  const std::string & label = std::string(),
58  const Transform & pose = Transform(),
59  const Transform & groundTruthPose = Transform(),
60  const SensorData & sensorData = SensorData());
61  Signature(const SensorData & data);
62  virtual ~Signature();
63 
67  float compareTo(const Signature & signature) const;
68  bool isBadSignature() const;
69 
70  int id() const {return _id;}
71  int mapId() const {return _mapId;}
72 
73  void setWeight(int weight) {_modified=_weight!=weight;_weight = weight;}
74  int getWeight() const {return _weight;}
75 
76  void setLabel(const std::string & label) {_modified=_label.compare(label)!=0;_label = label;}
77  const std::string & getLabel() const {return _label;}
78 
79  double getStamp() const {return _stamp;}
80 
81  void addLinks(const std::list<Link> & links);
82  void addLinks(const std::map<int, Link> & links);
83  void addLink(const Link & link);
84 
85  bool hasLink(int idTo, Link::Type type = Link::kUndef) const;
86 
87  void changeLinkIds(int idFrom, int idTo);
88 
89  void removeLinks(bool keepSelfReferringLinks = false);
90  void removeLink(int idTo);
91  void removeVirtualLinks();
92 
93  void addLandmark(const Link & landmark);
94  const std::map<int, Link> & getLandmarks() const {return _landmarks;}
95  void removeLandmarks();
96  void removeLandmark(int landmarkId);
97 
98  void setSaved(bool saved) {_saved = saved;}
99  void setModified(bool modified) {_modified = modified; _linksModified = modified;}
100 
101  const std::multimap<int, Link> & getLinks() const {return _links;}
102  bool isSaved() const {return _saved;}
103  bool isModified() const {return _modified || _linksModified;}
104  bool isLinksModified() const {return _linksModified;}
105 
106  //visual words stuff
107  void removeAllWords();
108  void changeWordsRef(int oldWordId, int activeWordId);
109  void setWords(const std::multimap<int, int> & words, const std::vector<cv::KeyPoint> & keypoints, const std::vector<cv::Point3f> & words3, const cv::Mat & descriptors);
110  bool isEnabled() const {return _enabled;}
111  void setEnabled(bool enabled) {_enabled = enabled;}
112  const std::multimap<int, int> & getWords() const {return _words;}
113  const std::vector<cv::KeyPoint> & getWordsKpts() const {return _wordsKpts;}
114  int getInvalidWordsCount() const {return _invalidWordsCount;}
115  const std::map<int, int> & getWordsChanged() const {return _wordsChanged;}
116  const cv::Mat & getWordsDescriptors() const {return _wordsDescriptors;}
117  void setWordsDescriptors(const cv::Mat & descriptors);
118 
119  //metric stuff
120  void setPose(const Transform & pose) {_pose = pose;}
121  void setGroundTruthPose(const Transform & pose) {_groundTruthPose = pose;}
122  void setVelocity(float vx, float vy, float vz, float vroll, float vpitch, float vyaw) {
123  _velocity = std::vector<float>(6,0);
124  _velocity[0]=vx;
125  _velocity[1]=vy;
126  _velocity[2]=vz;
127  _velocity[3]=vroll;
128  _velocity[4]=vpitch;
129  _velocity[5]=vyaw;
130  }
131 
132  const std::vector<cv::Point3f> & getWords3() const {return _words3;}
133  const Transform & getPose() const {return _pose;}
134  cv::Mat getPoseCovariance() const;
135  const Transform & getGroundTruthPose() const {return _groundTruthPose;}
136  const std::vector<float> & getVelocity() const {return _velocity;}
137 
138  SensorData & sensorData() {return _sensorData;}
139  const SensorData & sensorData() const {return _sensorData;}
140 
141  unsigned long getMemoryUsed(bool withSensorData=true) const; // Return memory usage in Bytes
142 
143 private:
144  int _id;
145  int _mapId;
146  double _stamp;
147  std::multimap<int, Link> _links; // id, transform
148  std::map<int, Link> _landmarks;
149  int _weight;
150  std::string _label;
151  bool _saved; // If it's saved to bd
152  bool _modified;
153  bool _linksModified; // Optimization when updating signatures in database
154 
155  // Contains all words (Some can be duplicates -> if a word appears 2
156  // times in the signature, it will be 2 times in this list)
157  // Words match with the CvSeq keypoints and descriptors
158  std::multimap<int, int> _words; // word <id, keypoint index>
159  std::vector<cv::KeyPoint> _wordsKpts;
160  std::vector<cv::Point3f> _words3; // in base_link frame (localTransform applied))
162  std::map<int, int> _wordsChanged; // <oldId, newId>
163  bool _enabled;
165 
168  std::vector<float> _velocity;
169 
171 };
172 
173 } // namespace rtabmap
rtabmap::SensorData
Definition: SensorData.h:51
rtabmap::Signature::isEnabled
bool isEnabled() const
Definition: Signature.h:110
rtabmap::Signature::getWordsChanged
const std::map< int, int > & getWordsChanged() const
Definition: Signature.h:115
rtabmap::Signature::_label
std::string _label
Definition: Signature.h:150
rtabmap::Signature::_id
int _id
Definition: Signature.h:144
rtabmap::Signature::_words3
std::vector< cv::Point3f > _words3
Definition: Signature.h:160
rtabmap::Signature::getLinks
const std::multimap< int, Link > & getLinks() const
Definition: Signature.h:101
rtabmap::Signature::getWords
const std::multimap< int, int > & getWords() const
Definition: Signature.h:112
rtabmap::Signature::_pose
Transform _pose
Definition: Signature.h:166
rtabmap::Signature::getWordsKpts
const std::vector< cv::KeyPoint > & getWordsKpts() const
Definition: Signature.h:113
rtabmap::Signature::_velocity
std::vector< float > _velocity
Definition: Signature.h:168
SensorData.h
rtabmap::Signature::getWordsDescriptors
const cv::Mat & getWordsDescriptors() const
Definition: Signature.h:116
rtabmap::Signature::mapId
int mapId() const
Definition: Signature.h:71
rtabmap::Signature::getGroundTruthPose
const Transform & getGroundTruthPose() const
Definition: Signature.h:135
rtabmap::Signature::_saved
bool _saved
Definition: Signature.h:151
Transform.h
rtabmap::Signature::_groundTruthPose
Transform _groundTruthPose
Definition: Signature.h:167
type
rtabmap::Signature::getLabel
const std::string & getLabel() const
Definition: Signature.h:77
rtabmap::Signature::setSaved
void setSaved(bool saved)
Definition: Signature.h:98
rtabmap::Signature::_weight
int _weight
Definition: Signature.h:149
rtabmap::Signature::getWeight
int getWeight() const
Definition: Signature.h:74
rtabmap::Signature::_stamp
double _stamp
Definition: Signature.h:146
rtabmap::Signature::_sensorData
SensorData _sensorData
Definition: Signature.h:170
rtabmap::Signature::getInvalidWordsCount
int getInvalidWordsCount() const
Definition: Signature.h:114
rtabmap::Signature::_modified
bool _modified
Definition: Signature.h:152
rtabmap::Signature::setGroundTruthPose
void setGroundTruthPose(const Transform &pose)
Definition: Signature.h:121
data
int data[]
rtabmap::Signature::setEnabled
void setEnabled(bool enabled)
Definition: Signature.h:111
vy
StridedVectorType vy(make_vector(y, *n, std::abs(*incy)))
rtabmap::Signature::isLinksModified
bool isLinksModified() const
Definition: Signature.h:104
rtabmap::Signature::getLandmarks
const std::map< int, Link > & getLandmarks() const
Definition: Signature.h:94
rtabmap::Signature::setPose
void setPose(const Transform &pose)
Definition: Signature.h:120
rtabmap::Signature::getWords3
const std::vector< cv::Point3f > & getWords3() const
Definition: Signature.h:132
rtabmap::Signature::_links
std::multimap< int, Link > _links
Definition: Signature.h:147
rtabmap::Signature::_linksModified
bool _linksModified
Definition: Signature.h:153
rtabmap::Signature::_wordsDescriptors
cv::Mat _wordsDescriptors
Definition: Signature.h:161
rtabmap::Signature::setLabel
void setLabel(const std::string &label)
Definition: Signature.h:76
rtabmap::Signature::getStamp
double getStamp() const
Definition: Signature.h:79
rtabmap::Signature::_landmarks
std::map< int, Link > _landmarks
Definition: Signature.h:148
rtabmap::Signature::setVelocity
void setVelocity(float vx, float vy, float vz, float vroll, float vpitch, float vyaw)
Definition: Signature.h:122
rtabmap::Signature::_wordsKpts
std::vector< cv::KeyPoint > _wordsKpts
Definition: Signature.h:159
rtabmap::Signature::getPose
const Transform & getPose() const
Definition: Signature.h:133
rtabmap::Transform
Definition: Transform.h:41
rtabmap::Signature::setWeight
void setWeight(int weight)
Definition: Signature.h:73
rtabmap::Signature::id
int id() const
Definition: Signature.h:70
rtabmap::Signature::_words
std::multimap< int, int > _words
Definition: Signature.h:158
rtabmap::Signature::_enabled
bool _enabled
Definition: Signature.h:163
rtabmap::Signature::_mapId
int _mapId
Definition: Signature.h:145
rtabmap::Signature::sensorData
const SensorData & sensorData() const
Definition: Signature.h:139
rtabmap::Signature::isModified
bool isModified() const
Definition: Signature.h:103
rtabmap::Signature::setModified
void setModified(bool modified)
Definition: Signature.h:99
rtabmap::Signature::sensorData
SensorData & sensorData()
Definition: Signature.h:138
rtabmap
Definition: CameraARCore.cpp:35
rtabmap::Signature::getVelocity
const std::vector< float > & getVelocity() const
Definition: Signature.h:136
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
vx
StridedVectorType vx(make_vector(x, *n, std::abs(*incx)))
rtabmap::Signature::isSaved
bool isSaved() const
Definition: Signature.h:102


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