template <typename PointT> void
pcl::SampleConsensusModelStick<PointT>::selectWithinDistance (
const Eigen::VectorXf &model_coefficients, const double threshold, std::vector<int> &inliers)
{
// Needs a valid set of model coefficients
if (!isModelValid (model_coefficients))
return;
float sqr_threshold = static_cast<float> (threshold * threshold);
int nr_p = 0;
inliers.resize (indices_->size ());
error_sqr_dists_.resize (indices_->size ());
// Obtain the line point and direction
Eigen::Vector4f line_pt1 (model_coefficients[0], model_coefficients[1], model_coefficients[2], 0);
Eigen::Vector4f line_pt2 (model_coefficients[3], model_coefficients[4], model_coefficients[5], 0);
Eigen::Vector4f line_dir = line_pt2 - line_pt1;
//Eigen::Vector4f line_dir (model_coefficients[3], model_coefficients[4], model_coefficients[5], 0);
//Eigen::Vector4f line_dir (model_coefficients[3] - model_coefficients[0], model_coefficients[4] - model_coefficients[1], model_coefficients[5] - model_coefficients[2], 0);
line_dir.normalize ();
//float norm = line_dir.squaredNorm ();
// Iterate through the 3d points and calculate the distances from them to the line
for (size_t i = 0; i < indices_->size (); ++i)
{
// Calculate the distance from the point to the line
// D = ||(P2-P1) x (P1-P0)|| / ||P2-P1|| = norm (cross (p2-p1, p2-p0)) / norm(p2-p1)
Eigen::Vector4f dir = input_->points[(*indices_)[i]].getVector4fMap () - line_pt1;
//float u = dir.dot (line_dir);
// If the point falls outside of the segment, ignore it
//if (u < 0.0f || u > 1.0f)
// continue;
float sqr_distance = dir.cross3 (line_dir).squaredNorm ();
if (sqr_distance < sqr_threshold)
{
// Returns the indices of the points whose squared distances are smaller than the threshold
inliers[nr_p] = (*indices_)[i];
error_sqr_dists_[nr_p] = static_cast<double> (sqr_distance);
++nr_p;
}
}
inliers.resize (nr_p);
error_sqr_dists_.resize (nr_p);
}
template <typename PointT> int
pcl::SampleConsensusModelStick<PointT>::countWithinDistance (
const Eigen::VectorXf &model_coefficients, const double threshold) const
{
// Needs a valid set of model coefficients
if (!isModelValid (model_coefficients))
return (0);
float sqr_threshold = static_cast<float> (threshold * threshold);
int nr_i = 0, nr_o = 0;
// Obtain the line point and direction
Eigen::Vector4f line_pt1 (model_coefficients[0], model_coefficients[1], model_coefficients[2], 0);
Eigen::Vector4f line_pt2 (model_coefficients[3], model_coefficients[4], model_coefficients[5], 0);
Eigen::Vector4f line_dir = line_pt2 - line_pt1;
line_dir.normalize ();
//Eigen::Vector4f line_dir (model_coefficients[3] - model_coefficients[0], model_coefficients[4] - model_coefficients[1], model_coefficients[5] - model_coefficients[2], 0);
//Eigen::Vector4f line_dir (model_coefficients[3], model_coefficients[4], model_coefficients[5], 0);
// Iterate through the 3d points and calculate the distances from them to the line
for (size_t i = 0; i < indices_->size (); ++i)
{
// Calculate the distance from the point to the line
// D = ||(P2-P1) x (P1-P0)|| / ||P2-P1|| = norm (cross (p2-p1, p2-p0)) / norm(p2-p1)
Eigen::Vector4f dir = input_->points[(*indices_)[i]].getVector4fMap () - line_pt1;
//float u = dir.dot (line_dir);
// If the point falls outside of the segment, ignore it
//if (u < 0.0f || u > 1.0f)
// continue;
float sqr_distance = dir.cross3 (line_dir).squaredNorm ();
// Use a larger threshold (4 times the radius) to get more points in
if (sqr_distance < sqr_threshold)
nr_i++;
else if (sqr_distance < 4 * sqr_threshold)
nr_o++;
}
return (nr_i - nr_o < 0 ? 0 : nr_i - nr_o);
}
template <typename PointT, typename PointNT, typename PointFeature> void
pcl::NormalBasedSignatureEstimation<PointT, PointNT, PointFeature>::computeFeature (FeatureCloud &output)
{
// do a few checks before starting the computations
PointFeature test_feature;
(void)test_feature;
if (N_prime_ * M_prime_ != sizeof (test_feature.values) / sizeof (float))
{
PCL_ERROR ("NormalBasedSignatureEstimation: not using the proper signature size: %u vs %u\n", N_prime_ * M_prime_, sizeof (test_feature.values) / sizeof (float));
return;
}
std::vector<int> k_indices;
std::vector<float> k_sqr_distances;
tree_->setInputCloud (input_);
output.points.resize (indices_->size ());
for (size_t index_i = 0; index_i < indices_->size (); ++index_i)
{
size_t point_i = (*indices_)[index_i];
Eigen::MatrixXf s_matrix (N_, M_);
Eigen::Vector4f center_point = input_->points[point_i].getVector4fMap ();
for (size_t k = 0; k < N_; ++k)
{
Eigen::VectorXf s_row (M_);
for (size_t l = 0; l < M_; ++l)
{
Eigen::Vector4f normal = normals_->points[point_i].getNormalVector4fMap ();
Eigen::Vector4f normal_u = Eigen::Vector4f::Zero ();
Eigen::Vector4f normal_v = Eigen::Vector4f::Zero ();
if (fabs (normal.x ()) > 0.0001f)
{
normal_u.x () = - normal.y () / normal.x ();
normal_u.y () = 1.0f;
normal_u.z () = 0.0f;
normal_u.normalize ();
}
else if (fabs (normal.y ()) > 0.0001f)
{
normal_u.x () = 1.0f;
normal_u.y () = - normal.x () / normal.y ();
normal_u.z () = 0.0f;
normal_u.normalize ();
}
else
{
normal_u.x () = 0.0f;
normal_u.y () = 1.0f;
normal_u.z () = - normal.y () / normal.z ();
}
normal_v = normal.cross3 (normal_u);
Eigen::Vector4f zeta_point = 2.0f * static_cast<float> (l + 1) * scale_h_ / static_cast<float> (M_) *
(cosf (2.0f * static_cast<float> (M_PI) * static_cast<float> ((k + 1) / N_)) * normal_u +
sinf (2.0f * static_cast<float> (M_PI) * static_cast<float> ((k + 1) / N_)) * normal_v);
// Compute normal by using the neighbors
Eigen::Vector4f zeta_point_plus_center = zeta_point + center_point;
PointT zeta_point_pcl;
zeta_point_pcl.x = zeta_point_plus_center.x (); zeta_point_pcl.y = zeta_point_plus_center.y (); zeta_point_pcl.z = zeta_point_plus_center.z ();
tree_->radiusSearch (zeta_point_pcl, search_radius_, k_indices, k_sqr_distances);
// Do k nearest search if there are no neighbors nearby
if (k_indices.size () == 0)
{
k_indices.resize (5);
k_sqr_distances.resize (5);
tree_->nearestKSearch (zeta_point_pcl, 5, k_indices, k_sqr_distances);
}
Eigen::Vector4f average_normal = Eigen::Vector4f::Zero ();
float average_normalization_factor = 0.0f;
// Normals weighted by 1/squared_distances
for (size_t nn_i = 0; nn_i < k_indices.size (); ++nn_i)
{
if (k_sqr_distances[nn_i] < 1e-7f)
{
average_normal = normals_->points[k_indices[nn_i]].getNormalVector4fMap ();
average_normalization_factor = 1.0f;
break;
}
average_normal += normals_->points[k_indices[nn_i]].getNormalVector4fMap () / k_sqr_distances[nn_i];
average_normalization_factor += 1.0f / k_sqr_distances[nn_i];
}
average_normal /= average_normalization_factor;
float s = zeta_point.dot (average_normal) / zeta_point.norm ();
s_row[l] = s;
}
// do DCT on the s_matrix row-wise
Eigen::VectorXf dct_row (M_);
for (int m = 0; m < s_row.size (); ++m)
{
float Xk = 0.0f;
for (int n = 0; n < s_row.size (); ++n)
Xk += static_cast<float> (s_row[n] * cos (M_PI / (static_cast<double> (M_ * n) + 0.5) * static_cast<double> (k)));
dct_row[m] = Xk;
}
s_row = dct_row;
s_matrix.row (k).matrix () = dct_row;
}
// do DFT on the s_matrix column-wise
Eigen::MatrixXf dft_matrix (N_, M_);
for (size_t column_i = 0; column_i < M_; ++column_i)
{
Eigen::VectorXf dft_col (N_);
for (size_t k = 0; k < N_; ++k)
{
float Xk_real = 0.0f, Xk_imag = 0.0f;
for (size_t n = 0; n < N_; ++n)
{
Xk_real += static_cast<float> (s_matrix (n, column_i) * cos (2.0f * M_PI / static_cast<double> (N_ * k * n)));
Xk_imag += static_cast<float> (s_matrix (n, column_i) * sin (2.0f * M_PI / static_cast<double> (N_ * k * n)));
}
dft_col[k] = sqrtf (Xk_real*Xk_real + Xk_imag*Xk_imag);
}
dft_matrix.col (column_i).matrix () = dft_col;
}
Eigen::MatrixXf final_matrix = dft_matrix.block (0, 0, N_prime_, M_prime_);
PointFeature feature_point;
for (size_t i = 0; i < N_prime_; ++i)
for (size_t j = 0; j < M_prime_; ++j)
feature_point.values[i*M_prime_ + j] = final_matrix (i, j);
output.points[index_i] = feature_point;
}
}
void
pcl_ros::ConvexHull2D::input_indices_callback (const PointCloudConstPtr &cloud,
const PointIndicesConstPtr &indices)
{
// No subscribers, no work
if (pub_output_.getNumSubscribers () <= 0 && pub_plane_.getNumSubscribers () <= 0)
return;
PointCloud output;
// If cloud is given, check if it's valid
if (!isValid (cloud))
{
NODELET_ERROR ("[%s::input_indices_callback] Invalid input!", getName ().c_str ());
// Publish an empty message
output.header = cloud->header;
pub_output_.publish (output.makeShared ());
return;
}
// If indices are given, check if they are valid
if (indices && !isValid (indices, "indices"))
{
NODELET_ERROR ("[%s::input_indices_callback] Invalid indices!", getName ().c_str ());
// Publish an empty message
output.header = cloud->header;
pub_output_.publish (output.makeShared ());
return;
}
/// DEBUG
if (indices)
NODELET_DEBUG ("[%s::input_indices_model_callback]\n"
" - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n"
" - PointIndices with %zu values, stamp %f, and frame %s on topic %s received.",
getName ().c_str (),
cloud->width * cloud->height, pcl::getFieldsList (*cloud).c_str (), cloud->header.stamp.toSec (), cloud->header.frame_id.c_str (), getMTPrivateNodeHandle ().resolveName ("input").c_str (),
indices->indices.size (), indices->header.stamp.toSec (), indices->header.frame_id.c_str (), getMTPrivateNodeHandle ().resolveName ("indices").c_str ());
else
NODELET_DEBUG ("[%s::input_indices_callback] PointCloud with %d data points, stamp %f, and frame %s on topic %s received.", getName ().c_str (), cloud->width * cloud->height, cloud->header.stamp.toSec (), cloud->header.frame_id.c_str (), getMTPrivateNodeHandle ().resolveName ("input").c_str ());
// Reset the indices and surface pointers
IndicesPtr indices_ptr;
if (indices)
indices_ptr.reset (new std::vector<int> (indices->indices));
impl_.setInputCloud (cloud);
impl_.setIndices (indices_ptr);
// Estimate the feature
impl_.reconstruct (output);
// If more than 3 points are present, send a PolygonStamped hull too
if (output.points.size () >= 3)
{
geometry_msgs::PolygonStamped poly;
poly.header = output.header;
poly.polygon.points.resize (output.points.size ());
// Get three consecutive points (without copying)
pcl::Vector4fMap O = output.points[1].getVector4fMap ();
pcl::Vector4fMap B = output.points[0].getVector4fMap ();
pcl::Vector4fMap A = output.points[2].getVector4fMap ();
// Check the direction of points -- polygon must have CCW direction
Eigen::Vector4f OA = A - O;
Eigen::Vector4f OB = B - O;
Eigen::Vector4f N = OA.cross3 (OB);
double theta = N.dot (O);
bool reversed = false;
if (theta < (M_PI / 2.0))
reversed = true;
for (size_t i = 0; i < output.points.size (); ++i)
{
if (reversed)
{
size_t j = output.points.size () - i - 1;
poly.polygon.points[i].x = output.points[j].x;
poly.polygon.points[i].y = output.points[j].y;
poly.polygon.points[i].z = output.points[j].z;
}
else
{
poly.polygon.points[i].x = output.points[i].x;
poly.polygon.points[i].y = output.points[i].y;
poly.polygon.points[i].z = output.points[i].z;
}
}
pub_plane_.publish (boost::make_shared<const geometry_msgs::PolygonStamped> (poly));
}
// Publish a Boost shared ptr const data
output.header = cloud->header;
pub_output_.publish (output.makeShared ());
}
template <typename PointInT, typename PointNT, typename PointOutT> bool
pcl::FPFHEstimation<PointInT, PointNT, PointOutT>::computePairFeatures (
const pcl::PointCloud<PointInT> &cloud, const pcl::PointCloud<PointNT> &normals,
int p_idx, int q_idx, float &f1, float &f2, float &f3, float &f4)
{
// Compute the Cartesian difference between the two points
Eigen::Vector4f delta = cloud.points[q_idx].getVector4fMap () - cloud.points[p_idx].getVector4fMap ();
delta[3] = 0;
// Compute the Euclidean norm = || p_idx - q_idx ||
float distance_sqr = delta.squaredNorm ();
if (distance_sqr == 0)
{
ROS_ERROR ("Euclidean distance between points %d and %d is 0!", p_idx, q_idx);
f1 = f2 = f3 = f4 = 0;
return (false);
}
// Estimate f4 = || delta ||
f4 = sqrt (distance_sqr);
// Create a Darboux frame coordinate system u-v-w
// u = n1; v = (p_idx - q_idx) x u / || (p_idx - q_idx) x u ||; w = u x v
pcl::Vector4fMapConst u = normals.points[p_idx].getNormalVector4fMap ();
// Estimate f3 = u * delta / || delta ||
// delta[3] = 0 (line 59)
f3 = u.dot (delta) / f4;
// v = delta * u
Eigen::Vector4f v = Eigen::Vector4f::Zero ();
v = delta.cross3 (u);
distance_sqr = v.squaredNorm ();
if (distance_sqr == 0)
{
ROS_ERROR ("Norm of Delta x U is 0 for point %d and %d!", p_idx, q_idx);
f1 = f2 = f3 = f4 = 0;
return (false);
}
// Copy the q_idx normal
Eigen::Vector4f nq (normals.points[q_idx].normal_x,
normals.points[q_idx].normal_y,
normals.points[q_idx].normal_z,
0);
// Normalize the vector
v /= sqrt (distance_sqr);
// Compute delta (w) = u x v
delta = u.cross3 (v);
// Compute f2 = v * n2;
// v[3] = 0 (line 82)
f2 = v.dot (nq);
// Compute f1 = arctan (w * n2, u * n2) i.e. angle of n2 in the x=u, y=w coordinate system
// delta[3] = 0 (line 59), nq[3] = 0 (line 97)
f1 = atan2f (delta.dot (nq), u.dot (nq)); // @todo: optimize this
return (true);
}
