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 (
int 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>;
void detectKeypoints(PointCloudOut &output)
Abstract key point detection method.
uint32_t width
The point cloud width (if organized as an image-structure).
TrajkovicKeypoint2D implements Trajkovic and Hedley corner detector on organized pooint cloud using i...
void push_back(const PointT &pt)
Insert a new point in the cloud, at the end of the container.
PointCloud represents the base class in PCL for storing collections of 3D points. ...
void clear()
Removes all points in a cloud and sets the width and height to 0.
bool is_dense
True if no points are invalid (e.g., have NaN or Inf values).
uint32_t height
The point cloud height (if organized as an image-structure).