本文整理汇总了C++中eigen::Vector4f类的典型用法代码示例。如果您正苦于以下问题:C++ Vector4f类的具体用法?C++ Vector4f怎么用?C++ Vector4f使用的例子?那么恭喜您, 这里精选的类代码示例或许可以为您提供帮助。
在下文中一共展示了Vector4f类的20个代码示例,这些例子默认根据受欢迎程度排序。您可以为喜欢或者感觉有用的代码点赞,您的评价将有助于我们的系统推荐出更棒的C++代码示例。
示例1:
template <typename PointT> inline void
pcl::getMinMax3D (const pcl::PointCloud<PointT> &cloud, const std::vector<int> &indices,
Eigen::Vector4f &min_pt, Eigen::Vector4f &max_pt)
{
min_pt.setConstant (FLT_MAX);
max_pt.setConstant (-FLT_MAX);
// If the data is dense, we don't need to check for NaN
if (cloud.is_dense)
{
for (size_t i = 0; i < indices.size (); ++i)
{
pcl::Array4fMapConst pt = cloud.points[indices[i]].getArray4fMap ();
min_pt = min_pt.array ().min (pt);
max_pt = max_pt.array ().max (pt);
}
}
// NaN or Inf values could exist => check for them
else
{
for (size_t i = 0; i < indices.size (); ++i)
{
// Check if the point is invalid
if (!pcl_isfinite (cloud.points[indices[i]].x) ||
!pcl_isfinite (cloud.points[indices[i]].y) ||
!pcl_isfinite (cloud.points[indices[i]].z))
continue;
pcl::Array4fMapConst pt = cloud.points[indices[i]].getArray4fMap ();
min_pt = min_pt.array ().min (pt);
max_pt = max_pt.array ().max (pt);
}
}
}
开发者ID:Bastl34,项目名称:PCL,代码行数:33,代码来源:common.hpp
示例2: p
template <typename PointT, typename PointNT> void
pcl::SampleConsensusModelNormalPlane<PointT, PointNT>::getDistancesToModel (
const Eigen::VectorXf &model_coefficients, std::vector<double> &distances)
{
if (!normals_)
{
PCL_ERROR ("[pcl::SampleConsensusModelNormalPlane::getDistancesToModel] No input dataset containing normals was given!\n");
return;
}
// Check if the model is valid given the user constraints
if (!isModelValid (model_coefficients))
{
distances.clear ();
return;
}
// Obtain the plane normal
Eigen::Vector4f coeff = model_coefficients;
coeff[3] = 0;
distances.resize (indices_->size ());
// Iterate through the 3d points and calculate the distances from them to the plane
for (size_t i = 0; i < indices_->size (); ++i)
{
// Calculate the distance from the point to the plane normal as the dot product
// D = (P-A).N/|N|
Eigen::Vector4f p (input_->points[(*indices_)[i]].x, input_->points[(*indices_)[i]].y, input_->points[(*indices_)[i]].z, 0);
Eigen::Vector4f n (normals_->points[(*indices_)[i]].normal[0], normals_->points[(*indices_)[i]].normal[1], normals_->points[(*indices_)[i]].normal[2], 0);
double d_euclid = fabs (coeff.dot (p) + model_coefficients[3]);
// Calculate the angular distance between the point normal and the plane normal
double d_normal = fabs (getAngle3D (n, coeff));
d_normal = (std::min) (d_normal, M_PI - d_normal);
distances[i] = fabs (normal_distance_weight_ * d_normal + (1 - normal_distance_weight_) * d_euclid);
}
}
开发者ID:Bardo91,项目名称:pcl,代码行数:39,代码来源:sac_model_normal_plane.hpp
示例3:
template <typename PointT> void
pcl::MaximumLikelihoodSampleConsensus<PointT>::getMinMax (
const PointCloudConstPtr &cloud,
const boost::shared_ptr <std::vector<int> > &indices,
Eigen::Vector4f &min_p,
Eigen::Vector4f &max_p)
{
min_p.setConstant (FLT_MAX);
max_p.setConstant (-FLT_MAX);
min_p[3] = max_p[3] = 0;
for (size_t i = 0; i < indices->size (); ++i)
{
if (cloud->points[(*indices)[i]].x < min_p[0]) min_p[0] = cloud->points[(*indices)[i]].x;
if (cloud->points[(*indices)[i]].y < min_p[1]) min_p[1] = cloud->points[(*indices)[i]].y;
if (cloud->points[(*indices)[i]].z < min_p[2]) min_p[2] = cloud->points[(*indices)[i]].z;
if (cloud->points[(*indices)[i]].x > max_p[0]) max_p[0] = cloud->points[(*indices)[i]].x;
if (cloud->points[(*indices)[i]].y > max_p[1]) max_p[1] = cloud->points[(*indices)[i]].y;
if (cloud->points[(*indices)[i]].z > max_p[2]) max_p[2] = cloud->points[(*indices)[i]].z;
}
}
开发者ID:9gel,项目名称:hellopcl,代码行数:22,代码来源:mlesac.hpp
示例4: return
template <typename PointT, typename PointNT> int
pcl::SampleConsensusModelCylinder<PointT, PointNT>::countWithinDistance (
const Eigen::VectorXf &model_coefficients, const double threshold)
{
// Check if the model is valid given the user constraints
if (!isModelValid (model_coefficients))
return (0);
int nr_p = 0;
Eigen::Vector4f line_pt (model_coefficients[0], model_coefficients[1], model_coefficients[2], 0);
Eigen::Vector4f line_dir (model_coefficients[3], model_coefficients[4], model_coefficients[5], 0);
float ptdotdir = line_pt.dot (line_dir);
float dirdotdir = 1.0f / line_dir.dot (line_dir);
// Iterate through the 3d points and calculate the distances from them to the sphere
for (size_t i = 0; i < indices_->size (); ++i)
{
// Aproximate the distance from the point to the cylinder as the difference between
// dist(point,cylinder_axis) and cylinder radius
Eigen::Vector4f pt (input_->points[(*indices_)[i]].x, input_->points[(*indices_)[i]].y, input_->points[(*indices_)[i]].z, 0);
Eigen::Vector4f n (normals_->points[(*indices_)[i]].normal[0], normals_->points[(*indices_)[i]].normal[1], normals_->points[(*indices_)[i]].normal[2], 0);
double d_euclid = fabs (pointToLineDistance (pt, model_coefficients) - model_coefficients[6]);
// Calculate the point's projection on the cylinder axis
float k = (pt.dot (line_dir) - ptdotdir) * dirdotdir;
Eigen::Vector4f pt_proj = line_pt + k * line_dir;
Eigen::Vector4f dir = pt - pt_proj;
dir.normalize ();
// Calculate the angular distance between the point normal and the (dir=pt_proj->pt) vector
double d_normal = fabs (getAngle3D (n, dir));
d_normal = (std::min) (d_normal, M_PI - d_normal);
if (fabs (normal_distance_weight_ * d_normal + (1 - normal_distance_weight_) * d_euclid) < threshold)
nr_p++;
}
return (nr_p);
}
开发者ID:Bastl34,项目名称:PCL,代码行数:38,代码来源:sac_model_cylinder.hpp
示例5: fabs
//TODO: move to semantics
void
PlaneExtraction::findClosestTable(std::vector<pcl::PointCloud<Point>, Eigen::aligned_allocator<pcl::PointCloud<Point> > >& v_cloud_hull,
std::vector<pcl::ModelCoefficients>& v_coefficients_plane,
Eigen::Vector3f& robot_pose,
unsigned int& idx)
{
std::vector<unsigned int> table_candidates;
for(unsigned int i=0; i<v_cloud_hull.size(); i++)
{
//if(fabs(v_coefficients_plane[i].values[3])>0.5 && fabs(v_coefficients_plane[i].values[3])<1.2)
table_candidates.push_back(i);
}
if(table_candidates.size()>0)
{
for(unsigned int i=0; i<table_candidates.size(); i++)
{
double d_min = 1000;
double d = d_min;
Eigen::Vector4f centroid;
pcl::compute3DCentroid(v_cloud_hull[i], centroid);
//for(unsigned int j=0; j<v_cloud_hull[i].size(); j++)
//{
// Eigen::Vector3f p = v_cloud_hull[i].points[j].getVector3fMap();
// d += fabs((p-robot_pose).norm());
//}
//d /= v_cloud_hull[i].size();
Eigen::Vector3f centroid3 = centroid.head(3);
d = fabs((centroid3-robot_pose).norm());
ROS_INFO("d: %f", d);
if(d<d_min)
{
d_min = d;
idx = table_candidates[i];
}
}
}
}
开发者ID:Etimr,项目名称:cob_environment_perception,代码行数:38,代码来源:plane_extraction.cpp
示例6: calc_quaternion_average
geometry_msgs::Quaternion calc_quaternion_average( std::vector<geometry_msgs::Pose> pose_vector ){
Eigen::Matrix4f averaging_matrix;
Eigen::Vector4f quaternion;
averaging_matrix.setZero();
for( unsigned int sample_ii=0; sample_ii<pose_vector.size(); sample_ii++){
quaternion(0) = pose_vector[sample_ii].orientation.x;
quaternion(1) = pose_vector[sample_ii].orientation.y;
quaternion(2) = pose_vector[sample_ii].orientation.z;
quaternion(3) = pose_vector[sample_ii].orientation.w;
averaging_matrix = averaging_matrix + quaternion*quaternion.transpose();
}// for all samples
Eigen::EigenSolver<Eigen::Matrix4f> ev_solver;
ev_solver.compute( averaging_matrix);
Eigen::Vector4cf eigen_values = ev_solver.eigenvalues();
float max_ev=eigen_values(0).real();
unsigned int max_ii = 0;
for( unsigned int ev_ii=1; ev_ii<4; ev_ii++){
if( eigen_values(ev_ii).real()>max_ev ){
max_ev = eigen_values(ev_ii).real();
max_ii=ev_ii;
}
}
Eigen::Vector4f eigen_vector = ev_solver.eigenvectors().col(max_ii).real();
eigen_vector.normalize();
geometry_msgs::Quaternion quaternion_msg;
quaternion_msg.x = eigen_vector(0);
quaternion_msg.y = eigen_vector(1);
quaternion_msg.z = eigen_vector(2);
quaternion_msg.w = eigen_vector(3);
return quaternion_msg;
}
开发者ID:jondo,项目名称:omnirob_robin,代码行数:37,代码来源:marker_calibration.cpp
示例7: return
template <typename PointT, typename PointNT> int
pcl::SampleConsensusModelCone<PointT, PointNT>::countWithinDistance (
const Eigen::VectorXf &model_coefficients, const double threshold)
{
// Check if the model is valid given the user constraints
if (!isModelValid (model_coefficients))
return (0);
int nr_p = 0;
Eigen::Vector4f apex (model_coefficients[0], model_coefficients[1], model_coefficients[2], 0);
Eigen::Vector4f axis_dir (model_coefficients[3], model_coefficients[4], model_coefficients[5], 0);
float opening_angle = model_coefficients[6];
float apexdotdir = apex.dot (axis_dir);
float dirdotdir = 1.0f / axis_dir.dot (axis_dir);
// Iterate through the 3d points and calculate the distances from them to the cone
for (size_t i = 0; i < indices_->size (); ++i)
{
Eigen::Vector4f pt (input_->points[(*indices_)[i]].x, input_->points[(*indices_)[i]].y, input_->points[(*indices_)[i]].z, 0);
Eigen::Vector4f n (normals_->points[(*indices_)[i]].normal[0], normals_->points[(*indices_)[i]].normal[1], normals_->points[(*indices_)[i]].normal[2], 0);
// Calculate the point's projection on the cone axis
float k = (pt.dot (axis_dir) - apexdotdir) * dirdotdir;
Eigen::Vector4f pt_proj = apex + k * axis_dir;
// Calculate the direction of the point from center
Eigen::Vector4f pp_pt_dir = pt - pt_proj;
pp_pt_dir.normalize ();
// Calculate the actual radius of the cone at the level of the projected point
Eigen::Vector4f height = apex - pt_proj;
double actual_cone_radius = tan(opening_angle) * height.norm ();
height.normalize ();
// Calculate the cones perfect normals
Eigen::Vector4f cone_normal = sinf (opening_angle) * height + cosf (opening_angle) * pp_pt_dir;
// Aproximate the distance from the point to the cone as the difference between
// dist(point,cone_axis) and actual cone radius
double d_euclid = fabs (pointToAxisDistance (pt, model_coefficients) - actual_cone_radius);
// Calculate the angular distance between the point normal and the (dir=pt_proj->pt) vector
double d_normal = fabs (getAngle3D (n, cone_normal));
d_normal = (std::min) (d_normal, M_PI - d_normal);
if (fabs (normal_distance_weight_ * d_normal + (1 - normal_distance_weight_) * d_euclid) < threshold)
nr_p++;
}
return (nr_p);
}
开发者ID:liangdu,项目名称:pcl,代码行数:52,代码来源:sac_model_cone.hpp
示例8: exit
template <typename PointInT> double
pcl::tracking::NormalCoherence<PointInT>::computeCoherence (PointInT &source, PointInT &target)
{
Eigen::Vector4f n = source.getNormalVector4fMap ();
Eigen::Vector4f n_dash = target.getNormalVector4fMap ();
if ( n.norm () <= 1e-5 || n_dash.norm () <= 1e-5 )
{
PCL_ERROR("norm might be ZERO!\n");
std::cout << "source: " << source << std::endl;
std::cout << "target: " << target << std::endl;
exit (1);
return 0.0;
}
else
{
n.normalize ();
n_dash.normalize ();
double theta = pcl::getAngle3D (n, n_dash);
if (!pcl_isnan (theta))
return 1.0 / (1.0 + weight_ * theta * theta);
else
return 0.0;
}
}
开发者ID:Bardo91,项目名称:pcl,代码行数:24,代码来源:normal_coherence.hpp
示例9: return
vtkSmartPointer<vtkDataSet>
pcl::visualization::createLine (const Eigen::Vector4f &pt1, const Eigen::Vector4f &pt2)
{
vtkSmartPointer<vtkLineSource> line = vtkSmartPointer<vtkLineSource>::New ();
line->SetPoint1 (pt1.x (), pt1.y (), pt1.z ());
line->SetPoint2 (pt2.x (), pt2.y (), pt2.z ());
line->Update ();
return (line->GetOutput ());
}
开发者ID:MorS25,项目名称:pcl-fuerte,代码行数:10,代码来源:shapes.cpp
示例10: lineToLineSegment
bool
pcl::lineWithLineIntersection (const Eigen::VectorXf &line_a,
const Eigen::VectorXf &line_b,
Eigen::Vector4f &point, double sqr_eps)
{
Eigen::Vector4f p1, p2;
lineToLineSegment (line_a, line_b, p1, p2);
// If the segment size is smaller than a pre-given epsilon...
double sqr_dist = (p1 - p2).squaredNorm ();
if (sqr_dist < sqr_eps)
{
point = p1;
return (true);
}
point.setZero ();
return (false);
}
开发者ID:BITVoyager,项目名称:pcl,代码行数:18,代码来源:intersections.hpp
示例11: given
template <typename PointT, typename PointNT> bool
pcl::SampleConsensusModelCone<PointT, PointNT>::doSamplesVerifyModel (
const std::set<int> &indices, const Eigen::VectorXf &model_coefficients, const double threshold)
{
// Needs a valid model coefficients
if (model_coefficients.size () != 7)
{
PCL_ERROR ("[pcl::SampleConsensusModelCone::doSamplesVerifyModel] Invalid number of model coefficients given (%lu)!\n", model_coefficients.size ());
return (false);
}
Eigen::Vector4f apex (model_coefficients[0], model_coefficients[1], model_coefficients[2], 0);
Eigen::Vector4f axis_dir (model_coefficients[3], model_coefficients[4], model_coefficients[5], 0);
float openning_angle = model_coefficients[6];
float apexdotdir = apex.dot (axis_dir);
float dirdotdir = 1.0f / axis_dir.dot (axis_dir);
// Iterate through the 3d points and calculate the distances from them to the cone
for (std::set<int>::const_iterator it = indices.begin (); it != indices.end (); ++it)
{
Eigen::Vector4f pt (input_->points[*it].x, input_->points[*it].y, input_->points[*it].z, 0);
// Calculate the point's projection on the cone axis
float k = (pt.dot (axis_dir) - apexdotdir) * dirdotdir;
Eigen::Vector4f pt_proj = apex + k * axis_dir;
Eigen::Vector4f dir = pt - pt_proj;
dir.normalize ();
// Calculate the actual radius of the cone at the level of the projected point
Eigen::Vector4f height = apex - pt_proj;
double actual_cone_radius = tan (openning_angle) * height.norm ();
// Aproximate the distance from the point to the cone as the difference between
// dist(point,cone_axis) and actual cone radius
if (fabs (static_cast<double>(pointToAxisDistance (pt, model_coefficients) - actual_cone_radius)) > threshold)
return (false);
}
return (true);
}
开发者ID:liangdu,项目名称:pcl,代码行数:41,代码来源:sac_model_cone.hpp
示例12: return
template <typename PointT> inline unsigned int
pcl::compute3DCentroid (const pcl::PointCloud<PointT> &cloud, Eigen::Vector4f ¢roid)
{
if (cloud.points.empty ())
return (0);
// Initialize to 0
centroid.setZero ();
// For each point in the cloud
// If the data is dense, we don't need to check for NaN
if (cloud.is_dense)
{
for (size_t i = 0; i < cloud.points.size (); ++i)
centroid += cloud.points[i].getVector4fMap ();
centroid[3] = 0;
centroid /= static_cast<float> (cloud.points.size ());
return (static_cast<unsigned int> (cloud.points.size ()));
}
// NaN or Inf values could exist => check for them
else
{
unsigned cp = 0;
for (size_t i = 0; i < cloud.points.size (); ++i)
{
// Check if the point is invalid
if (!isFinite (cloud [i]))
continue;
centroid += cloud[i].getVector4fMap ();
++cp;
}
centroid[3] = 0;
centroid /= static_cast<float> (cp);
return (cp);
}
}
开发者ID:9gel,项目名称:hellopcl,代码行数:38,代码来源:centroid.hpp
示例13:
template <typename PointT> inline void
pcl::compute3DCentroid (const pcl::PointCloud<PointT> &cloud, const std::vector<int> &indices,
Eigen::Vector4f ¢roid)
{
// Initialize to 0
centroid.setZero ();
if (indices.empty ())
return;
// For each point in the cloud
int cp = 0;
// If the data is dense, we don't need to check for NaN
if (cloud.is_dense)
{
for (size_t i = 0; i < indices.size (); ++i)
centroid += cloud.points[indices[i]].getVector4fMap ();
centroid[3] = 0;
centroid /= indices.size ();
}
// NaN or Inf values could exist => check for them
else
{
for (size_t i = 0; i < indices.size (); ++i)
{
// Check if the point is invalid
if (!pcl_isfinite (cloud.points[indices[i]].x) ||
!pcl_isfinite (cloud.points[indices[i]].y) ||
!pcl_isfinite (cloud.points[indices[i]].z))
continue;
centroid += cloud.points[indices[i]].getVector4fMap ();
cp++;
}
centroid[3] = 0;
centroid /= cp;
}
}
开发者ID:jonaswitt,项目名称:nestk,代码行数:37,代码来源:feature.hpp
示例14: cloud_filtered_
template<typename PointType> void
pcl::apps::DominantPlaneSegmentation<PointType>::compute_table_plane ()
{
// Has the input dataset been set already?
if (!input_)
{
PCL_WARN ("[pcl::apps::DominantPlaneSegmentation] No input dataset given!\n");
return;
}
CloudConstPtr cloud_;
CloudPtr cloud_filtered_ (new Cloud ());
CloudPtr cloud_downsampled_ (new Cloud ());
pcl::PointCloud<pcl::Normal>::Ptr cloud_normals_ (new pcl::PointCloud<pcl::Normal> ());
pcl::PointIndices::Ptr table_inliers_ (new pcl::PointIndices ());
pcl::ModelCoefficients::Ptr table_coefficients_ (new pcl::ModelCoefficients ());
CloudPtr table_projected_ (new Cloud ());
CloudPtr table_hull_ (new Cloud ());
typename pcl::search::KdTree<PointType>::Ptr normals_tree_ (new pcl::search::KdTree<PointType>);
// Normal estimation parameters
n3d_.setKSearch (k_);
n3d_.setSearchMethod (normals_tree_);
// Table model fitting parameters
seg_.setDistanceThreshold (sac_distance_threshold_);
seg_.setMaxIterations (2000);
seg_.setNormalDistanceWeight (0.1);
seg_.setOptimizeCoefficients (true);
seg_.setModelType (pcl::SACMODEL_NORMAL_PLANE);
seg_.setMethodType (pcl::SAC_RANSAC);
seg_.setProbability (0.99);
proj_.setModelType (pcl::SACMODEL_NORMAL_PLANE);
bb_cluster_proj_.setModelType (pcl::SACMODEL_NORMAL_PLANE);
// ---[ PassThroughFilter
pass_.setFilterLimits (min_z_bounds_, max_z_bounds_);
pass_.setFilterFieldName ("z");
pass_.setInputCloud (input_);
pass_.filter (*cloud_filtered_);
if (int (cloud_filtered_->points.size ()) < k_)
{
PCL_WARN ("[DominantPlaneSegmentation] Filtering returned %zu points! Aborting.",
cloud_filtered_->points.size ());
return;
}
// Downsample the point cloud
grid_.setLeafSize (downsample_leaf_, downsample_leaf_, downsample_leaf_);
grid_.setDownsampleAllData (false);
grid_.setInputCloud (cloud_filtered_);
grid_.filter (*cloud_downsampled_);
// ---[ Estimate the point normals
n3d_.setInputCloud (cloud_downsampled_);
n3d_.compute (*cloud_normals_);
// ---[ Perform segmentation
seg_.setInputCloud (cloud_downsampled_);
seg_.setInputNormals (cloud_normals_);
seg_.segment (*table_inliers_, *table_coefficients_);
if (table_inliers_->indices.size () == 0)
{
PCL_WARN ("[DominantPlaneSegmentation] No Plane Inliers points! Aborting.");
return;
}
// ---[ Extract the plane
proj_.setInputCloud (cloud_downsampled_);
proj_.setIndices (table_inliers_);
proj_.setModelCoefficients (table_coefficients_);
proj_.filter (*table_projected_);
// ---[ Estimate the convex hull
std::vector<pcl::Vertices> polygons;
CloudPtr table_hull (new Cloud ());
hull_.setInputCloud (table_projected_);
hull_.reconstruct (*table_hull, polygons);
// Compute the plane coefficients
Eigen::Vector4f model_coefficients;
EIGEN_ALIGN16 Eigen::Matrix3f covariance_matrix;
model_coefficients[0] = table_coefficients_->values[0];
model_coefficients[1] = table_coefficients_->values[1];
model_coefficients[2] = table_coefficients_->values[2];
model_coefficients[3] = table_coefficients_->values[3];
// Need to flip the plane normal towards the viewpoint
Eigen::Vector4f vp (0, 0, 0, 0);
// See if we need to flip any plane normals
vp -= table_hull->points[0].getVector4fMap ();
vp[3] = 0;
// Dot product between the (viewpoint - point) and the plane normal
float cos_theta = vp.dot (model_coefficients);
// Flip the plane normal
//.........这里部分代码省略.........
开发者ID:Bastl34,项目名称:PCL,代码行数:101,代码来源:dominant_plane_segmentation.hpp
示例15: cloud_cb
void cloud_cb (const sensor_msgs::PointCloud2ConstPtr& input)
{
// ... do data processing
sensor_msgs::PointCloud2 output;
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr clustered_cloud (new pcl::PointCloud<pcl::PointXYZ>);
pcl::fromROSMsg (*input, *cloud);
// Creating the KdTree object for the search method of the extraction
pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ>);
tree->setInputCloud (cloud);
std::vector<pcl::PointIndices> cluster_indices;
pcl::EuclideanClusterExtraction<pcl::PointXYZ> ec;
ec.setClusterTolerance (0.10); // 2cm
ec.setMinClusterSize (20);
ec.setMaxClusterSize (2500);
ec.setSearchMethod (tree);
ec.setInputCloud(cloud);
ec.extract (cluster_indices);
ROS_INFO("cluster_indices has %d data points.", (int) cluster_indices.size());
ROS_INFO("cloud has %d data points.", (int) cloud->points.size());
visualization_msgs::Marker marker;
marker.header = cloud->header;
marker.id = 1;
marker.type = visualization_msgs::Marker::CUBE_LIST;
marker.action = visualization_msgs::Marker::ADD;
marker.color.r = 1;
marker.color.g = 0;
marker.color.b = 0;
marker.color.a = 0.7;
marker.scale.x = 0.2;
marker.scale.y = 0.2;
marker.scale.z = 0.2;
marker.lifetime = ros::Duration(60.0);
Eigen::Vector4f minPoint;
Eigen::Vector4f maxPoint;
// pcl::getMinMax3D(*cloud, minPoint, maxPoint);
for (std::vector<pcl::PointIndices>::const_iterator it = cluster_indices.begin (); it != cluster_indices.end (); ++it)
{
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_cluster (new pcl::PointCloud<pcl::PointXYZ>);
for (std::vector<int>::const_iterator pit = it->indices.begin (); pit != it->indices.end (); pit++)
cloud_cluster->points.push_back (cloud->points[*pit]);
std::cout << "PointCloud representing the Cluster: " << cloud_cluster->points.size () << " data points." << std::endl;
// Merge current clusters to whole point cloud
*clustered_cloud += *cloud_cluster;
// for(size_t j = 0; j < cloud_cluster->points.size() - 1; j++)
// {
/*
geometry_msgs::Point pt1;
pt1.x = cloud_cluster->points[j].x;
pt1.y = cloud_cluster->points[j].y;
pt1.z = cloud_cluster->points[j].z;
geometry_msgs::Point pt2;
pt2.x = cloud_cluster->points[j+1].x;
pt2.y = cloud_cluster->points[j+1].y;
pt2.z = cloud_cluster->points[j+1].z;
marker.points.push_back(pt1);
marker.points.push_back(pt2);
*/
//Seg for top of prism
geometry_msgs::Point pt3;
pt3.x = 0.0;
pt3.y = 0.0;
pt3.z = 0.0;
std_msgs::ColorRGBA colors;
colors.r = 0.0;
colors.g = 0.0;
colors.b = 0.0;
for(size_t i=0; i<cloud_cluster->points.size(); i++)
{
pt3.x += cloud_cluster->points[i].x;
pt3.y += cloud_cluster->points[i].y;
pt3.z += cloud_cluster->points[i].z;
}
pt3.x /= cloud_cluster->points.size();
pt3.y /= cloud_cluster->points.size();
pt3.z /= cloud_cluster->points.size();
pcl::getMinMax3D(*cloud_cluster, minPoint, maxPoint);
marker.scale.y= maxPoint.y();
//marker.scale.x= maxPoint.x();
//marker.scale.z= maxPoint.z();
marker.scale.x= maxPoint.x()-minPoint.x();
colors.r = marker.scale.x;
// colors.g = marker.scale.y;
//marker.scale.z= maxPoint.z()-minPoint.z();
//pt3.z = maxPoint.z();
//geometry_msgs::Point pt4;
//pt4.x = cloud_cluster->points[j+1].x;
//pt4.y = cloud_cluster->points[j+1].y;
//pt4.z = cloud_cluster->points[j+1].z;
//pt4.z = maxPoint.z();
//.........这里部分代码省略.........
开发者ID:Jae-hyun,项目名称:beginner_tutorials,代码行数:101,代码来源:pcl_euclideanClusterExtraction.cpp
示例16: viewpoint
template <typename PointInT, typename PointNT, typename PointOutT> void
pcl::VFHEstimation<PointInT, PointNT, PointOutT>::computeFeature (PointCloudOut &output)
{
// ---[ Step 1a : compute the centroid in XYZ space
Eigen::Vector4f xyz_centroid;
if (use_given_centroid_)
xyz_centroid = centroid_to_use_;
else
compute3DCentroid (*surface_, *indices_, xyz_centroid); // Estimate the XYZ centroid
// ---[ Step 1b : compute the centroid in normal space
Eigen::Vector4f normal_centroid = Eigen::Vector4f::Zero ();
int cp = 0;
// If the data is dense, we don't need to check for NaN
if (use_given_normal_)
normal_centroid = normal_to_use_;
else
{
if (normals_->is_dense)
{
for (size_t i = 0; i < indices_->size (); ++i)
{
normal_centroid += normals_->points[(*indices_)[i]].getNormalVector4fMap ();
cp++;
}
}
// NaN or Inf values could exist => check for them
else
{
for (size_t i = 0; i < indices_->size (); ++i)
{
if (!pcl_isfinite (normals_->points[(*indices_)[i]].normal[0])
||
!pcl_isfinite (normals_->points[(*indices_)[i]].normal[1])
||
!pcl_isfinite (normals_->points[(*indices_)[i]].normal[2]))
continue;
normal_centroid += normals_->points[(*indices_)[i]].getNormalVector4fMap ();
cp++;
}
}
normal_centroid /= cp;
}
// Compute the direction of view from the viewpoint to the centroid
Eigen::Vector4f viewpoint (vpx_, vpy_, vpz_, 0);
Eigen::Vector4f d_vp_p = viewpoint - xyz_centroid;
d_vp_p.normalize ();
// Estimate the SPFH at nn_indices[0] using the entire cloud
computePointSPFHSignature (xyz_centroid, normal_centroid, *surface_, *normals_, *indices_);
// We only output _1_ signature
output.points.resize (1);
output.width = 1;
output.height = 1;
// Estimate the FPFH at nn_indices[0] using the entire cloud and copy the resultant signature
for (int d = 0; d < hist_f1_.size (); ++d)
output.points[0].histogram[d + 0] = hist_f1_[d];
size_t data_size = hist_f1_.size ();
for (int d = 0; d < hist_f2_.size (); ++d)
output.points[0].histogram[d + data_size] = hist_f2_[d];
data_size += hist_f2_.size ();
for (int d = 0; d < hist_f3_.size (); ++d)
output.points[0].histogram[d + data_size] = hist_f3_[d];
data_size += hist_f3_.size ();
for (int d = 0; d < hist_f4_.size (); ++d)
output.points[0].histogram[d + data_size] = hist_f4_[d];
// ---[ Step 2 : obtain the viewpoint component
hist_vp_.setZero (nr_bins_vp_);
double hist_incr;
if (normalize_bins_)
hist_incr = 100.0 / (double)(indices_->size ());
else
hist_incr = 1.0;
for (size_t i = 0; i < indices_->size (); ++i)
{
Eigen::Vector4f normal (normals_->points[(*indices_)[i]].normal[0],
normals_->points[(*indices_)[i]].normal[1],
normals_->points[(*indices_)[i]].normal[2], 0);
// Normalize
double alpha = (normal.dot (d_vp_p) + 1.0) * 0.5;
int fi = floor (alpha * hist_vp_.size ());
if (fi < 0)
fi = 0;
if (fi > ((int)hist_vp_.size () - 1))
fi = hist_vp_.size () - 1;
// Bin into the histogram
hist_vp_ [fi] += hist_incr;
}
//.........这里部分代码省略.........
开发者ID:gimlids,项目名称:BodyScanner,代码行数:101,代码来源:vfh.hpp
示例17: cloud
template<typename PointT, typename PointNT, typename PointLT> void
pcl::OrganizedMultiPlaneSegmentation<PointT, PointNT, PointLT>::segment (std::vector<ModelCoefficients>& model_coefficients,
std::vector<PointIndices>& inlier_indices,
std::vector<Eigen::Vector4f, Eigen::aligned_allocator<Eigen::Vector4f> >& centroids,
std::vector <Eigen::Matrix3f, Eigen::aligned_allocator<Eigen::Matrix3f> >& covariances,
pcl::PointCloud<PointLT>& labels,
std::vector<pcl::PointIndices>& label_indices)
{
if (!initCompute ())
return;
// Check that we got the same number of points and normals
if (static_cast<int> (normals_->points.size ()) != static_cast<int> (input_->points.size ()))
{
PCL_ERROR ("[pcl::%s::segment] Number of points in input cloud (%zu) and normal cloud (%zu) do not match!\n",
getClassName ().c_str (), input_->points.size (),
normals_->points.size ());
return;
}
// Check that the cloud is organized
if (!input_->isOrganized ())
{
PCL_ERROR ("[pcl::%s::segment] Organized point cloud is required for this plane extraction method!\n",
getClassName ().c_str ());
return;
}
// Calculate range part of planes' hessian normal form
std::vector<float> plane_d (input_->points.size ());
for (unsigned int i = 0; i < input_->size (); ++i)
plane_d[i] = input_->points[i].getVector3fMap ().dot (normals_->points[i].getNormalVector3fMap ());
// Make a comparator
//PlaneCoefficientComparator<PointT,PointNT> plane_comparator (plane_d);
compare_->setPlaneCoeffD (plane_d);
compare_->setInputCloud (input_);
compare_->setInputNormals (normals_);
compare_->setAngularThreshold (static_cast<float> (angular_threshold_));
compare_->setDistanceThreshold (static_cast<float> (distance_threshold_), true);
// Set up the output
OrganizedConnectedComponentSegmentation<PointT,pcl::Label> connected_component (compare_);
connected_component.setInputCloud (input_);
connected_component.segment (labels, label_indices);
Eigen::Vector4f clust_centroid = Eigen::Vector4f::Zero ();
Eigen::Vector4f vp = Eigen::Vector4f::Zero ();
Eigen::Matrix3f clust_cov;
pcl::ModelCoefficients model;
model.values.resize (4);
// Fit Planes to each cluster
for (size_t i = 0; i < label_indices.size (); i++)
{
if (static_cast<unsigned> (label_indices[i].indices.size ()) > min_inliers_)
{
pcl::computeMeanAndCovarianceMatrix (*input_, label_indices[i].indices, clust_cov, clust_centroid);
Eigen::Vector4f plane_params;
EIGEN_ALIGN16 Eigen::Vector3f::Scalar eigen_value;
EIGEN_ALIGN16 Eigen::Vector3f eigen_vector;
pcl::eigen33 (clust_cov, eigen_value, eigen_vector);
plane_params[0] = eigen_vector[0];
plane_params[1] = eigen_vector[1];
plane_params[2] = eigen_vector[2];
plane_params[3] = 0;
plane_params[3] = -1 * plane_params.dot (clust_centroid);
vp -= clust_centroid;
float cos_theta = vp.dot (plane_params);
if (cos_theta < 0)
{
plane_params *= -1;
plane_params[3] = 0;
plane_params[3] = -1 * plane_params.dot (clust_centroid);
}
// Compute the curvature surface change
float curvature;
float eig_sum = clust_cov.coeff (0) + clust_cov.coeff (4) + clust_cov.coeff (8);
if (eig_sum != 0)
curvature = fabsf (eigen_value / eig_sum);
else
curvature = 0;
if (curvature < maximum_curvature_)
{
model.values[0] = plane_params[0];
model.values[1] = plane_params[1];
model.values[2] = plane_params[2];
model.values[3] = plane_params[3];
model_coefficients.push_back (model);
inlier_indices.push_back (label_indices[i]);
centroids.push_back (clust_centroid);
covariances.push_back (clust_cov);
}
}
}
//.........这里部分代码省略.........
开发者ID:hitsjt,项目名称:StanfordPCL,代码行数:101,代码来源:organized_multi_plane_segmentation.hpp
示例18: checkCloud
bool checkCloud(const sensor_msgs::PointCloud2& cloud_msg,
typename pcl::PointCloud<T>::Ptr hand_cloud,
//typename pcl::PointCloud<T>::Ptr finger_cloud,
const std::string& frame_id,
tf::Vector3& hand_position,
tf::Vector3& arm_position,
tf::Vector3& arm_direction)
{
typename pcl::PointCloud<T>::Ptr cloud(new pcl::PointCloud<T>);
pcl::fromROSMsg(cloud_msg, *cloud);
if((cloud->points.size() < g_config.min_cluster_size) ||
(cloud->points.size() > g_config.max_cluster_size))
return false;
pcl::PCA<T> pca;
pca.setInputCloud(cloud);
Eigen::Vector4f mean = pca.getMean();
if((mean.coeff(0) < g_config.min_x) || (mean.coeff(0) > g_config.max_x)) return false;
if((mean.coeff(1) < g_config.min_y) || (mean.coeff(1) > g_config.max_y)) return false;
if((mean.coeff(2) < g_config.min_z) || (mean.coeff(2) > g_config.max_z)) return false;
Eigen::Vector3f eigen_value = pca.getEigenValues();
double ratio = eigen_value.coeff(0) / eigen_value.coeff(1);
if((ratio < g_config.min_eigen_value_ratio) || (ratio > g_config.max_eigen_value_ratio)) return false;
T search_point;
Eigen::Matrix3f ev = pca.getEigenVectors();
Eigen::Vector3f main_axis(ev.coeff(0, 0), ev.coeff(1, 0), ev.coeff(2, 0));
main_axis.normalize();
arm_direction.setX(main_axis.coeff(0));
arm_direction.setY(main_axis.coeff(1));
arm_direction.setZ(main_axis.coeff(2));
arm_position.setX(mean.coeff(0));
arm_position.setY(mean.coeff(1));
arm_position.setZ(mean.coeff(2));
main_axis = (-main_axis * 0.3) + Eigen::Vector3f(mean.coeff(0), mean.coeff(1), mean.coeff(2));
search_point.x = main_axis.coeff(0);
search_point.y = main_axis.coeff(1);
search_point.z = main_axis.coeff(2);
//find hand
pcl::KdTreeFLANN<T> kdtree;
kdtree.setInputCloud(cloud);
//find the closet point from the serach_point
std::vector<int> point_indeices(1);
std::vector<float> point_distances(1);
if ( kdtree.nearestKSearch (search_point, 1, point_indeices, point_distances) > 0 )
{
//update search point
search_point = cloud->points[point_indeices[0]];
//show seach point
if(g_marker_array_pub.getNumSubscribers() != 0)
{
pushSimpleMarker(search_point.x, search_point.y, search_point.z,
1.0, 0, 0,
0.02,
g_marker_id, g_marker_array, frame_id);
}
//hand
point_indeices.clear();
point_distances.clear();
kdtree.radiusSearch(search_point, g_config.hand_lenght, point_indeices, point_distances);
for (size_t i = 0; i < point_indeices.size (); ++i)
{
hand_cloud->points.push_back(cloud->points[point_indeices[i]]);
hand_cloud->points.back().r = 255;
hand_cloud->points.back().g = 0;
hand_cloud->points.back().b = 0;
}
Eigen::Vector4f centroid;
pcl::compute3DCentroid(*hand_cloud, centroid);
if(g_marker_array_pub.getNumSubscribers() != 0)
{
pushSimpleMarker(centroid.coeff(0), centroid.coeff(1), centroid.coeff(2),
0.0, 1.0, 0,
0.02,
g_marker_id, g_marker_array, frame_id);
}
hand_position.setX(centroid.coeff(0));
hand_position.setY(centroid.coeff(1));
hand_position.setZ(centroid.coeff(2));
#if 0
//fingers
search_point.x = centroid.coeff(0);
search_point.y = centroid.coeff(1);
search_point.z = centroid.coeff(2);
std::vector<int> point_indeices_inner;
//.........这里部分代码省略.........
开发者ID:hiveground-ros-package,项目名称:hiveground_image_pipeline,代码行数:101,代码来源:pcl_hand_arm_detector.cpp
示例19: compute
int LoadPCD::compute()
{
//for each selected filename
for (int k = 0; k < m_filenames.size(); ++k)
{
Eigen::Vector4f origin;
Eigen::Quaternionf orientation;
QString filename = m_filenames[k];
boost::shared_ptr<PCLCloud> cloud_ptr_in = loadSensorMessage(filename, origin, orientation);
if (!cloud_ptr_in) //loading failed?
return 0;
PCLCloud::Ptr cloud_ptr;
if (!cloud_ptr_in->is_dense) //data may contain nans. Remove them
{
//now we need to remove nans
pcl::PassThrough<PCLCloud> passFilter;
passFilter.setInputCloud(cloud_ptr_in);
cloud_ptr = PCLCloud::Ptr(new PCLCloud);
passFilter.filter(*cloud_ptr);
}
else
{
cloud_ptr = cloud_ptr_in;
}
//now we construct a ccGBLSensor with these characteristics
ccGBLSensor * sensor = new ccGBLSensor;
// get orientation as rot matrix
Eigen::Matrix3f eigrot = orientation.toRotationMatrix();
// and copy it into a ccGLMatrix
ccGLMatrix ccRot;
for (int i = 0; i < 3; ++i)
{
for (int j = 0; j < 3; ++j)
{
ccRot.getColumn(j)[i] = eigrot(i,j);
}
}
ccRot.getColumn(3)[3] = 1.0;
// now in a format good for CloudComapre
//ccGLMatrix ccRot = ccGLMatrix::FromQuaternion(orientation.coeffs().data());
//ccRot = ccRot.transposed();
ccRot.setTranslation(origin.data());
sensor->setRigidTransformation(ccRot);
sensor->setDeltaPhi(static_cast<PointCoordinateType>(0.05));
sensor->setDeltaTheta(static_cast<PointCoordinateType>(0.05));
sensor->setVisible(true);
//uncertainty to some default
sensor->setUncertainty(static_cast<PointCoordinateType>(0.01));
ccPointCloud* out_cloud = sm2ccConverter(cloud_ptr).getCloud();
if (!out_cloud)
return -31;
sensor->setGraphicScale(out_cloud->getBB().getDiagNorm() / 10);
//do the projection on sensor
ccGenericPointCloud* cloud = ccHObjectCaster::ToGenericPointCloud(out_cloud);
int errorCode;
CCLib::SimpleCloud* projectedCloud = sensor->project(cloud,errorCode,true);
if (projectedCloud)
{
//DGM: we don't use it but we still have to delete it!
delete projectedCloud;
projectedCloud = 0;
}
QString cloud_name = QFileInfo(filename).baseName();
out_cloud->setName(cloud_name);
QFileInfo fi(filename);
QString containerName = Q
|
请发表评论