template <typename PointInT, typename PointOutT> void
pcl::AgastKeypoint2D<PointInT, PointOutT>::detectKeypoints (PointCloudOut &output)
{
// image size
const size_t width = input_->width;
const size_t height = input_->height;
// destination for intensity data; will be forwarded to AGAST
std::vector<unsigned char> image_data (width*height);
for (size_t row_index = 0; row_index < height; ++row_index)
for (size_t col_index = 0; col_index < width; ++col_index)
image_data[row_index*width + col_index] = static_cast<unsigned char> (intensity_ ((*input_) (col_index, row_index)));
if (!detector_)
detector_.reset (new pcl::keypoints::agast::AgastDetector7_12s (width, height, threshold_, bmax_));
if (apply_non_max_suppression_)
{
pcl::PointCloud<pcl::PointUV> tmp_cloud;
detector_->detectKeypoints (image_data, tmp_cloud);
pcl::keypoints::internal::AgastApplyNonMaxSuppresion<PointOutT> anms (
image_data, tmp_cloud, detector_, output);
}
else
{
pcl::keypoints::internal::AgastDetector<PointOutT> dec (
image_data, detector_, output);
}
// we do not change the denseness
output.is_dense = true;
}
template <typename PointInT, typename PointOutT, typename IntensityT> void
pcl::BriskKeypoint2D<PointInT, PointOutT, IntensityT>::detectKeypoints (PointCloudOut &output)
{
// image size
const int width = int (input_->width);
const int height = int (input_->height);
// destination for intensity data; will be forwarded to BRISK
std::vector<unsigned char> image_data (width*height);
for (size_t i = 0; i < image_data.size (); ++i)
image_data[i] = static_cast<unsigned char> (intensity_ ((*input_)[i]));
pcl::keypoints::brisk::ScaleSpace brisk_scale_space (octaves_);
brisk_scale_space.constructPyramid (image_data, width, height);
// Check if the template types are the same. If true, avoid a copy.
// The PointOutT MUST be registered using the POINT_CLOUD_REGISTER_POINT_STRUCT macro!
if (isSamePointType<PointOutT, pcl::PointWithScale> ())
brisk_scale_space.getKeypoints (threshold_, output.points);
else
{
pcl::PointCloud<pcl::PointWithScale> output_temp;
brisk_scale_space.getKeypoints (threshold_, output_temp.points);
pcl::copyPointCloud<pcl::PointWithScale, PointOutT> (output_temp, output);
}
// we do not change the denseness
output.width = int (output.points.size ());
output.height = 1;
output.is_dense = false; // set to false to be sure
// 2nd pass to remove the invalid set of 3D keypoints
if (remove_invalid_3D_keypoints_)
{
PointCloudOut output_clean;
for (size_t i = 0; i < output.size (); ++i)
{
PointOutT pt;
// Interpolate its position in 3D, as the "u" and "v" are subpixel accurate
bilinearInterpolation (input_, output[i].x, output[i].y, pt);
// Check if the point is finite
if (pcl::isFinite (pt))
output_clean.push_back (output[i]);
}
output = output_clean;
output.is_dense = true; // set to true as there's no keypoint at an invalid XYZ
}
}
template <typename PointInT, typename PointOutT, typename IntensityT> void
pcl::HarrisKeypoint2D<PointInT, PointOutT, IntensityT>::detectKeypoints (PointCloudOut &output)
{
derivatives_cols_.resize (input_->width, input_->height);
derivatives_rows_.resize (input_->width, input_->height);
//Compute cloud intensities first derivatives along columns and rows
//!!! nsallem 20120220 : we don't test here for density so if one term in nan the result is nan
int w = static_cast<int> (input_->width) - 1;
int h = static_cast<int> (input_->height) - 1;
// j = 0 --> j-1 out of range ; use 0
// i = 0 --> i-1 out of range ; use 0
derivatives_cols_(0,0) = (intensity_ ((*input_) (0,1)) - intensity_ ((*input_) (0,0))) * 0.5;
derivatives_rows_(0,0) = (intensity_ ((*input_) (1,0)) - intensity_ ((*input_) (0,0))) * 0.5;
// #ifdef _OPENMP
// //#pragma omp parallel for shared (derivatives_cols_, input_) num_threads (threads_)
// #pragma omp parallel for num_threads (threads_)
// #endif
for(int i = 1; i < w; ++i)
{
derivatives_cols_(i,0) = (intensity_ ((*input_) (i,1)) - intensity_ ((*input_) (i,0))) * 0.5;
}
derivatives_rows_(w,0) = (intensity_ ((*input_) (w,0)) - intensity_ ((*input_) (w-1,0))) * 0.5;
derivatives_cols_(w,0) = (intensity_ ((*input_) (w,1)) - intensity_ ((*input_) (w,0))) * 0.5;
// #ifdef _OPENMP
// //#pragma omp parallel for shared (derivatives_cols_, derivatives_rows_, input_) num_threads (threads_)
// #pragma omp parallel for num_threads (threads_)
// #endif
for(int j = 1; j < h; ++j)
{
// i = 0 --> i-1 out of range ; use 0
derivatives_rows_(0,j) = (intensity_ ((*input_) (1,j)) - intensity_ ((*input_) (0,j))) * 0.5;
for(int i = 1; i < w; ++i)
{
// derivative with respect to rows
derivatives_rows_(i,j) = (intensity_ ((*input_) (i+1,j)) - intensity_ ((*input_) (i-1,j))) * 0.5;
// derivative with respect to cols
derivatives_cols_(i,j) = (intensity_ ((*input_) (i,j+1)) - intensity_ ((*input_) (i,j-1))) * 0.5;
}
// i = w --> w+1 out of range ; use w
derivatives_rows_(w,j) = (intensity_ ((*input_) (w,j)) - intensity_ ((*input_) (w-1,j))) * 0.5;
}
// j = h --> j+1 out of range use h
derivatives_cols_(0,h) = (intensity_ ((*input_) (0,h)) - intensity_ ((*input_) (0,h-1))) * 0.5;
derivatives_rows_(0,h) = (intensity_ ((*input_) (1,h)) - intensity_ ((*input_) (0,h))) * 0.5;
// #ifdef _OPENMP
// //#pragma omp parallel for shared (derivatives_cols_, input_) num_threads (threads_)
// #pragma omp parallel for num_threads (threads_)
// #endif
for(int i = 1; i < w; ++i)
{
derivatives_cols_(i,h) = (intensity_ ((*input_) (i,h)) - intensity_ ((*input_) (i,h-1))) * 0.5;
}
derivatives_rows_(w,h) = (intensity_ ((*input_) (w,h)) - intensity_ ((*input_) (w-1,h))) * 0.5;
derivatives_cols_(w,h) = (intensity_ ((*input_) (w,h)) - intensity_ ((*input_) (w,h-1))) * 0.5;
float highest_response_;
switch (method_)
{
case HARRIS:
responseHarris(*response_, highest_response_);
break;
case NOBLE:
responseNoble(*response_, highest_response_);
break;
case LOWE:
responseLowe(*response_, highest_response_);
break;
case TOMASI:
responseTomasi(*response_, highest_response_);
break;
}
if (!nonmax_)
output = *response_;
else
{
threshold_*= highest_response_;
std::sort (indices_->begin (), indices_->end (),
boost::bind (&HarrisKeypoint2D::greaterIntensityAtIndices, this, _1, _2));
output.clear ();
output.reserve (response_->size());
std::vector<bool> occupency_map (response_->size (), false);
int width (response_->width);
int height (response_->height);
const int occupency_map_size (occupency_map.size ());
#ifdef _OPENMP
#pragma omp parallel for shared (output, occupency_map) private (width, height) num_threads(threads_)
#endif
for (int idx = 0; idx < occupency_map_size; ++idx)
{
if (occupency_map[idx] || response_->points [indices_->at (idx)].intensity < threshold_ || !isFinite (response_->points[idx]))
continue;
#ifdef _OPENMP
#pragma omp critical
#endif
output.push_back (response_->at (indices_->at (idx)));
int u_end = std::min (width, indices_->at (idx) % width + min_distance_);
int v_end = std::min (height, indices_->at (idx) / width + min_distance_);
for(int u = std::max (0, indices_->at (idx) % width - min_distance_); u < u_end; ++u)
for(int v = std::max (0, indices_->at (idx) / width - min_distance_); v < v_end; ++v)
occupency_map[v*input_->width+u] = true;
}
// if (refine_)
// refineCorners (output);
output.height = 1;
output.width = static_cast<uint32_t> (output.size());
}
// we don not change the denseness
output.is_dense = input_->is_dense;
}
template <typename PointInT, typename IntensityT> void
pcl::tracking::PyramidalKLTTracker<PointInT, IntensityT>::computePyramids (const PointCloudInConstPtr& input,
std::vector<FloatImageConstPtr>& pyramid,
pcl::InterpolationType border_type) const
{
int step = 3;
pyramid.resize (step * nb_levels_);
FloatImageConstPtr previous;
FloatImagePtr tmp (new FloatImage (input->width, input->height));
#ifdef _OPENMP
#pragma omp parallel for num_threads (threads_)
#endif
for (int i = 0; i < static_cast<int> (input->size ()); ++i)
tmp->points[i] = intensity_ (input->points[i]);
previous = tmp;
FloatImagePtr img (new FloatImage (previous->width + 2*track_width_,
previous->height + 2*track_height_));
pcl::copyPointCloud (*tmp, *img, track_height_, track_height_, track_width_, track_width_,
border_type, 0.f);
pyramid[0] = img;
// compute first level gradients
FloatImagePtr g_x (new FloatImage (input->width, input->height));
FloatImagePtr g_y (new FloatImage (input->width, input->height));
derivatives (*img, *g_x, *g_y);
// copy to bigger clouds
FloatImagePtr grad_x (new FloatImage (previous->width + 2*track_width_,
previous->height + 2*track_height_));
pcl::copyPointCloud (*g_x, *grad_x, track_height_, track_height_, track_width_, track_width_,
pcl::BORDER_CONSTANT, 0.f);
pyramid[1] = grad_x;
FloatImagePtr grad_y (new FloatImage (previous->width + 2*track_width_,
previous->height + 2*track_height_));
pcl::copyPointCloud (*g_y, *grad_y, track_height_, track_height_, track_width_, track_width_,
pcl::BORDER_CONSTANT, 0.f);
pyramid[2] = grad_y;
for (int level = 1; level < nb_levels_; ++level)
{
// compute current level and current level gradients
FloatImageConstPtr current;
FloatImageConstPtr g_x;
FloatImageConstPtr g_y;
downsample (previous, current, g_x, g_y);
// copy to bigger clouds
FloatImagePtr image (new FloatImage (current->width + 2*track_width_,
current->height + 2*track_height_));
pcl::copyPointCloud (*current, *image, track_height_, track_height_, track_width_, track_width_,
border_type, 0.f);
pyramid[level*step] = image;
FloatImagePtr gradx (new FloatImage (g_x->width + 2*track_width_, g_x->height + 2*track_height_));
pcl::copyPointCloud (*g_x, *gradx, track_height_, track_height_, track_width_, track_width_,
pcl::BORDER_CONSTANT, 0.f);
pyramid[level*step + 1] = gradx;
FloatImagePtr grady (new FloatImage (g_y->width + 2*track_width_, g_y->height + 2*track_height_));
pcl::copyPointCloud (*g_y, *grady, track_height_, track_height_, track_width_, track_width_,
pcl::BORDER_CONSTANT, 0.f);
pyramid[level*step + 2] = grady;
// set the new level
previous = current;
}
}
template <typename PointInT, typename PointNT, typename PointOutT, typename IntensitySelectorT> void
pcl::IntensityGradientEstimation <PointInT, PointNT, PointOutT, IntensitySelectorT>::computePointIntensityGradient (
const pcl::PointCloud <PointInT> &cloud, const std::vector <int> &indices,
const Eigen::Vector3f &point, float mean_intensity, const Eigen::Vector3f &normal, Eigen::Vector3f &gradient)
{
if (indices.size () < 3)
{
gradient[0] = gradient[1] = gradient[2] = std::numeric_limits<float>::quiet_NaN ();
return;
}
Eigen::Matrix3f A = Eigen::Matrix3f::Zero ();
Eigen::Vector3f b = Eigen::Vector3f::Zero ();
for (size_t i_point = 0; i_point < indices.size (); ++i_point)
{
PointInT p = cloud.points[indices[i_point]];
if (!pcl_isfinite (p.x) ||
!pcl_isfinite (p.y) ||
!pcl_isfinite (p.z) ||
!pcl_isfinite (intensity_ (p)))
continue;
p.x -= point[0];
p.y -= point[1];
p.z -= point[2];
intensity_.demean (p, mean_intensity);
A (0, 0) += p.x * p.x;
A (0, 1) += p.x * p.y;
A (0, 2) += p.x * p.z;
A (1, 1) += p.y * p.y;
A (1, 2) += p.y * p.z;
A (2, 2) += p.z * p.z;
b[0] += p.x * intensity_ (p);
b[1] += p.y * intensity_ (p);
b[2] += p.z * intensity_ (p);
}
// Fill in the lower triangle of A
A (1, 0) = A (0, 1);
A (2, 0) = A (0, 2);
A (2, 1) = A (1, 2);
//*
Eigen::Vector3f x = A.colPivHouseholderQr ().solve (b);
/*/
Eigen::Vector3f eigen_values;
Eigen::Matrix3f eigen_vectors;
eigen33 (A, eigen_vectors, eigen_values);
b = eigen_vectors.transpose () * b;
if ( eigen_values (0) != 0)
b (0) /= eigen_values (0);
else
b (0) = 0;
if ( eigen_values (1) != 0)
b (1) /= eigen_values (1);
else
b (1) = 0;
if ( eigen_values (2) != 0)
b (2) /= eigen_values (2);
else
b (2) = 0;
Eigen::Vector3f x = eigen_vectors * b;
// if (A.col (0).squaredNorm () != 0)
// x [0] /= A.col (0).squaredNorm ();
// b -= x [0] * A.col (0);
//
//
// if (A.col (1).squaredNorm () != 0)
// x [1] /= A.col (1).squaredNorm ();
// b -= x[1] * A.col (1);
//
// x [2] = b.dot (A.col (2));
// if (A.col (2).squaredNorm () != 0)
// x[2] /= A.col (2).squaredNorm ();
// Fit a hyperplane to the data
//*/
// std::cout << A << "\n*\n" << bb << "\n=\n" << x << "\nvs.\n" << x2 << "\n\n";
// std::cout << A * x << "\nvs.\n" << A * x2 << "\n\n------\n";
// Project the gradient vector, x, onto the tangent plane
gradient = (Eigen::Matrix3f::Identity () - normal*normal.transpose ()) * x;
}
template <typename PointInT, typename PointNT, typename PointOutT, typename IntensitySelectorT> void
pcl::IntensityGradientEstimation<PointInT, PointNT, PointOutT, IntensitySelectorT>::computeFeature (PointCloudOut &output)
{
// Allocate enough space to hold the results
// \note This resize is irrelevant for a radiusSearch ().
std::vector<int> nn_indices (k_);
std::vector<float> nn_dists (k_);
output.is_dense = true;
// If the data is dense, we don't need to check for NaN
if (surface_->is_dense)
{
#ifdef _OPENMP
#pragma omp parallel for shared (output) private (nn_indices, nn_dists) num_threads(threads_)
#endif
// Iterating over the entire index vector
for (int idx = 0; idx < static_cast<int> (indices_->size ()); ++idx)
{
PointOutT &p_out = output.points[idx];
if (!this->searchForNeighbors ((*indices_)[idx], search_parameter_, nn_indices, nn_dists))
{
p_out.gradient[0] = p_out.gradient[1] = p_out.gradient[2] = std::numeric_limits<float>::quiet_NaN ();
output.is_dense = false;
continue;
}
Eigen::Vector3f centroid;
float mean_intensity = 0;
// Initialize to 0
centroid.setZero ();
for (size_t i = 0; i < nn_indices.size (); ++i)
{
centroid += surface_->points[nn_indices[i]].getVector3fMap ();
mean_intensity += intensity_ (surface_->points[nn_indices[i]]);
}
centroid /= static_cast<float> (nn_indices.size ());
mean_intensity /= static_cast<float> (nn_indices.size ());
Eigen::Vector3f normal = Eigen::Vector3f::Map (normals_->points[(*indices_) [idx]].normal);
Eigen::Vector3f gradient;
computePointIntensityGradient (*surface_, nn_indices, centroid, mean_intensity, normal, gradient);
p_out.gradient[0] = gradient[0];
p_out.gradient[1] = gradient[1];
p_out.gradient[2] = gradient[2];
}
}
else
{
#ifdef _OPENMP
#pragma omp parallel for shared (output) private (nn_indices, nn_dists) num_threads(threads_)
#endif
// Iterating over the entire index vector
for (int idx = 0; idx < static_cast<int> (indices_->size ()); ++idx)
{
PointOutT &p_out = output.points[idx];
if (!isFinite ((*surface_) [(*indices_)[idx]]) ||
!this->searchForNeighbors ((*indices_)[idx], search_parameter_, nn_indices, nn_dists))
{
p_out.gradient[0] = p_out.gradient[1] = p_out.gradient[2] = std::numeric_limits<float>::quiet_NaN ();
output.is_dense = false;
continue;
}
Eigen::Vector3f centroid;
float mean_intensity = 0;
// Initialize to 0
centroid.setZero ();
unsigned cp = 0;
for (size_t i = 0; i < nn_indices.size (); ++i)
{
// Check if the point is invalid
if (!isFinite ((*surface_) [nn_indices[i]]))
continue;
centroid += surface_->points [nn_indices[i]].getVector3fMap ();
mean_intensity += intensity_ (surface_->points [nn_indices[i]]);
++cp;
}
centroid /= static_cast<float> (cp);
mean_intensity /= static_cast<float> (cp);
Eigen::Vector3f normal = Eigen::Vector3f::Map (normals_->points[(*indices_) [idx]].normal);
Eigen::Vector3f gradient;
computePointIntensityGradient (*surface_, nn_indices, centroid, mean_intensity, normal, gradient);
p_out.gradient[0] = gradient[0];
p_out.gradient[1] = gradient[1];
p_out.gradient[2] = gradient[2];
}
}
}
template <typename PointInT, typename PointOutT, typename NormalT, typename IntensityT> void
pcl::SUSANKeypoint<PointInT, PointOutT, NormalT, IntensityT>::detectKeypoints (PointCloudOut &output)
{
boost::shared_ptr<pcl::PointCloud<PointOutT> > response (new pcl::PointCloud<PointOutT> ());
response->reserve (surface_->size ());
// Check if the output has a "label" field
label_idx_ = pcl::getFieldIndex<PointOutT> (output, "label", out_fields_);
const int input_size = static_cast<int> (input_->size ());
//#ifdef _OPENMP
//#pragma omp parallel for shared (response) num_threads(threads_)
//#endif
for (int point_index = 0; point_index < input_size; ++point_index)
{
const PointInT& point_in = input_->points [point_index];
const NormalT& normal_in = normals_->points [point_index];
if (!isFinite (point_in) || !isFinite (normal_in))
continue;
else
{
Eigen::Vector3f nucleus = point_in.getVector3fMap ();
Eigen::Vector3f nucleus_normal = normals_->points [point_index].getNormalVector3fMap ();
float nucleus_intensity = intensity_ (point_in);
std::vector<int> nn_indices;
std::vector<float> nn_dists;
tree_->radiusSearch (point_in, search_radius_, nn_indices, nn_dists);
float area = 0;
Eigen::Vector3f centroid = Eigen::Vector3f::Zero ();
// Exclude nucleus from the usan
std::vector<int> usan; usan.reserve (nn_indices.size () - 1);
for (std::vector<int>::const_iterator index = nn_indices.begin(); index != nn_indices.end(); ++index)
{
if ((*index != point_index) && pcl_isfinite (normals_->points[*index].normal_x))
{
// if the point fulfill condition
if ((fabs (nucleus_intensity - intensity_ (input_->points[*index])) <= intensity_threshold_) ||
(1 - nucleus_normal.dot (normals_->points[*index].getNormalVector3fMap ()) <= angular_threshold_))
{
++area;
centroid += input_->points[*index].getVector3fMap ();
usan.push_back (*index);
}
}
}
float geometric_threshold = 0.5f * (static_cast<float> (nn_indices.size () - 1));
if ((area > 0) && (area < geometric_threshold))
{
// if no geometric validation required add the point to the response
if (!geometric_validation_)
{
PointOutT point_out;
point_out.getVector3fMap () = point_in.getVector3fMap ();
//point_out.intensity = geometric_threshold - area;
intensity_out_.set (point_out, geometric_threshold - area);
// if a label field is found use it to save the index
if (label_idx_ != -1)
{
// save the index in the cloud
uint32_t label = static_cast<uint32_t> (point_index);
memcpy (reinterpret_cast<char*> (&point_out) + out_fields_[label_idx_].offset,
&label, sizeof (uint32_t));
}
//#ifdef _OPENMP
//#pragma omp critical
//#endif
response->push_back (point_out);
}
else
{
centroid /= area;
Eigen::Vector3f dist = nucleus - centroid;
// Check the distance <= distance_threshold_
if (dist.norm () >= distance_threshold_)
{
// point is valid from distance point of view
Eigen::Vector3f nc = centroid - nucleus;
// Check the contiguity
std::vector<int>::const_iterator usan_it = usan.begin ();
for (; usan_it != usan.end (); ++usan_it)
{
if (!isWithinNucleusCentroid (nucleus, centroid, nc, input_->points[*usan_it]))
break;
}
// All points within usan lies on the segment [nucleus centroid]
if (usan_it == usan.end ())
{
PointOutT point_out;
point_out.getVector3fMap () = point_in.getVector3fMap ();
// point_out.intensity = geometric_threshold - area;
intensity_out_.set (point_out, geometric_threshold - area);
// if a label field is found use it to save the index
if (label_idx_ != -1)
{
// save the index in the cloud
uint32_t label = static_cast<uint32_t> (point_index);
memcpy (reinterpret_cast<char*> (&point_out) + out_fields_[label_idx_].offset,
&label, sizeof (uint32_t));
}
//#ifdef _OPENMP
//#pragma omp critical
//#endif
response->push_back (point_out);
}
}
}
}
}
}
response->height = 1;
response->width = static_cast<uint32_t> (response->size ());
if (!nonmax_)
output = *response;
else
{
output.points.clear ();
output.points.reserve (response->points.size());
//#ifdef _OPENMP
//#pragma omp parallel for shared (output) num_threads(threads_)
//#endif
for (int idx = 0; idx < static_cast<int> (response->points.size ()); ++idx)
{
const PointOutT& point_in = response->points [idx];
const NormalT& normal_in = normals_->points [idx];
//const float intensity = response->points[idx].intensity;
const float intensity = intensity_out_ (response->points[idx]);
if (!isFinite (point_in) || !isFinite (normal_in) || (intensity == 0))
continue;
std::vector<int> nn_indices;
std::vector<float> nn_dists;
tree_->radiusSearch (idx, search_radius_, nn_indices, nn_dists);
bool is_minima = true;
for (std::vector<int>::const_iterator nn_it = nn_indices.begin(); nn_it != nn_indices.end(); ++nn_it)
{
// if (intensity > response->points[*nn_it].intensity)
if (intensity > intensity_out_ (response->points[*nn_it]))
{
is_minima = false;
break;
}
}
if (is_minima)
//#ifdef _OPENMP
//#pragma omp critical
//#endif
output.points.push_back (response->points[idx]);
}
output.height = 1;
output.width = static_cast<uint32_t> (output.points.size());
}
// we don not change the denseness
output.is_dense = input_->is_dense;
}
template <typename PointInT, typename PointOutT, typename KeypointT, typename IntensityT> void
pcl::BRISK2DEstimation<PointInT, PointOutT, KeypointT, IntensityT>::compute (
PointCloudOutT &output)
{
if (!input_cloud_->isOrganized ())
{
PCL_ERROR ("[pcl::%s::initCompute] %s doesn't support non organized clouds!\n", name_.c_str ());
return;
}
// image size
const int width = int (input_cloud_->width);
const int height = int (input_cloud_->height);
// destination for intensity data; will be forwarded to BRISK
std::vector<unsigned char> image_data (width*height);
for (size_t row_index = 0; row_index < height; ++row_index)
{
for (size_t col_index = 0; col_index < width; ++col_index)
{
image_data[row_index*width + col_index] = static_cast<unsigned char> (intensity_ ((*input_cloud_) (col_index, row_index)));
}
}
// Remove keypoints very close to the border
size_t ksize = keypoints_->points.size ();
std::vector<int> kscales; // remember the scale per keypoint
kscales.resize (ksize);
// initialize constants
static const float log2 = 0.693147180559945f;
static const float lb_scalerange = std::log (scalerange_) / (log2);
typename std::vector<KeypointT, Eigen::aligned_allocator<KeypointT> >::iterator beginning = keypoints_->points.begin ();
std::vector<int>::iterator beginningkscales = kscales.begin ();
static const float basic_size_06 = basic_size_ * 0.6f;
unsigned int basicscale = 0;
if (!scale_invariance_enabled_)
basicscale = std::max (static_cast<int> (float (scales_) / lb_scalerange * (log (1.45f * basic_size_ / (basic_size_06)) / log2) + 0.5f), 0);
for (size_t k = 0; k < ksize; k++)
{
unsigned int scale;
if (scale_invariance_enabled_)
{
scale = std::max (static_cast<int> (float (scales_) / lb_scalerange * (log (keypoints_->points[k].size / (basic_size_06)) / log2) + 0.5f), 0);
// saturate
if (scale >= scales_) scale = scales_ - 1;
kscales[k] = scale;
}
else
{
scale = basicscale;
kscales[k] = scale;
}
const int border = size_list_[scale];
const int border_x = width - border;
const int border_y = height - border;
if (RoiPredicate (float (border), float (border), float (border_x), float (border_y), keypoints_->points[k]))
{
keypoints_->points.erase (beginning + k);
kscales.erase (beginningkscales + k);
if (k == 0)
{
beginning = keypoints_->points.begin ();
beginningkscales = kscales.begin ();
}
ksize--;
k--;
}
}
// first, calculate the integral image over the whole image:
// current integral image
std::vector<int> integral; // the integral image
//integral (image, integral);
int* values = new int[points_]; // for temporary use
// resize the descriptors:
//output = zeros (ksize, strings_);
// now do the extraction for all keypoints:
// temporary variables containing gray values at sample points:
int t1;
int t2;
// the feature orientation
int direction0;
int direction1;
unsigned char* ptr = &output.points[0].descriptor[0];
for (size_t k = 0; k < ksize; k++)
{
int theta;
KeypointT &kp = keypoints_->points[k];
const int& scale = kscales[k];
int shifter = 0;
int* pvalues = values;
const float& x = float (kp.x);
const float& y = float (kp.y);
if (true) // kp.angle==-1
{
if (!rotation_invariance_enabled_)
// don't compute the gradient direction, just assign a rotation of 0°
theta = 0;
else
{
// get the gray values in the unrotated pattern
for (unsigned int i = 0; i < points_; i++)
*(pvalues++) = smoothedIntensity (image_data, width, height, integral, x, y, scale, 0, i);
direction0 = 0;
direction1 = 0;
// now iterate through the long pairings
const BriskLongPair* max = long_pairs_ + no_long_pairs_;
for (BriskLongPair* iter = long_pairs_; iter < max; ++iter)
{
t1 = *(values + iter->i);
t2 = *(values + iter->j);
const int delta_t = (t1 - t2);
// update the direction:
const int tmp0 = delta_t * (iter->weighted_dx) / 1024;
const int tmp1 = delta_t * (iter->weighted_dy) / 1024;
direction0 += tmp0;
direction1 += tmp1;
}
kp.angle = atan2 (float (direction1), float (direction0)) / float (M_PI) * 180.0f;
theta = static_cast<int> ((float (n_rot_) * kp.angle) / (360.0f) + 0.5f);
if (theta < 0)
theta += n_rot_;
if (theta >= int (n_rot_))
theta -= n_rot_;
}
}
else
{
// figure out the direction:
//int theta=rotationInvariance*round((_n_rot*atan2(direction.at<int>(0,0),direction.at<int>(1,0)))/(2*M_PI));
if (!rotation_invariance_enabled_)
theta = 0;
else
{
theta = static_cast<int> (n_rot_ * (kp.angle / (360.0)) + 0.5);
if (theta < 0)
theta += n_rot_;
if (theta >= int (n_rot_))
theta -= n_rot_;
}
}
// now also extract the stuff for the actual direction:
// let us compute the smoothed values
shifter = 0;
//unsigned int mean=0;
pvalues = values;
// get the gray values in the rotated pattern
for (unsigned int i = 0; i < points_; i++)
*(pvalues++) = smoothedIntensity (image_data, width, height, integral, x, y, scale, theta, i);
#ifdef __GNUC__
typedef uint32_t __attribute__ ((__may_alias__)) UINT32_ALIAS;
#endif
#ifdef _MSC_VER
// Todo: find the equivalent to may_alias
#define UCHAR_ALIAS uint32_t //__declspec(noalias)
#endif
// now iterate through all the pairings
UINT32_ALIAS* ptr2 = reinterpret_cast<UINT32_ALIAS*> (ptr);
const BriskShortPair* max = short_pairs_ + no_short_pairs_;
for (BriskShortPair* iter = short_pairs_; iter < max; ++iter)
{
t1 = *(values + iter->i);
t2 = *(values + iter->j);
if (t1 > t2)
*ptr2 |= ((1) << shifter);
// else already initialized with zero
// take care of the iterators:
++shifter;
if (shifter == 32)
{
shifter = 0;
++ptr2;
}
}
ptr += strings_;
// Account for the scale + orientation;
ptr += sizeof (output.points[0].scale);
ptr += sizeof (output.points[0].orientation);
}
// we do not change the denseness
output.width = int (output.points.size ());
output.height = 1;
output.is_dense = true;
// clean-up
delete [] values;
}
