template <typename PointInT, typename PointNT, typename PointOutT> void
pcl::PFHEstimation<PointInT, PointNT, PointOutT>::computePointPFHSignature (
const pcl::PointCloud<PointInT> &cloud, const pcl::PointCloud<PointNT> &normals,
const std::vector<int> &indices, int nr_split, Eigen::VectorXf &pfh_histogram)
{
int h_index, h_p;
// Clear the resultant point histogram
pfh_histogram.setZero ();
// Factorization constant
float hist_incr = 100.0f / static_cast<float> (indices.size () * (indices.size () - 1) / 2);
std::pair<int, int> key;
// Iterate over all the points in the neighborhood
for (size_t i_idx = 0; i_idx < indices.size (); ++i_idx)
{
for (size_t j_idx = 0; j_idx < i_idx; ++j_idx)
{
// If the 3D points are invalid, don't bother estimating, just continue
if (!isFinite (cloud.points[indices[i_idx]]) || !isFinite (cloud.points[indices[j_idx]]))
continue;
if (use_cache_)
{
// In order to create the key, always use the smaller index as the first key pair member
int p1, p2;
// if (indices[i_idx] >= indices[j_idx])
// {
p1 = indices[i_idx];
p2 = indices[j_idx];
// }
// else
// {
// p1 = indices[j_idx];
// p2 = indices[i_idx];
// }
key = std::pair<int, int> (p1, p2);
// Check to see if we already estimated this pair in the global hashmap
std::map<std::pair<int, int>, Eigen::Vector4f, std::less<std::pair<int, int> >, Eigen::aligned_allocator<Eigen::Vector4f> >::iterator fm_it = feature_map_.find (key);
if (fm_it != feature_map_.end ())
pfh_tuple_ = fm_it->second;
else
{
// Compute the pair NNi to NNj
if (!computePairFeatures (cloud, normals, indices[i_idx], indices[j_idx],
pfh_tuple_[0], pfh_tuple_[1], pfh_tuple_[2], pfh_tuple_[3]))
continue;
}
}
else
if (!computePairFeatures (cloud, normals, indices[i_idx], indices[j_idx],
pfh_tuple_[0], pfh_tuple_[1], pfh_tuple_[2], pfh_tuple_[3]))
continue;
// Normalize the f1, f2, f3 features and push them in the histogram
f_index_[0] = static_cast<int> (floor (nr_split * ((pfh_tuple_[0] + M_PI) * d_pi_)));
if (f_index_[0] < 0) f_index_[0] = 0;
if (f_index_[0] >= nr_split) f_index_[0] = nr_split - 1;
f_index_[1] = static_cast<int> (floor (nr_split * ((pfh_tuple_[1] + 1.0) * 0.5)));
if (f_index_[1] < 0) f_index_[1] = 0;
if (f_index_[1] >= nr_split) f_index_[1] = nr_split - 1;
f_index_[2] = static_cast<int> (floor (nr_split * ((pfh_tuple_[2] + 1.0) * 0.5)));
if (f_index_[2] < 0) f_index_[2] = 0;
if (f_index_[2] >= nr_split) f_index_[2] = nr_split - 1;
// Copy into the histogram
h_index = 0;
h_p = 1;
for (int d = 0; d < 3; ++d)
{
h_index += h_p * f_index_[d];
h_p *= nr_split;
}
pfh_histogram[h_index] += hist_incr;
if (use_cache_)
{
// Save the value in the hashmap
feature_map_[key] = pfh_tuple_;
// Use a maximum cache so that we don't go overboard on RAM usage
key_list_.push (key);
// Check to see if we need to remove an element due to exceeding max_size
if (key_list_.size () > max_cache_size_)
{
// Remove the last element.
feature_map_.erase (key_list_.back ());
key_list_.pop ();
}
}
}
}
}
template <typename PointT> void
pcl::SampleConsensusModelPlane<PointT>::projectPoints (
const std::vector<int> &inliers, const Eigen::VectorXf &model_coefficients, PointCloud &projected_points, bool copy_data_fields)
{
// Needs a valid set of model coefficients
if (model_coefficients.size () != 4)
{
PCL_ERROR ("[pcl::SampleConsensusModelPlane::projectPoints] Invalid number of model coefficients given (%zu)!\n", model_coefficients.size ());
return;
}
projected_points.header = input_->header;
projected_points.is_dense = input_->is_dense;
Eigen::Vector4f mc (model_coefficients[0], model_coefficients[1], model_coefficients[2], 0);
// normalize the vector perpendicular to the plane...
mc.normalize ();
// ... and store the resulting normal as a local copy of the model coefficients
Eigen::Vector4f tmp_mc = model_coefficients;
tmp_mc[0] = mc[0];
tmp_mc[1] = mc[1];
tmp_mc[2] = mc[2];
// Copy all the data fields from the input cloud to the projected one?
if (copy_data_fields)
{
// Allocate enough space and copy the basics
projected_points.points.resize (input_->points.size ());
projected_points.width = input_->width;
projected_points.height = input_->height;
typedef typename pcl::traits::fieldList<PointT>::type FieldList;
// Iterate over each point
for (size_t i = 0; i < input_->points.size (); ++i)
// Iterate over each dimension
pcl::for_each_type <FieldList> (NdConcatenateFunctor <PointT, PointT> (input_->points[i], projected_points.points[i]));
// Iterate through the 3d points and calculate the distances from them to the plane
for (size_t i = 0; i < inliers.size (); ++i)
{
// Calculate the distance from the point to the plane
Eigen::Vector4f p (input_->points[inliers[i]].x,
input_->points[inliers[i]].y,
input_->points[inliers[i]].z,
1);
// use normalized coefficients to calculate the scalar projection
float distance_to_plane = tmp_mc.dot (p);
pcl::Vector4fMap pp = projected_points.points[inliers[i]].getVector4fMap ();
pp = p - mc * distance_to_plane; // mc[3] = 0, therefore the 3rd coordinate is safe
}
}
else
{
// Allocate enough space and copy the basics
projected_points.points.resize (inliers.size ());
projected_points.width = static_cast<uint32_t> (inliers.size ());
projected_points.height = 1;
typedef typename pcl::traits::fieldList<PointT>::type FieldList;
// Iterate over each point
for (size_t i = 0; i < inliers.size (); ++i)
// Iterate over each dimension
pcl::for_each_type <FieldList> (NdConcatenateFunctor <PointT, PointT> (input_->points[inliers[i]], projected_points.points[i]));
// Iterate through the 3d points and calculate the distances from them to the plane
for (size_t i = 0; i < inliers.size (); ++i)
{
// Calculate the distance from the point to the plane
Eigen::Vector4f p (input_->points[inliers[i]].x,
input_->points[inliers[i]].y,
input_->points[inliers[i]].z,
1);
// use normalized coefficients to calculate the scalar projection
float distance_to_plane = tmp_mc.dot (p);
pcl::Vector4fMap pp = projected_points.points[i].getVector4fMap ();
pp = p - mc * distance_to_plane; // mc[3] = 0, therefore the 3rd coordinate is safe
}
}
}
template <typename PointT> void
pcl::SampleConsensusModelCircle2D<PointT>::projectPoints (
const std::vector<int> &inliers, const Eigen::VectorXf &model_coefficients,
PointCloud &projected_points, bool copy_data_fields)
{
// Needs a valid set of model coefficients
if (model_coefficients.size () != 3)
{
PCL_ERROR ("[pcl::SampleConsensusModelCircle2D::projectPoints] Invalid number of model coefficients given (%zu)!\n", model_coefficients.size ());
return;
}
projected_points.header = input_->header;
projected_points.is_dense = input_->is_dense;
// Copy all the data fields from the input cloud to the projected one?
if (copy_data_fields)
{
// Allocate enough space and copy the basics
projected_points.points.resize (input_->points.size ());
projected_points.width = input_->width;
projected_points.height = input_->height;
typedef typename pcl::traits::fieldList<PointT>::type FieldList;
// Iterate over each point
for (size_t i = 0; i < projected_points.points.size (); ++i)
// Iterate over each dimension
pcl::for_each_type <FieldList> (NdConcatenateFunctor <PointT, PointT> (input_->points[i], projected_points.points[i]));
// Iterate through the 3d points and calculate the distances from them to the plane
for (size_t i = 0; i < inliers.size (); ++i)
{
float dx = input_->points[inliers[i]].x - model_coefficients[0];
float dy = input_->points[inliers[i]].y - model_coefficients[1];
float a = sqrtf ( (model_coefficients[2] * model_coefficients[2]) / (dx * dx + dy * dy) );
projected_points.points[inliers[i]].x = a * dx + model_coefficients[0];
projected_points.points[inliers[i]].y = a * dy + model_coefficients[1];
}
}
else
{
// Allocate enough space and copy the basics
projected_points.points.resize (inliers.size ());
projected_points.width = static_cast<uint32_t> (inliers.size ());
projected_points.height = 1;
typedef typename pcl::traits::fieldList<PointT>::type FieldList;
// Iterate over each point
for (size_t i = 0; i < inliers.size (); ++i)
// Iterate over each dimension
pcl::for_each_type <FieldList> (NdConcatenateFunctor <PointT, PointT> (input_->points[inliers[i]], projected_points.points[i]));
// Iterate through the 3d points and calculate the distances from them to the plane
for (size_t i = 0; i < inliers.size (); ++i)
{
float dx = input_->points[inliers[i]].x - model_coefficients[0];
float dy = input_->points[inliers[i]].y - model_coefficients[1];
float a = sqrtf ( (model_coefficients[2] * model_coefficients[2]) / (dx * dx + dy * dy) );
projected_points.points[i].x = a * dx + model_coefficients[0];
projected_points.points[i].y = a * dy + model_coefficients[1];
}
}
}
template <typename PointT> void
pcl::SampleConsensusModelCircle3D<PointT>::projectPoints (
const std::vector<int> &inliers, const Eigen::VectorXf &model_coefficients,
PointCloud &projected_points, bool copy_data_fields)
{
// Needs a valid set of model coefficients
if (model_coefficients.size () != 7)
{
PCL_ERROR ("[pcl::SampleConsensusModelCircle3D::projectPoints] Invalid number of model coefficients given (%lu)!\n", model_coefficients.size ());
return;
}
projected_points.header = input_->header;
projected_points.is_dense = input_->is_dense;
// Copy all the data fields from the input cloud to the projected one?
if (copy_data_fields)
{
// Allocate enough space and copy the basics
projected_points.points.resize (input_->points.size ());
projected_points.width = input_->width;
projected_points.height = input_->height;
typedef typename pcl::traits::fieldList<PointT>::type FieldList;
// Iterate over each point
for (size_t i = 0; i < projected_points.points.size (); ++i)
// Iterate over each dimension
pcl::for_each_type <FieldList> (NdConcatenateFunctor <PointT, PointT> (input_->points[i], projected_points.points[i]));
// Iterate through the 3d points and calculate the distances from them to the plane
for (size_t i = 0; i < inliers.size (); ++i)
{
// what i have:
// P : Sample Point
Eigen::Vector3d P (input_->points[inliers[i]].x, input_->points[inliers[i]].y, input_->points[inliers[i]].z);
// C : Circle Center
Eigen::Vector3d C (model_coefficients[0], model_coefficients[1], model_coefficients[2]);
// N : Circle (Plane) Normal
Eigen::Vector3d N (model_coefficients[4], model_coefficients[5], model_coefficients[6]);
// r : Radius
double r = model_coefficients[3];
Eigen::Vector3d helper_vectorPC = P - C;
// 1.1. get line parameter
//float lambda = (helper_vectorPC.dot(N)) / N.squaredNorm() ;
double lambda = (-(helper_vectorPC.dot (N))) / N.dot (N);
// Projected Point on plane
Eigen::Vector3d P_proj = P + lambda * N;
Eigen::Vector3d helper_vectorP_projC = P_proj - C;
// K : Point on Circle
Eigen::Vector3d K = C + r * helper_vectorP_projC.normalized ();
projected_points.points[i].x = static_cast<float> (K[0]);
projected_points.points[i].y = static_cast<float> (K[1]);
projected_points.points[i].z = static_cast<float> (K[2]);
}
}
else
{
// Allocate enough space and copy the basics
projected_points.points.resize (inliers.size ());
projected_points.width = uint32_t (inliers.size ());
projected_points.height = 1;
typedef typename pcl::traits::fieldList<PointT>::type FieldList;
// Iterate over each point
for (size_t i = 0; i < inliers.size (); ++i)
// Iterate over each dimension
pcl::for_each_type <FieldList> (NdConcatenateFunctor <PointT, PointT> (input_->points[inliers[i]], projected_points.points[i]));
// Iterate through the 3d points and calculate the distances from them to the plane
for (size_t i = 0; i < inliers.size (); ++i)
{
// what i have:
// P : Sample Point
Eigen::Vector3d P (input_->points[inliers[i]].x, input_->points[inliers[i]].y, input_->points[inliers[i]].z);
// C : Circle Center
Eigen::Vector3d C (model_coefficients[0], model_coefficients[1], model_coefficients[2]);
// N : Circle (Plane) Normal
Eigen::Vector3d N (model_coefficients[4], model_coefficients[5], model_coefficients[6]);
// r : Radius
double r = model_coefficients[3];
Eigen::Vector3d helper_vectorPC = P - C;
// 1.1. get line parameter
double lambda = (-(helper_vectorPC.dot (N))) / N.dot (N);
// Projected Point on plane
Eigen::Vector3d P_proj = P + lambda * N;
Eigen::Vector3d helper_vectorP_projC = P_proj - C;
// K : Point on Circle
Eigen::Vector3d K = C + r * helper_vectorP_projC.normalized ();
projected_points.points[i].x = static_cast<float> (K[0]);
projected_points.points[i].y = static_cast<float> (K[1]);
projected_points.points[i].z = static_cast<float> (K[2]);
}
}
}
客服电话
APP下载
官方微信
请发表评论