本文整理汇总了C++中pcl_isfinite函数的典型用法代码示例。如果您正苦于以下问题：C++ pcl_isfinite函数的具体用法？C++ pcl_isfinite怎么用？C++ pcl_isfinite使用的例子？那么恭喜您, 这里精选的函数代码示例或许可以为您提供帮助。
在下文中一共展示了pcl_isfinite函数的15个代码示例，这些例子默认根据受欢迎程度排序。您可以为喜欢或者感觉有用的代码点赞，您的评价将有助于系统推荐出更棒的C++代码示例。
示例1: pt
template <typename PointT, typename Scalar> void
pcl::transformPointCloud (const pcl::PointCloud<PointT> &cloud_in,
const std::vector<int> &indices,
pcl::PointCloud<PointT> &cloud_out,
const Eigen::Transform<Scalar, 3, Eigen::Affine> &transform)
{
size_t npts = indices.size ();
// In order to transform the data, we need to remove NaNs
cloud_out.is_dense = cloud_in.is_dense;
cloud_out.header = cloud_in.header;
cloud_out.width = static_cast<int> (npts);
cloud_out.height = 1;
cloud_out.points.resize (npts);
if (cloud_in.is_dense)
{
// If the dataset is dense, simply transform it!
for (size_t i = 0; i < npts; ++i)
{
// Copy fields first, then transform xyz data
//cloud_out.points[i] = cloud_in.points[indices[i]];
//cloud_out.points[i].getVector3fMap () = transform*cloud_out.points[i].getVector3fMap ();
Eigen::Matrix<Scalar, 3, 1> pt (cloud_in[indices[i]].x, cloud_in[indices[i]].y, cloud_in[indices[i]].z);
cloud_out[i].x = static_cast<float> (transform (0, 0) * pt.coeffRef (0) + transform (0, 1) * pt.coeffRef (1) + transform (0, 2) * pt.coeffRef (2) + transform (0, 3));
cloud_out[i].y = static_cast<float> (transform (1, 0) * pt.coeffRef (0) + transform (1, 1) * pt.coeffRef (1) + transform (1, 2) * pt.coeffRef (2) + transform (1, 3));
cloud_out[i].z = static_cast<float> (transform (2, 0) * pt.coeffRef (0) + transform (2, 1) * pt.coeffRef (1) + transform (2, 2) * pt.coeffRef (2) + transform (2, 3));
}
}
else
{
// Dataset might contain NaNs and Infs, so check for them first,
// otherwise we get errors during the multiplication (?)
for (size_t i = 0; i < npts; ++i)
{
if (!pcl_isfinite (cloud_in.points[indices[i]].x) ||
!pcl_isfinite (cloud_in.points[indices[i]].y) ||
!pcl_isfinite (cloud_in.points[indices[i]].z))
continue;
//cloud_out.points[i] = cloud_in.points[indices[i]];
//cloud_out.points[i].getVector3fMap () = transform*cloud_out.points[i].getVector3fMap ();
Eigen::Matrix<Scalar, 3, 1> pt (cloud_in[indices[i]].x, cloud_in[indices[i]].y, cloud_in[indices[i]].z);
cloud_out[i].x = static_cast<float> (transform (0, 0) * pt.coeffRef (0) + transform (0, 1) * pt.coeffRef (1) + transform (0, 2) * pt.coeffRef (2) + transform (0, 3));
cloud_out[i].y = static_cast<float> (transform (1, 0) * pt.coeffRef (0) + transform (1, 1) * pt.coeffRef (1) + transform (1, 2) * pt.coeffRef (2) + transform (1, 3));
cloud_out[i].z = static_cast<float> (transform (2, 0) * pt.coeffRef (0) + transform (2, 1) * pt.coeffRef (1) + transform (2, 2) * pt.coeffRef (2) + transform (2, 3));
}
}
}示例2:
template<typename PointT> inline void
pcl::getMaxDistance (const pcl::PointCloud<PointT> &cloud, const std::vector<int> &indices,
const Eigen::Vector4f &pivot_pt, Eigen::Vector4f &max_pt)
{
float max_dist = -FLT_MAX;
int max_idx = -1;
float dist;
// 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::Vector4fMapConst pt = cloud.points[indices[i]].getVector4fMap ();
dist = (pivot_pt - pt).norm ();
if (dist > max_dist)
{
max_idx = static_cast<int> (i);
max_dist = dist;
}
}
}
// 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::Vector4fMapConst pt = cloud.points[indices[i]].getVector4fMap ();
dist = (pivot_pt - pt).norm ();
if (dist > max_dist)
{
max_idx = static_cast<int> (i);
max_dist = dist;
}
}
}
max_pt = cloud.points[indices[max_idx]].getVector4fMap ();
}示例3: computeCloudResolution
double Registrator::computeCloudResolution(const pcl::PointCloud<TYPE_Point_Sensor>::ConstPtr &cloud)
{
double res = 0.0;
int n_points = 0;
int nres;
pcl::search::KdTree<TYPE_Point_Sensor> tree;
tree.setInputCloud (cloud);
for (int iii=0; iii<cloud->size(); ++iii)
{
///////////////////////////////////////////////////////
///////////////////////////////////////////////////////
if ( !pcl_isfinite( (*cloud)[iii].x ) || /////////
!pcl_isfinite( (*cloud)[iii].y ) || /////////
!pcl_isfinite( (*cloud)[iii].z ) ) continue;
///////////////////////////////////////////////////////
///////////////////////////////////////////////////////
//std::cout << "computeCloudResolution - TYPE_Point_Sensor" << "\n"
// << (*cloud)[iii].x << "\t\t"
// << (*cloud)[iii].y << "\t\t"
// << (*cloud)[iii].z << "\t\t"
// << std::endl;
std::vector<int> indices( 2 ); // dummy
std::vector<float> sqr_distances( 2 );
//Considering the second neighbor since the first is the point itself !!!
nres = tree.nearestKSearch(iii, 2, indices, sqr_distances);
if (nres == 2)
{
res += sqrt (sqr_distances[1]);
++n_points;
}
}
//////////////////////////////////////
if (n_points != 0) res /= n_points;
//////////////////////////////////////
return res;
}开发者ID:caomw，项目名称:InHandScanningICCV15_Reconstruction，代码行数:44，代码来源:registrator_RegNEW_x_CloudResolution.cpp
示例4: int
template<typename PointT> inline void
pcl::getMaxDistance (const pcl::PointCloud<PointT> &cloud, const Eigen::Vector4f &pivot_pt, Eigen::Vector4f &max_pt)
{
float max_dist = -FLT_MAX;
int max_idx = -1;
float dist;
// 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)
{
pcl::Vector4fMapConst pt = cloud.points[i].getVector4fMap ();
dist = (pivot_pt - pt).norm ();
if (dist > max_dist)
{
max_idx = int (i);
max_dist = dist;
}
}
}
// NaN or Inf values could exist => check for them
else
{
for (size_t i = 0; i < cloud.points.size (); ++i)
{
// Check if the point is invalid
if (!pcl_isfinite (cloud.points[i].x) || !pcl_isfinite (cloud.points[i].y) || !pcl_isfinite (cloud.points[i].z))
continue;
pcl::Vector4fMapConst pt = cloud.points[i].getVector4fMap ();
dist = (pivot_pt - pt).norm ();
if (dist > max_dist)
{
max_idx = int (i);
max_dist = dist;
}
}
}
if(max_idx != -1)
max_pt = cloud.points[max_idx].getVector4fMap ();
else
max_pt = Eigen::Vector4f(std::numeric_limits<float>::quiet_NaN(),std::numeric_limits<float>::quiet_NaN(),std::numeric_limits<float>::quiet_NaN(),std::numeric_limits<float>::quiet_NaN());
}示例5: return
bool
af::isMatrixHealthy (const Eigen::MatrixXd &matrix)
{
for (int i = 0; i < matrix.rows (); ++i)
for (int j = 0; j < matrix.cols (); ++j)
if (!pcl_isfinite (matrix (i, j)))
return (false);
return (true);
}示例6: PCL_ERROR
void
pcl::getMinMax3D (const sensor_msgs::PointCloud2ConstPtr &cloud, int x_idx, int y_idx, int z_idx,
Eigen::Vector4f &min_pt, Eigen::Vector4f &max_pt)
{
// @todo fix this
if (cloud->fields[x_idx].datatype != sensor_msgs::PointField::FLOAT32 ||
cloud->fields[y_idx].datatype != sensor_msgs::PointField::FLOAT32 ||
cloud->fields[z_idx].datatype != sensor_msgs::PointField::FLOAT32)
{
PCL_ERROR ("[pcl::getMinMax3D] XYZ dimensions are not float type!\n");
return;
}
Eigen::Array4f min_p, max_p;
min_p.setConstant (FLT_MAX);
max_p.setConstant (-FLT_MAX);
int nr_points = cloud->width * cloud->height;
Eigen::Array4f pt = Eigen::Array4f::Zero ();
Eigen::Array4i xyz_offset (cloud->fields[x_idx].offset, cloud->fields[y_idx].offset, cloud->fields[z_idx].offset, 0);
for (int cp = 0; cp < nr_points; ++cp)
{
// Unoptimized memcpys: assume fields x, y, z are in random order
memcpy (&pt[0], &cloud->data[xyz_offset[0]], sizeof (float));
memcpy (&pt[1], &cloud->data[xyz_offset[1]], sizeof (float));
memcpy (&pt[2], &cloud->data[xyz_offset[2]], sizeof (float));
// Check if the point is invalid
if (!pcl_isfinite (pt[0]) ||
!pcl_isfinite (pt[1]) ||
!pcl_isfinite (pt[2]))
{
xyz_offset += cloud->point_step;
continue;
}
xyz_offset += cloud->point_step;
min_p = (min_p.min) (pt);
max_p = (max_p.max) (pt);
}
min_pt = min_p;
max_pt = max_p;
}示例7: removePointsWithNanNormals
// IMPLEMENTATIONS
void removePointsWithNanNormals(pcl::PointCloud<pcl::PointNormal>& cloud)
{
pcl::PointCloud<pcl::PointNormal> tmp;
for (size_t i = 0; i < cloud.points.size(); ++i)
{
if (pcl_isfinite(cloud.points[i].normal_x))
tmp.push_back(cloud.points[i]);
}
cloud = tmp;
}示例8:
template <typename PointInT, typename PointOutT> void
pcl::MovingLeastSquares<PointInT, PointOutT>::projectPointToMLSSurface (float &u_disp, float &v_disp,
Eigen::Vector3d &u, Eigen::Vector3d &v,
Eigen::Vector3d &plane_normal,
Eigen::Vector3d &mean,
float &curvature,
Eigen::VectorXd &c_vec,
int num_neighbors,
PointOutT &result_point,
pcl::Normal &result_normal) const
{
double n_disp = 0.0f;
double d_u = 0.0f, d_v = 0.0f;
// HARDCODED 5*nr_coeff_ to guarantee that the computed polynomial had a proper point set basis
if (polynomial_fit_ && num_neighbors >= 5*nr_coeff_ && pcl_isfinite (c_vec[0]))
{
// Compute the displacement along the normal using the fitted polynomial
// and compute the partial derivatives needed for estimating the normal
int j = 0;
float u_pow = 1.0f, v_pow = 1.0f, u_pow_prev = 1.0f, v_pow_prev = 1.0f;
for (int ui = 0; ui <= order_; ++ui)
{
v_pow = 1;
for (int vi = 0; vi <= order_ - ui; ++vi)
{
// Compute displacement along normal
n_disp += u_pow * v_pow * c_vec[j++];
// Compute partial derivatives
if (ui >= 1)
d_u += c_vec[j-1] * ui * u_pow_prev * v_pow;
if (vi >= 1)
d_v += c_vec[j-1] * vi * u_pow * v_pow_prev;
v_pow_prev = v_pow;
v_pow *= v_disp;
}
u_pow_prev = u_pow;
u_pow *= u_disp;
}
}
result_point.x = static_cast<float> (mean[0] + u[0] * u_disp + v[0] * v_disp + plane_normal[0] * n_disp);
result_point.y = static_cast<float> (mean[1] + u[1] * u_disp + v[1] * v_disp + plane_normal[1] * n_disp);
result_point.z = static_cast<float> (mean[2] + u[2] * u_disp + v[2] * v_disp + plane_normal[2] * n_disp);
Eigen::Vector3d normal = plane_normal - d_u * u - d_v * v;
normal.normalize ();
result_normal.normal_x = static_cast<float> (normal[0]);
result_normal.normal_y = static_cast<float> (normal[1]);
result_normal.normal_z = static_cast<float> (normal[2]);
result_normal.curvature = curvature;
}示例9:
template <typename PointNT> void
pcl::MarchingCubesGreedy<PointNT>::voxelizeData()
{
for (size_t cp = 0; cp < input_->points.size(); ++cp)
{
// Check if the point is invalid
if (!pcl_isfinite (input_->points[cp].x) ||
!pcl_isfinite (input_->points[cp].y) ||
!pcl_isfinite (input_->points[cp].z))
continue;
Eigen::Vector3i index_3d;
MarchingCubes<PointNT>::getCellIndex (input_->points[cp].getVector4fMap (), index_3d);
uint64_t index_1d = MarchingCubes<PointNT>::getIndexIn1D (index_3d);
Leaf cell_data;
for (int i = 0; i < 8; ++i)
cell_data.vertex[i] = 1;
cell_hash_map_[index_1d] = cell_data;
// the vertices are shared by 8 voxels, so we need to update all 8 of them
HashMap neighbor_list;
this->getNeighborList1D (cell_data, index_3d, neighbor_list);
BOOST_FOREACH (typename HashMap::value_type entry, neighbor_list)
{
Eigen::Vector3i i3d;
this->getIndexIn3D(entry.first, i3d);
// if the neighbor doesn't exist, add it, otherwise we need to do an OR operation on the vertices
if (cell_hash_map_.find (entry.first) == cell_hash_map_.end ())
{
cell_hash_map_[entry.first] = entry.second;
}
else
{
for (int i = 0; i < 8; ++i)
{
if (entry.second.vertex[i] > 0)
cell_hash_map_[entry.first].vertex[i] = entry.second.vertex[i];
}
}
}
}示例10: current_frame_z
template <typename PointInT, typename PointNT, typename PointOutT, typename PointRFT> void
pcl::SHOTEstimationBase<PointInT, PointNT, PointOutT, PointRFT>::createBinDistanceShape (
int index,
const std::vector<int> &indices,
std::vector<double> &bin_distance_shape)
{
bin_distance_shape.resize (indices.size ());
const PointRFT& current_frame = frames_->points[index];
//if (!pcl_isfinite (current_frame.rf[0]) || !pcl_isfinite (current_frame.rf[4]) || !pcl_isfinite (current_frame.rf[11]))
//return;
Eigen::Vector4f current_frame_z (current_frame.z_axis[0], current_frame.z_axis[1], current_frame.z_axis[2], 0);
unsigned nan_counter = 0;
for (size_t i_idx = 0; i_idx < indices.size (); ++i_idx)
{
// check NaN normal
const Eigen::Vector4f& normal_vec = normals_->points[indices[i_idx]].getNormalVector4fMap ();
if (!pcl_isfinite (normal_vec[0]) ||
!pcl_isfinite (normal_vec[1]) ||
!pcl_isfinite (normal_vec[2]))
{
bin_distance_shape[i_idx] = std::numeric_limits<double>::quiet_NaN ();
++nan_counter;
} else
{
//double cosineDesc = feat[i].rf[6]*normal[0] + feat[i].rf[7]*normal[1] + feat[i].rf[8]*normal[2];
double cosineDesc = normal_vec.dot (current_frame_z);
if (cosineDesc > 1.0)
cosineDesc = 1.0;
if (cosineDesc < - 1.0)
cosineDesc = - 1.0;
bin_distance_shape[i_idx] = ((1.0 + cosineDesc) * nr_shape_bins_) / 2;
}
}
if (nan_counter > 0)
PCL_WARN ("[pcl::%s::createBinDistanceShape] Point %d has %d (%f%%) NaN normals in its neighbourhood\n",
getClassName ().c_str (), index, nan_counter, (static_cast<float>(nan_counter)*100.f/static_cast<float>(indices.size ())));
}示例11: planarAlignmentTransform
Eigen::Affine3d planarAlignmentTransform (const Eigen::Vector4d& target, const Eigen::Vector4d& to_align)
{
Eigen::Vector3d target_norm (target[0], target[1], target[2]);
Eigen::Vector3d align_norm (to_align[0], to_align[1], to_align[2]);
double angle = acos (align_norm.dot (target_norm));
Eigen::Vector3d axis = align_norm.cross (target_norm);
axis.normalize ();
Eigen::Affine3d transform;
// If the normal is near identical, we'll get an invalid transform, and should instead use identity
if ((pcl_isfinite (angle)) && (pcl_isfinite (axis[0])) && (pcl_isfinite (axis[1])) && (pcl_isfinite (axis[2])))
{
transform = Eigen::AngleAxisd (angle, axis);
}
else
{
transform = Eigen::Affine3d::Identity ();
}
//transform = Eigen::AngleAxisd (angle, axis);
transform.translation () = target_norm * -1 * (target[3] - to_align[3]);
return (transform);
}示例12: real
void
pcl::BivariatePolynomialT<real>::findCriticalPoints (std::vector<real>& x_values, std::vector<real>& y_values,
std::vector<int>& types) const
{
x_values.clear ();
y_values.clear ();
types.clear ();
if (degree == 2)
{
real x = (real(2)*parameters[2]*parameters[3] - parameters[1]*parameters[4]) /
(parameters[1]*parameters[1] - real(4)*parameters[0]*parameters[3]),
y = (real(-2)*parameters[0]*x - parameters[2]) / parameters[1];
if (!pcl_isfinite(x) || !pcl_isfinite(y))
return;
int type = 2;
real det_H = real(4)*parameters[0]*parameters[3] - parameters[1]*parameters[1];
//std::cout << "det(H) = "<<det_H<<"\n";
if (det_H > real(0)) // Check Hessian determinant
{
if (parameters[0]+parameters[3] < real(0)) // Check Hessian trace
type = 0;
else
type = 1;
}
x_values.push_back(x);
y_values.push_back(y);
types.push_back(type);
//real gx, gy;
//((BivariatePolynomialT<real>*)this)->getValueOfGradient (x, y, gx, gy);
//std::cout << "Check: p'("<<x<<", "<<y<<") = ("<<gx<<", "<<gy<<")\n";
}
else
{
std::cerr << __PRETTY_FUNCTION__ << " is not implemented for polynomials of degree "<<degree<<". Sorry.\n";
}
}示例13: removeNaNs
void removeNaNs(const PointCloudNormal::Ptr& cloudInput, PointCloudNormal::Ptr& cloudOutput, std::vector<int>& removedPoints)
{
removedPoints.clear();
cloudOutput->header = cloudInput->header;
for(size_t i=0; i < cloudInput->points.size(); i++)
{
if(!pcl_isfinite(cloudInput->points[i].x) ||
!pcl_isfinite(cloudInput->points[i].y) ||
!pcl_isfinite(cloudInput->points[i].z))
{
// ROS_INFO_STREAM("NAN FOUND IN POINT COORDINATE. " << i);
removedPoints.push_back((int)i);
}
else if(!pcl_isfinite(cloudInput->points[i].normal_x) ||
!pcl_isfinite(cloudInput->points[i].normal_y) ||
!pcl_isfinite(cloudInput->points[i].normal_z))
{
// ROS_INFO_STREAM("NAN FOUND IN NORMAL. Specify larger K or search radius to calculate normals " << i);
removedPoints.push_back(int(i));
}
else
cloudOutput->points.push_back(cloudInput->points[i]);
}
cloudOutput->height = 1;
cloudOutput->width = static_cast<uint32_t>(cloudOutput->points.size());
ROS_INFO_STREAM("Size of removed indices" << removedPoints.size());
}示例14:
template <typename PointT> void
pcl::transformPointCloud (const pcl::PointCloud<PointT> &cloud_in,
pcl::PointCloud<PointT> &cloud_out,
const Eigen::Matrix4f &transform)
{
if (&cloud_in != &cloud_out)
{
// Note: could be replaced by cloud_out = cloud_in
cloud_out.header = cloud_in.header;
cloud_out.width = cloud_in.width;
cloud_out.height = cloud_in.height;
cloud_out.is_dense = cloud_in.is_dense;
cloud_out.points.reserve (cloud_out.points.size ());
cloud_out.points.assign (cloud_in.points.begin (), cloud_in.points.end ());
}
Eigen::Matrix3f rot = transform.block<3, 3> (0, 0);
Eigen::Vector3f trans = transform.block<3, 1> (0, 3);
// If the data is dense, we don't need to check for NaN
if (cloud_in.is_dense)
{
for (size_t i = 0; i < cloud_out.points.size (); ++i)
cloud_out.points[i].getVector3fMap () = rot *
cloud_in.points[i].getVector3fMap () + trans;
}
// Dataset might contain NaNs and Infs, so check for them first.
else
{
for (size_t i = 0; i < cloud_out.points.size (); ++i)
{
if (!pcl_isfinite (cloud_in.points[i].x) ||
!pcl_isfinite (cloud_in.points[i].y) ||
!pcl_isfinite (cloud_in.points[i].z))
continue;
cloud_out.points[i].getVector3fMap () = rot *
cloud_in.points[i].getVector3fMap () + trans;
}
}
}示例15: assert
template <typename PointInT, typename PointOutT, typename PointRFT> void
pcl::UniqueShapeContext<PointInT, PointOutT, PointRFT>::computeFeature (PointCloudOut &output)
{
assert (descriptor_length_ == 1960);
output.is_dense = true;
for (size_t point_index = 0; point_index < indices_->size (); ++point_index)
{
//output[point_index].descriptor.resize (descriptor_length_);
// If the point is not finite, set the descriptor to NaN and continue
const PointRFT& current_frame = (*frames_)[point_index];
if (!isFinite ((*input_)[(*indices_)[point_index]]) ||
!pcl_isfinite (current_frame.x_axis[0]) ||
!pcl_isfinite (current_frame.y_axis[0]) ||
!pcl_isfinite (current_frame.z_axis[0]) )
{
for (size_t i = 0; i < descriptor_length_; ++i)
output[point_index].descriptor[i] = std::numeric_limits<float>::quiet_NaN ();
memset (output[point_index].rf, 0, sizeof (output[point_index].rf[0]) * 9);
output.is_dense = false;
continue;
}
for (int d = 0; d < 3; ++d)
{
output.points[point_index].rf[0 + d] = current_frame.x_axis[d];
output.points[point_index].rf[3 + d] = current_frame.y_axis[d];
output.points[point_index].rf[6 + d] = current_frame.z_axis[d];
}
std::vector<float> descriptor (descriptor_length_);
computePointDescriptor (point_index, descriptor);
for (size_t j = 0; j < descriptor_length_; ++j)
output [point_index].descriptor[j] = descriptor[j];
}
}