template <typename PointInT, typename PointOutT, typename NormalT> void
pcl::TrajkovicKeypoint3D<PointInT, PointOutT, NormalT>::detectKeypoints (PointCloudOut &output)
{
response_.reset (new pcl::PointCloud<float> (input_->width, input_->height));
const Normals &normals = *normals_;
const PointCloudIn &input = *input_;
pcl::PointCloud<float>& response = *response_;
const int w = static_cast<int> (input_->width) - half_window_size_;
const int h = static_cast<int> (input_->height) - half_window_size_;
if (method_ == FOUR_CORNERS)
{
#ifdef _OPENMP
#pragma omp parallel for num_threads (threads_)
#endif
for(int j = half_window_size_; j < h; ++j)
{
for(int i = half_window_size_; i < w; ++i)
{
if (!isFinite (input (i,j))) continue;
const NormalT ¢er = normals (i,j);
if (!isFinite (center)) continue;
int count = 0;
const NormalT &up = getNormalOrNull (i, j-half_window_size_, count);
const NormalT &down = getNormalOrNull (i, j+half_window_size_, count);
const NormalT &left = getNormalOrNull (i-half_window_size_, j, count);
const NormalT &right = getNormalOrNull (i+half_window_size_, j, count);
// Get rid of isolated points
if (!count) continue;
float sn1 = squaredNormalsDiff (up, center);
float sn2 = squaredNormalsDiff (down, center);
float r1 = sn1 + sn2;
float r2 = squaredNormalsDiff (right, center) + squaredNormalsDiff (left, center);
float d = std::min (r1, r2);
if (d < first_threshold_) continue;
sn1 = sqrt (sn1);
sn2 = sqrt (sn2);
float b1 = normalsDiff (right, up) * sn1;
b1+= normalsDiff (left, down) * sn2;
float b2 = normalsDiff (right, down) * sn2;
b2+= normalsDiff (left, up) * sn1;
float B = std::min (b1, b2);
float A = r2 - r1 - 2*B;
response (i,j) = ((B < 0) && ((B + A) > 0)) ? r1 - ((B*B)/A) : d;
}
}
}
else
{
#ifdef _OPENMP
#pragma omp parallel for num_threads (threads_)
#endif
for(int j = half_window_size_; j < h; ++j)
{
for(int i = half_window_size_; i < w; ++i)
{
if (!isFinite (input (i,j))) continue;
const NormalT ¢er = normals (i,j);
if (!isFinite (center)) continue;
int count = 0;
const NormalT &up = getNormalOrNull (i, j-half_window_size_, count);
const NormalT &down = getNormalOrNull (i, j+half_window_size_, count);
const NormalT &left = getNormalOrNull (i-half_window_size_, j, count);
const NormalT &right = getNormalOrNull (i+half_window_size_, j, count);
const NormalT &upleft = getNormalOrNull (i-half_window_size_, j-half_window_size_, count);
const NormalT &upright = getNormalOrNull (i+half_window_size_, j-half_window_size_, count);
const NormalT &downleft = getNormalOrNull (i-half_window_size_, j+half_window_size_, count);
const NormalT &downright = getNormalOrNull (i+half_window_size_, j+half_window_size_, count);
// Get rid of isolated points
if (!count) continue;
std::vector<float> r (4,0);
r[0] = squaredNormalsDiff (up, center);
r[0]+= squaredNormalsDiff (down, center);
r[1] = squaredNormalsDiff (upright, center);
r[1]+= squaredNormalsDiff (downleft, center);
r[2] = squaredNormalsDiff (right, center);
r[2]+= squaredNormalsDiff (left, center);
r[3] = squaredNormalsDiff (downright, center);
r[3]+= squaredNormalsDiff (upleft, center);
float d = *(std::min_element (r.begin (), r.end ()));
if (d < first_threshold_) continue;
std::vector<float> B (4,0);
std::vector<float> A (4,0);
std::vector<float> sumAB (4,0);
B[0] = normalsDiff (upright, up) * normalsDiff (up, center);
B[0]+= normalsDiff (downleft, down) * normalsDiff (down, center);
B[1] = normalsDiff (right, upright) * normalsDiff (upright, center);
B[1]+= normalsDiff (left, downleft) * normalsDiff (downleft, center);
B[2] = normalsDiff (downright, right) * normalsDiff (downright, center);
B[2]+= normalsDiff (upleft, left) * normalsDiff (upleft, center);
B[3] = normalsDiff (down, downright) * normalsDiff (downright, center);
B[3]+= normalsDiff (up, upleft) * normalsDiff (upleft, center);
A[0] = r[1] - r[0] - B[0] - B[0];
A[1] = r[2] - r[1] - B[1] - B[1];
A[2] = r[3] - r[2] - B[2] - B[2];
A[3] = r[0] - r[3] - B[3] - B[3];
sumAB[0] = A[0] + B[0];
sumAB[1] = A[1] + B[1];
sumAB[2] = A[2] + B[2];
sumAB[3] = A[3] + B[3];
if ((*std::max_element (B.begin (), B.end ()) < 0) &&
(*std::min_element (sumAB.begin (), sumAB.end ()) > 0))
{
std::vector<float> D (4,0);
D[0] = B[0] * B[0] / A[0];
D[1] = B[1] * B[1] / A[1];
D[2] = B[2] * B[2] / A[2];
D[3] = B[3] * B[3] / A[3];
response (i,j) = *(std::min (D.begin (), D.end ()));
}
else
response (i,j) = d;
}
}
}
// Non maximas suppression
std::vector<int> indices = *indices_;
std::sort (indices.begin (), indices.end (),
boost::bind (&TrajkovicKeypoint3D::greaterCornernessAtIndices, this, _1, _2));
output.clear ();
output.reserve (input_->size ());
std::vector<bool> occupency_map (indices.size (), false);
const int width (input_->width);
const int height (input_->height);
const int occupency_map_size (indices.size ());
#ifdef _OPENMP
#pragma omp parallel for shared (output) num_threads (threads_)
#endif
for (int i = 0; i < indices.size (); ++i)
{
int idx = indices[i];
if ((response_->points[idx] < second_threshold_) || occupency_map[idx])
continue;
PointOutT p;
p.getVector3fMap () = input_->points[idx].getVector3fMap ();
p.intensity = response_->points [idx];
#ifdef _OPENMP
#pragma omp critical
#endif
{
output.push_back (p);
keypoints_indices_->indices.push_back (idx);
}
const int x = idx % width;
const int y = idx / width;
const int u_end = std::min (width, x + half_window_size_);
const int v_end = std::min (height, y + half_window_size_);
for(int v = std::max (0, y - half_window_size_); v < v_end; ++v)
for(int u = std::max (0, x - half_window_size_); u < u_end; ++u)
occupency_map[v*width + u] = true;
}
output.height = 1;
output.width = static_cast<uint32_t> (output.size());
// we don not change the denseness
output.is_dense = true;
}
template<typename PointInT, typename PointOutT, typename NormalT> void
pcl::ISSKeypoint3D<PointInT, PointOutT, NormalT>::detectKeypoints (PointCloudOut &output)
{
// Make sure the output cloud is empty
output.points.clear ();
if (border_radius_ > 0.0)
edge_points_ = getBoundaryPoints (*(input_->makeShared ()), border_radius_, angle_threshold_);
bool* borders = new bool [input_->size()];
int index;
#ifdef _OPENMP
#pragma omp parallel for num_threads(threads_)
#endif
for (index = 0; index < int (input_->size ()); index++)
{
borders[index] = false;
PointInT current_point = input_->points[index];
if ((border_radius_ > 0.0) && (pcl::isFinite(current_point)))
{
std::vector<int> nn_indices;
std::vector<float> nn_distances;
this->searchForNeighbors (static_cast<int> (index), border_radius_, nn_indices, nn_distances);
for (size_t j = 0 ; j < nn_indices.size (); j++)
{
if (edge_points_[nn_indices[j]])
{
borders[index] = true;
break;
}
}
}
}
#ifdef _OPENMP
Eigen::Vector3d *omp_mem = new Eigen::Vector3d[threads_];
for (size_t i = 0; i < threads_; i++)
omp_mem[i].setZero (3);
#else
Eigen::Vector3d *omp_mem = new Eigen::Vector3d[1];
omp_mem[0].setZero (3);
#endif
double *prg_local_mem = new double[input_->size () * 3];
double **prg_mem = new double * [input_->size ()];
for (size_t i = 0; i < input_->size (); i++)
prg_mem[i] = prg_local_mem + 3 * i;
#ifdef _OPENMP
#pragma omp parallel for num_threads(threads_)
#endif
for (index = 0; index < static_cast<int> (input_->size ()); index++)
{
#ifdef _OPENMP
int tid = omp_get_thread_num ();
#else
int tid = 0;
#endif
PointInT current_point = input_->points[index];
if ((!borders[index]) && pcl::isFinite(current_point))
{
//if the considered point is not a border point and the point is "finite", then compute the scatter matrix
Eigen::Matrix3d cov_m = Eigen::Matrix3d::Zero ();
getScatterMatrix (static_cast<int> (index), cov_m);
Eigen::SelfAdjointEigenSolver<Eigen::Matrix3d> solver (cov_m);
const double& e1c = solver.eigenvalues ()[2];
const double& e2c = solver.eigenvalues ()[1];
const double& e3c = solver.eigenvalues ()[0];
if (!pcl_isfinite (e1c) || !pcl_isfinite (e2c) || !pcl_isfinite (e3c))
continue;
if (e3c < 0)
{
PCL_WARN ("[pcl::%s::detectKeypoints] : The third eigenvalue is negative! Skipping the point with index %i.\n",
name_.c_str (), index);
continue;
}
omp_mem[tid][0] = e2c / e1c;
omp_mem[tid][1] = e3c / e2c;;
omp_mem[tid][2] = e3c;
}
for (int d = 0; d < omp_mem[tid].size (); d++)
prg_mem[index][d] = omp_mem[tid][d];
}
for (index = 0; index < int (input_->size ()); index++)
{
if (!borders[index])
{
if ((prg_mem[index][0] < gamma_21_) && (prg_mem[index][1] < gamma_32_))
third_eigen_value_[index] = prg_mem[index][2];
}
}
bool* feat_max = new bool [input_->size()];
bool is_max;
#ifdef _OPENMP
#pragma omp parallel for private(is_max) num_threads(threads_)
#endif
for (index = 0; index < int (input_->size ()); index++)
{
feat_max [index] = false;
PointInT current_point = input_->points[index];
if ((third_eigen_value_[index] > 0.0) && (pcl::isFinite(current_point)))
{
std::vector<int> nn_indices;
std::vector<float> nn_distances;
int n_neighbors;
this->searchForNeighbors (static_cast<int> (index), non_max_radius_, nn_indices, nn_distances);
n_neighbors = static_cast<int> (nn_indices.size ());
if (n_neighbors >= min_neighbors_)
{
is_max = true;
for (int j = 0 ; j < n_neighbors; j++)
if (third_eigen_value_[index] < third_eigen_value_[nn_indices[j]])
is_max = false;
if (is_max)
feat_max[index] = true;
}
}
}
#ifdef _OPENMP
#pragma omp parallel for shared (output) num_threads(threads_)
#endif
for (index = 0; index < int (input_->size ()); index++)
{
if (feat_max[index])
#ifdef _OPENMP
#pragma omp critical
#endif
{
PointOutT p;
p.getVector3fMap () = input_->points[index].getVector3fMap ();
output.points.push_back(p);
keypoints_indices_->indices.push_back (index);
}
}
output.header = input_->header;
output.width = static_cast<uint32_t> (output.points.size ());
output.height = 1;
// Clear the contents of variables and arrays before the beginning of the next computation.
if (border_radius_ > 0.0)
normals_.reset (new pcl::PointCloud<NormalT>);
delete[] borders;
delete[] prg_mem;
delete[] prg_local_mem;
delete[] feat_max;
}
