38 #ifndef PCL_TRAJKOVIC_KEYPOINT_2D_IMPL_H_
39 #define PCL_TRAJKOVIC_KEYPOINT_2D_IMPL_H_
41 template <
typename Po
intInT,
typename Po
intOutT,
typename IntensityT>
bool
48 keypoints_indices_->indices.reserve (input_->size ());
50 if (!input_->isOrganized ())
52 PCL_ERROR (
"[pcl::%s::initCompute] %s doesn't support non organized clouds!\n", name_.c_str ());
56 if (indices_->size () != input_->size ())
58 PCL_ERROR (
"[pcl::%s::initCompute] %s doesn't support setting indices!\n", name_.c_str ());
62 if ((window_size_%2) == 0)
64 PCL_ERROR (
"[pcl::%s::initCompute] Window size must be odd!\n", name_.c_str ());
70 PCL_ERROR (
"[pcl::%s::initCompute] Window size must be >= 3x3!\n", name_.c_str ());
74 half_window_size_ = window_size_ / 2;
80 template <
typename Po
intInT,
typename Po
intOutT,
typename IntensityT>
void
84 int w =
static_cast<int> (input_->width) - half_window_size_;
85 int h =
static_cast<int> (input_->height) - half_window_size_;
90 #pragma omp parallel for num_threads (threads_)
92 for(
int j = half_window_size_; j < h; ++j)
94 for(
int i = half_window_size_; i < w; ++i)
96 float center = intensity_ ((*input_) (i,j));
97 float up = intensity_ ((*input_) (i, j-half_window_size_));
98 float down = intensity_ ((*input_) (i, j+half_window_size_));
99 float left = intensity_ ((*input_) (i-half_window_size_, j));
100 float right = intensity_ ((*input_) (i+half_window_size_, j));
102 float up_center = up - center;
103 float r1 = up_center * up_center;
104 float down_center = down - center;
105 r1+= down_center * down_center;
107 float right_center = right - center;
108 float r2 = right_center * right_center;
109 float left_center = left - center;
110 r2+= left_center * left_center;
112 float d = std::min (r1, r2);
114 if (d < first_threshold_)
117 float b1 = (right - up) * up_center;
118 b1+= (left - down) * down_center;
119 float b2 = (right - down) * down_center;
120 b2+= (left - up) * up_center;
121 float B = std::min (b1, b2);
122 float A = r2 - r1 - 2*
B;
124 (*response_) (i,j) = ((
B < 0) && ((
B + A) > 0)) ? r1 - ((
B*
B)/A) : d;
131 #pragma omp parallel for num_threads (threads_)
133 for(
int j = half_window_size_; j < h; ++j)
135 for(
int i = half_window_size_; i < w; ++i)
137 float center = intensity_ ((*input_) (i,j));
138 float up = intensity_ ((*input_) (i, j-half_window_size_));
139 float down = intensity_ ((*input_) (i, j+half_window_size_));
140 float left = intensity_ ((*input_) (i-half_window_size_, j));
141 float right = intensity_ ((*input_) (i+half_window_size_, j));
142 float upleft = intensity_ ((*input_) (i-half_window_size_, j-half_window_size_));
143 float upright = intensity_ ((*input_) (i+half_window_size_, j-half_window_size_));
144 float downleft = intensity_ ((*input_) (i-half_window_size_, j+half_window_size_));
145 float downright = intensity_ ((*input_) (i+half_window_size_, j+half_window_size_));
146 std::vector<float> r (4,0);
148 float up_center = up - center;
149 r[0] = up_center * up_center;
150 float down_center = down - center;
151 r[0]+= down_center * down_center;
153 float upright_center = upright - center;
154 r[1] = upright_center * upright_center;
155 float downleft_center = downleft - center;
156 r[1]+= downleft_center * downleft_center;
158 float right_center = right - center;
159 r[2] = right_center * right_center;
160 float left_center = left - center;
161 r[2]+= left_center * left_center;
163 float downright_center = downright - center;
164 r[3] = downright_center * downright_center;
165 float upleft_center = upleft - center;
166 r[3]+= upleft_center * upleft_center;
168 float d = *(std::min_element (r.begin (), r.end ()));
170 if (d < first_threshold_)
173 std::vector<float>
B (4,0);
174 std::vector<float> A (4,0);
175 std::vector<float> sumAB (4,0);
176 B[0] = (upright - up) * up_center;
177 B[0]+= (downleft - down) * down_center;
178 B[1] = (right - upright) * upright_center;
179 B[1]+= (left - downleft) * downleft_center;
180 B[2] = (downright - right) * downright_center;
181 B[2]+= (upleft - left) * upleft_center;
182 B[3] = (down - downright) * downright_center;
183 B[3]+= (up - upleft) * upleft_center;
184 A[0] = r[1] - r[0] -
B[0] -
B[0];
185 A[1] = r[2] - r[1] -
B[1] -
B[1];
186 A[2] = r[3] - r[2] -
B[2] -
B[2];
187 A[3] = r[0] - r[3] -
B[3] -
B[3];
188 sumAB[0] = A[0] +
B[0];
189 sumAB[1] = A[1] +
B[1];
190 sumAB[2] = A[2] +
B[2];
191 sumAB[3] = A[3] +
B[3];
192 if ((*std::max_element (
B.begin (),
B.end ()) < 0) &&
193 (*std::min_element (sumAB.begin (), sumAB.end ()) > 0))
195 std::vector<float> D (4,0);
196 D[0] =
B[0] *
B[0] / A[0];
197 D[1] =
B[1] *
B[1] / A[1];
198 D[2] =
B[2] *
B[2] / A[2];
199 D[3] =
B[3] *
B[3] / A[3];
200 (*response_) (i,j) = *(std::min (D.begin (), D.end ()));
203 (*response_) (i,j) = d;
209 std::vector<int> indices = *indices_;
210 std::sort (indices.begin (), indices.end (),
211 boost::bind (&TrajkovicKeypoint2D::greaterCornernessAtIndices,
this, _1, _2));
214 output.
reserve (input_->size ());
216 std::vector<bool> occupency_map (indices.size (),
false);
217 const int width (input_->width);
218 const int height (input_->height);
221 #pragma omp parallel for shared (output) num_threads (threads_)
223 for (
size_t i = 0; i < indices.size (); ++i)
225 int idx = indices[i];
226 if ((response_->points[idx] < second_threshold_) || occupency_map[idx])
230 p.getVector3fMap () = input_->points[idx].getVector3fMap ();
231 p.intensity = response_->points [idx];
238 keypoints_indices_->indices.push_back (idx);
241 const int x = idx % width;
242 const int y = idx / width;
243 const int u_end = std::min (width, x + half_window_size_);
244 const int v_end = std::min (height, y + half_window_size_);
245 for(
int v = std::max (0, y - half_window_size_); v < v_end; ++v)
246 for(
int u = std::max (0, x - half_window_size_); u < u_end; ++u)
247 occupency_map[v*width + u] =
true;
251 output.
width =
static_cast<uint32_t
> (output.
size());
256 #define PCL_INSTANTIATE_TrajkovicKeypoint2D(T,U,I) template class PCL_EXPORTS pcl::TrajkovicKeypoint2D<T,U,I>;