FORB.cpp
Go to the documentation of this file.
1 
13 #include <vector>
14 #include <string>
15 #include <sstream>
16 #include <stdint-gcc.h>
17 
18 #include "FORB.h"
19 
20 using namespace std;
21 
22 namespace DBoW2 {
23 
24 // --------------------------------------------------------------------------
25 
26 const int FORB::L=32;
27 
28 void FORB::meanValue(const std::vector<FORB::pDescriptor> &descriptors,
29  FORB::TDescriptor &mean)
30 {
31  if(descriptors.empty())
32  {
33  mean.release();
34  return;
35  }
36  else if(descriptors.size() == 1)
37  {
38  mean = descriptors[0]->clone();
39  }
40  else
41  {
42  vector<int> sum(FORB::L * 8, 0);
43 
44  for(size_t i = 0; i < descriptors.size(); ++i)
45  {
46  const cv::Mat &d = *descriptors[i];
47  const unsigned char *p = d.ptr<unsigned char>();
48 
49  for(int j = 0; j < d.cols; ++j, ++p)
50  {
51  if(*p & (1 << 7)) ++sum[ j*8 ];
52  if(*p & (1 << 6)) ++sum[ j*8 + 1 ];
53  if(*p & (1 << 5)) ++sum[ j*8 + 2 ];
54  if(*p & (1 << 4)) ++sum[ j*8 + 3 ];
55  if(*p & (1 << 3)) ++sum[ j*8 + 4 ];
56  if(*p & (1 << 2)) ++sum[ j*8 + 5 ];
57  if(*p & (1 << 1)) ++sum[ j*8 + 6 ];
58  if(*p & (1)) ++sum[ j*8 + 7 ];
59  }
60  }
61 
62  mean = cv::Mat::zeros(1, FORB::L, CV_8U);
63  unsigned char *p = mean.ptr<unsigned char>();
64 
65  const int N2 = (int)descriptors.size() / 2 + descriptors.size() % 2;
66  for(size_t i = 0; i < sum.size(); ++i)
67  {
68  if(sum[i] >= N2)
69  {
70  // set bit
71  *p |= 1 << (7 - (i % 8));
72  }
73 
74  if(i % 8 == 7) ++p;
75  }
76  }
77 }
78 
79 // --------------------------------------------------------------------------
80 
81 int FORB::distance(const FORB::TDescriptor &a,
82  const FORB::TDescriptor &b)
83 {
84  // Bit set count operation from
85  // http://graphics.stanford.edu/~seander/bithacks.html#CountBitsSetParallel
86 
87  const int *pa = a.ptr<int32_t>();
88  const int *pb = b.ptr<int32_t>();
89 
90  int dist=0;
91 
92  for(int i=0; i<8; i++, pa++, pb++)
93  {
94  unsigned int v = *pa ^ *pb;
95  v = v - ((v >> 1) & 0x55555555);
96  v = (v & 0x33333333) + ((v >> 2) & 0x33333333);
97  dist += (((v + (v >> 4)) & 0xF0F0F0F) * 0x1010101) >> 24;
98  }
99 
100  return dist;
101 }
102 
103 // --------------------------------------------------------------------------
104 
105 std::string FORB::toString(const FORB::TDescriptor &a)
106 {
107  stringstream ss;
108  const unsigned char *p = a.ptr<unsigned char>();
109 
110  for(int i = 0; i < a.cols; ++i, ++p)
111  {
112  ss << (int)*p << " ";
113  }
114 
115  return ss.str();
116 }
117 
118 // --------------------------------------------------------------------------
119 
120 void FORB::fromString(FORB::TDescriptor &a, const std::string &s)
121 {
122  a.create(1, FORB::L, CV_8U);
123  unsigned char *p = a.ptr<unsigned char>();
124 
125  stringstream ss(s);
126  for(int i = 0; i < FORB::L; ++i, ++p)
127  {
128  int n;
129  ss >> n;
130 
131  if(!ss.fail())
132  *p = (unsigned char)n;
133  }
134 
135 }
136 
137 // --------------------------------------------------------------------------
138 
139 void FORB::toMat32F(const std::vector<TDescriptor> &descriptors,
140  cv::Mat &mat)
141 {
142  if(descriptors.empty())
143  {
144  mat.release();
145  return;
146  }
147 
148  const size_t N = descriptors.size();
149 
150  mat.create(N, FORB::L*8, CV_32F);
151  float *p = mat.ptr<float>();
152 
153  for(size_t i = 0; i < N; ++i)
154  {
155  const int C = descriptors[i].cols;
156  const unsigned char *desc = descriptors[i].ptr<unsigned char>();
157 
158  for(int j = 0; j < C; ++j, p += 8)
159  {
160  p[0] = (desc[j] & (1 << 7) ? 1 : 0);
161  p[1] = (desc[j] & (1 << 6) ? 1 : 0);
162  p[2] = (desc[j] & (1 << 5) ? 1 : 0);
163  p[3] = (desc[j] & (1 << 4) ? 1 : 0);
164  p[4] = (desc[j] & (1 << 3) ? 1 : 0);
165  p[5] = (desc[j] & (1 << 2) ? 1 : 0);
166  p[6] = (desc[j] & (1 << 1) ? 1 : 0);
167  p[7] = desc[j] & (1);
168  }
169  }
170 }
171 
172 // --------------------------------------------------------------------------
173 
174 void FORB::toMat8U(const std::vector<TDescriptor> &descriptors,
175  cv::Mat &mat)
176 {
177  mat.create(descriptors.size(), 32, CV_8U);
178 
179  unsigned char *p = mat.ptr<unsigned char>();
180 
181  for(size_t i = 0; i < descriptors.size(); ++i, p += 32)
182  {
183  const unsigned char *d = descriptors[i].ptr<unsigned char>();
184  std::copy(d, d+32, p);
185  }
186 
187 }
188 
189 // --------------------------------------------------------------------------
190 
191 } // namespace DBoW2
192 
193 
d
cv::Mat TDescriptor
Descriptor type.
Definition: FORB.h:27


orb_slam2_with_maps_odom
Author(s): teng zhang
autogenerated on Fri Sep 25 2020 03:24:47