101 const Normals &normals = *normals_;
104 const int w =
static_cast<int> (input_->width) - half_window_size_;
105 const int h =
static_cast<int> (input_->height) - half_window_size_;
107 if (method_ == FOUR_CORNERS)
109#if OPENMP_LEGACY_CONST_DATA_SHARING_RULE
110#pragma omp parallel for \
112 shared(input, normals, response) \
113 num_threads(threads_)
115#pragma omp parallel for \
117 shared(h, input, normals, response, w) \
118 num_threads(threads_)
120 for(
int j = half_window_size_; j < h; ++j)
122 for(
int i = half_window_size_; i < w; ++i)
129 const NormalT &
up = getNormalOrNull (i, j-half_window_size_, count);
130 const NormalT &
down = getNormalOrNull (i, j+half_window_size_, count);
131 const NormalT &left = getNormalOrNull (i-half_window_size_, j, count);
132 const NormalT &right = getNormalOrNull (i+half_window_size_, j, count);
134 if (!count)
continue;
139 float r2 = squaredNormalsDiff (right,
center) + squaredNormalsDiff (left,
center);
141 float d = std::min (
r1,
r2);
142 if (d < first_threshold_)
continue;
146 float b1 = normalsDiff (right,
up) *
sn1;
148 float b2 = normalsDiff (right,
down) *
sn2;
149 b2+= normalsDiff (left,
up) *
sn1;
150 float B = std::min (
b1,
b2);
153 response (i,j) = ((
B < 0) && ((
B +
A) > 0)) ?
r1 - ((
B*
B)/
A) : d;
159#if OPENMP_LEGACY_CONST_DATA_SHARING_RULE
160#pragma omp parallel for \
162 shared(input, normals, response) \
163 num_threads(threads_)
165#pragma omp parallel for \
167 shared(h, input, normals, response, w) \
168 num_threads(threads_)
170 for(
int j = half_window_size_; j < h; ++j)
172 for(
int i = half_window_size_; i < w; ++i)
179 const NormalT &
up = getNormalOrNull (i, j-half_window_size_, count);
180 const NormalT &
down = getNormalOrNull (i, j+half_window_size_, count);
181 const NormalT &left = getNormalOrNull (i-half_window_size_, j, count);
182 const NormalT &right = getNormalOrNull (i+half_window_size_, j, count);
183 const NormalT &
upleft = getNormalOrNull (i-half_window_size_, j-half_window_size_, count);
184 const NormalT &
upright = getNormalOrNull (i+half_window_size_, j-half_window_size_, count);
185 const NormalT &
downleft = getNormalOrNull (i-half_window_size_, j+half_window_size_, count);
186 const NormalT &
downright = getNormalOrNull (i+half_window_size_, j+half_window_size_, count);
188 if (!count)
continue;
190 std::vector<float> r (4,0);
192 r[0] = squaredNormalsDiff (
up,
center);
198 r[2] = squaredNormalsDiff (right,
center);
199 r[2]+= squaredNormalsDiff (left,
center);
204 float d = *(std::min_element (r.begin (), r.end ()));
206 if (d < first_threshold_)
continue;
208 std::vector<float>
B (4,0);
209 std::vector<float>
A (4,0);
210 std::vector<float>
sumAB (4,0);
219 A[0] = r[1] - r[0] -
B[0] -
B[0];
220 A[1] = r[2] - r[1] -
B[1] -
B[1];
221 A[2] = r[3] - r[2] -
B[2] -
B[2];
222 A[3] = r[0] - r[3] -
B[3] -
B[3];
227 if ((*std::max_element (
B.begin (),
B.end ()) < 0) &&
228 (*std::min_element (
sumAB.begin (),
sumAB.end ()) > 0))
230 std::vector<float>
D (4,0);
231 D[0] =
B[0] *
B[0] /
A[0];
232 D[1] =
B[1] *
B[1] /
A[1];
233 D[2] =
B[2] *
B[2] /
A[2];
234 D[3] =
B[3] *
B[3] /
A[3];
235 response (i,j) = *(std::min (
D.begin (),
D.end ()));
244 std::sort (indices.begin (), indices.end (), [
this] (
int p1,
int p2) { return greaterCornernessAtIndices (p1, p2); });
247 output.reserve (input_->size ());
250 const int width (input_->width);
251 const int height (input_->height);
253#if OPENMP_LEGACY_CONST_DATA_SHARING_RULE
254#pragma omp parallel for \
256 shared(indices, occupency_map, output) \
257 num_threads(threads_)
259#pragma omp parallel for \
261 shared(height, indices, occupency_map, output, width) \
262 num_threads(threads_)
266 int idx = indices[
static_cast<std::size_t
>(i)];
267 if (((*response_)[idx] < second_threshold_) ||
occupency_map[idx])
271 p.getVector3fMap () = (*input_)[idx].getVector3fMap ();
272 p.intensity = response_->points [idx];
277 keypoints_indices_->indices.push_back (idx);
280 const int x = idx % width;
281 const int y = idx / width;
282 const int u_end = std::min (width, x + half_window_size_);
283 const int v_end = std::min (height, y + half_window_size_);
284 for(
int v = std::max (0, y - half_window_size_); v < v_end; ++v)
285 for(
int u = std::max (0, x - half_window_size_); u <
u_end; ++u)