template <typename PointT, typename Scalar> inline unsigned int
pcl::computeCovarianceMatrix (const pcl::PointCloud<PointT> &cloud,
Eigen::Matrix<Scalar, 3, 3> &covariance_matrix)
{
// create the buffer on the stack which is much faster than using cloud[indices[i]] and centroid as a buffer
Eigen::Matrix<Scalar, 1, 6, Eigen::RowMajor> accu = Eigen::Matrix<Scalar, 1, 6, Eigen::RowMajor>::Zero ();
unsigned int point_count;
if (cloud.is_dense)
{
point_count = static_cast<unsigned int> (cloud.size ());
// For each point in the cloud
for (size_t i = 0; i < point_count; ++i)
{
accu [0] += cloud[i].x * cloud[i].x;
accu [1] += cloud[i].x * cloud[i].y;
accu [2] += cloud[i].x * cloud[i].z;
accu [3] += cloud[i].y * cloud[i].y;
accu [4] += cloud[i].y * cloud[i].z;
accu [5] += cloud[i].z * cloud[i].z;
}
}
else
{
point_count = 0;
for (size_t i = 0; i < cloud.size (); ++i)
{
if (!isFinite (cloud[i]))
continue;
accu [0] += cloud[i].x * cloud[i].x;
accu [1] += cloud[i].x * cloud[i].y;
accu [2] += cloud[i].x * cloud[i].z;
accu [3] += cloud[i].y * cloud[i].y;
accu [4] += cloud[i].y * cloud[i].z;
accu [5] += cloud[i].z * cloud[i].z;
++point_count;
}
}
if (point_count != 0)
{
accu /= static_cast<Scalar> (point_count);
covariance_matrix.coeffRef (0) = accu [0];
covariance_matrix.coeffRef (1) = covariance_matrix.coeffRef (3) = accu [1];
covariance_matrix.coeffRef (2) = covariance_matrix.coeffRef (6) = accu [2];
covariance_matrix.coeffRef (4) = accu [3];
covariance_matrix.coeffRef (5) = covariance_matrix.coeffRef (7) = accu [4];
covariance_matrix.coeffRef (8) = accu [5];
}
return (point_count);
}
template <typename PointT> inline unsigned int
pcl::computeCovarianceMatrix (const pcl::PointCloud<PointT> &cloud,
const std::vector<int> &indices,
Eigen::Matrix3d &covariance_matrix)
{
// create the buffer on the stack which is much faster than using cloud.points[indices[i]] and centroid as a buffer
Eigen::Matrix<double, 1, 6, Eigen::RowMajor> accu = Eigen::Matrix<double, 1, 6, Eigen::RowMajor>::Zero ();
unsigned int point_count;
if (cloud.is_dense)
{
point_count = indices.size ();
for (std::vector<int>::const_iterator iIt = indices.begin (); iIt != indices.end (); ++iIt)
{
//const PointT& point = cloud[*iIt];
accu [0] += cloud[*iIt].x * cloud[*iIt].x;
accu [1] += cloud[*iIt].x * cloud[*iIt].y;
accu [2] += cloud[*iIt].x * cloud[*iIt].z;
accu [3] += cloud[*iIt].y * cloud[*iIt].y;
accu [4] += cloud[*iIt].y * cloud[*iIt].z;
accu [5] += cloud[*iIt].z * cloud[*iIt].z;
}
}
else
{
point_count = 0;
for (std::vector<int>::const_iterator iIt = indices.begin (); iIt != indices.end (); ++iIt)
{
if (!isFinite (cloud[*iIt]))
continue;
++point_count;
accu [0] += cloud[*iIt].x * cloud[*iIt].x;
accu [1] += cloud[*iIt].x * cloud[*iIt].y;
accu [2] += cloud[*iIt].x * cloud[*iIt].z;
accu [3] += cloud[*iIt].y * cloud[*iIt].y;
accu [4] += cloud[*iIt].y * cloud[*iIt].z;
accu [5] += cloud[*iIt].z * cloud[*iIt].z;
}
}
if (point_count != 0)
{
accu /= static_cast<double> (point_count);
covariance_matrix.coeffRef (0) = accu [0];
covariance_matrix.coeffRef (1) = covariance_matrix.coeffRef (3) = accu [1];
covariance_matrix.coeffRef (2) = covariance_matrix.coeffRef (6) = accu [2];
covariance_matrix.coeffRef (4) = accu [3];
covariance_matrix.coeffRef (5) = covariance_matrix.coeffRef (7) = accu [4];
covariance_matrix.coeffRef (8) = accu [5];
}
return (point_count);
}
Cardinality::CardinalityComparison Cardinality::compare(
const Cardinality& c) const {
if (isUnknown() || c.isUnknown()) {
return UNKNOWN;
} else if (isLargeFinite()) {
if (c.isLargeFinite()) {
return UNKNOWN;
} else if (c.isFinite()) {
return GREATER;
} else {
Assert(c.isInfinite());
return LESS;
}
} else if (c.isLargeFinite()) {
if (isLargeFinite()) {
return UNKNOWN;
} else if (isFinite()) {
return LESS;
} else {
Assert(isInfinite());
return GREATER;
}
} else if (isInfinite()) {
if (c.isFinite()) {
return GREATER;
} else {
return d_card < c.d_card ? GREATER : (d_card == c.d_card ? EQUAL : LESS);
}
} else if (c.isInfinite()) {
Assert(isFinite());
return LESS;
} else {
Assert(isFinite() && !isLargeFinite());
Assert(c.isFinite() && !c.isLargeFinite());
return d_card < c.d_card ? LESS : (d_card == c.d_card ? EQUAL : GREATER);
}
Unreachable();
}
LLCoordFrame::LLCoordFrame(const LLVector3 &origin,
const LLMatrix3 &rotation) :
mOrigin(origin),
mXAxis(rotation.mMatrix[VX]),
mYAxis(rotation.mMatrix[VY]),
mZAxis(rotation.mMatrix[VZ])
{
if( !isFinite() )
{
reset();
llwarns << "Non Finite in LLCoordFrame::LLCoordFrame()" << llendl;
}
}
void LLCoordFrame::setAxes(const LLVector3 &x_axis,
const LLVector3 &y_axis,
const LLVector3 &z_axis)
{
mXAxis = x_axis;
mYAxis = y_axis;
mZAxis = z_axis;
if( !isFinite() )
{
reset();
llwarns << "Non Finite in LLCoordFrame::setAxes()" << llendl;
}
}
template <typename PointT> inline unsigned int
pcl::computeCovarianceMatrix (const pcl::PointCloud<PointT> &cloud,
const std::vector<int> &indices,
Eigen::Matrix3f &covariance_matrix)
{
Eigen::Matrix<float, 1, 6, Eigen::RowMajor> accu = Eigen::Matrix<float, 1, 6, Eigen::RowMajor>::Zero ();
unsigned int point_count;
if (cloud.is_dense)
{
point_count = static_cast<unsigned int> (indices.size ());
for (std::vector<int>::const_iterator iIt = indices.begin (); iIt != indices.end (); ++iIt)
{
//const PointT& point = cloud[*iIt];
accu [0] += cloud[*iIt].x * cloud[*iIt].x;
accu [1] += cloud[*iIt].x * cloud[*iIt].y;
accu [2] += cloud[*iIt].x * cloud[*iIt].z;
accu [3] += cloud[*iIt].y * cloud[*iIt].y;
accu [4] += cloud[*iIt].y * cloud[*iIt].z;
accu [5] += cloud[*iIt].z * cloud[*iIt].z;
}
}
else
{
point_count = 0;
for (std::vector<int>::const_iterator iIt = indices.begin (); iIt != indices.end (); ++iIt)
{
if (!isFinite (cloud[*iIt]))
continue;
++point_count;
accu [0] += cloud[*iIt].x * cloud[*iIt].x;
accu [1] += cloud[*iIt].x * cloud[*iIt].y;
accu [2] += cloud[*iIt].x * cloud[*iIt].z;
accu [3] += cloud[*iIt].y * cloud[*iIt].y;
accu [4] += cloud[*iIt].y * cloud[*iIt].z;
accu [5] += cloud[*iIt].z * cloud[*iIt].z;
}
}
if (point_count != 0)
{
accu /= static_cast<float> (point_count);
covariance_matrix.coeffRef (0) = accu [0];
covariance_matrix.coeffRef (1) = covariance_matrix.coeffRef (3) = accu [1];
covariance_matrix.coeffRef (2) = covariance_matrix.coeffRef (6) = accu [2];
covariance_matrix.coeffRef (4) = accu [3];
covariance_matrix.coeffRef (5) = covariance_matrix.coeffRef (7) = accu [4];
covariance_matrix.coeffRef (8) = accu [5];
}
return (point_count);
}
template<typename PointT> void
pcl::CropBox<PointT>::applyFilter (std::vector<int> &indices)
{
indices.resize (input_->points.size ());
int indice_count = 0;
Eigen::Affine3f transform = Eigen::Affine3f::Identity ();
Eigen::Affine3f inverse_transform = Eigen::Affine3f::Identity ();
if (rotation_ != Eigen::Vector3f::Zero ())
{
pcl::getTransformation (0, 0, 0,
rotation_ (0), rotation_ (1), rotation_ (2),
transform);
inverse_transform = transform.inverse ();
}
for (size_t index = 0; index < indices_->size (); ++index)
{
if (!input_->is_dense)
// Check if the point is invalid
if (!isFinite (input_->points[index]))
continue;
// Get local point
PointT local_pt = input_->points[(*indices_)[index]];
// Transform point to world space
if (!(transform_.matrix ().isIdentity ()))
local_pt = pcl::transformPoint<PointT> (local_pt, transform_);
if (translation_ != Eigen::Vector3f::Zero ())
{
local_pt.x -= translation_ (0);
local_pt.y -= translation_ (1);
local_pt.z -= translation_ (2);
}
// Transform point to local space of crop box
if (!(inverse_transform.matrix ().isIdentity ()))
local_pt = pcl::transformPoint<PointT> (local_pt, inverse_transform);
if (local_pt.x < min_pt_[0] || local_pt.y < min_pt_[1] || local_pt.z < min_pt_[2])
continue;
if (local_pt.x > max_pt_[0] || local_pt.y > max_pt_[1] || local_pt.z > max_pt_[2])
continue;
indices[indice_count++] = (*indices_)[index];
}
indices.resize (indice_count);
}
template <typename PointInT, typename PointNT> void
pcl::PrincipalCurvaturesEstimation<PointInT, PointNT, Eigen::MatrixXf>::computeFeatureEigen (pcl::PointCloud<Eigen::MatrixXf> &output)
{
// Resize the output dataset
output.points.resize (indices_->size (), 5);
// 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;
// Save a few cycles by not checking every point for NaN/Inf values if the cloud is set to dense
if (input_->is_dense)
{
// Iterating over the entire index vector
for (size_t idx = 0; idx < indices_->size (); ++idx)
{
if (this->searchForNeighbors ((*indices_)[idx], search_parameter_, nn_indices, nn_dists) == 0)
{
output.points.row (idx).setConstant (std::numeric_limits<float>::quiet_NaN ());
output.is_dense = false;
continue;
}
// Estimate the principal curvatures at each patch
this->computePointPrincipalCurvatures (*normals_, (*indices_)[idx], nn_indices,
output.points (idx, 0), output.points (idx, 1), output.points (idx, 2),
output.points (idx, 3), output.points (idx, 4));
}
}
else
{
// Iterating over the entire index vector
for (size_t idx = 0; idx < indices_->size (); ++idx)
{
if (!isFinite ((*input_)[(*indices_)[idx]]) ||
this->searchForNeighbors ((*indices_)[idx], search_parameter_, nn_indices, nn_dists) == 0)
{
output.points.row (idx).setConstant (std::numeric_limits<float>::quiet_NaN ());
output.is_dense = false;
continue;
}
// Estimate the principal curvatures at each patch
this->computePointPrincipalCurvatures (*normals_, (*indices_)[idx], nn_indices,
output.points (idx, 0), output.points (idx, 1), output.points (idx, 2),
output.points (idx, 3), output.points (idx, 4));
}
}
}
template <typename PointInT, typename PointNT, typename PointOutT> void
pcl::PrincipalCurvaturesEstimation<PointInT, PointNT, PointOutT>::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;
// Save a few cycles by not checking every point for NaN/Inf values if the cloud is set to dense
if (input_->is_dense)
{
// Iterating over the entire index vector
for (size_t idx = 0; idx < indices_->size (); ++idx)
{
if (this->searchForNeighbors ((*indices_)[idx], search_parameter_, nn_indices, nn_dists) == 0)
{
output.points[idx].principal_curvature[0] = output.points[idx].principal_curvature[1] = output.points[idx].principal_curvature[2] =
output.points[idx].pc1 = output.points[idx].pc2 = std::numeric_limits<float>::quiet_NaN ();
output.is_dense = false;
continue;
}
// Estimate the principal curvatures at each patch
computePointPrincipalCurvatures (*normals_, (*indices_)[idx], nn_indices,
output.points[idx].principal_curvature[0], output.points[idx].principal_curvature[1], output.points[idx].principal_curvature[2],
output.points[idx].pc1, output.points[idx].pc2);
}
}
else
{
// Iterating over the entire index vector
for (size_t idx = 0; idx < indices_->size (); ++idx)
{
if (!isFinite ((*input_)[(*indices_)[idx]]) ||
this->searchForNeighbors ((*indices_)[idx], search_parameter_, nn_indices, nn_dists) == 0)
{
output.points[idx].principal_curvature[0] = output.points[idx].principal_curvature[1] = output.points[idx].principal_curvature[2] =
output.points[idx].pc1 = output.points[idx].pc2 = std::numeric_limits<float>::quiet_NaN ();
output.is_dense = false;
continue;
}
// Estimate the principal curvatures at each patch
computePointPrincipalCurvatures (*normals_, (*indices_)[idx], nn_indices,
output.points[idx].principal_curvature[0], output.points[idx].principal_curvature[1], output.points[idx].principal_curvature[2],
output.points[idx].pc1, output.points[idx].pc2);
}
}
}
half
floatToHalf (float f)
{
if (isFinite (f))
{
if (f > HALF_MAX)
return half::posInf();
if (f < -HALF_MAX)
return half::negInf();
}
return half (f);
}
LLCoordFrame::LLCoordFrame(const LLVector3 &origin, const LLQuaternion &q) :
mOrigin(origin)
{
LLMatrix3 rotation_matrix(q);
mXAxis.setVec(rotation_matrix.mMatrix[VX]);
mYAxis.setVec(rotation_matrix.mMatrix[VY]);
mZAxis.setVec(rotation_matrix.mMatrix[VZ]);
if( !isFinite() )
{
reset();
llwarns << "Non Finite in LLCoordFrame::LLCoordFrame()" << llendl;
}
}
LLCoordFrame::LLCoordFrame(const LLVector3 &x_axis,
const LLVector3 &y_axis,
const LLVector3 &z_axis) :
mOrigin(0.f, 0.f, 0.f),
mXAxis(x_axis),
mYAxis(y_axis),
mZAxis(z_axis)
{
if( !isFinite() )
{
reset();
llwarns << "Non Finite in LLCoordFrame::LLCoordFrame()" << llendl;
}
}
template <typename PointT> void
pcl::getPointCloudDifference (
const pcl::PointCloud<PointT> &src,
const pcl::PointCloud<PointT> &,
double threshold, const boost::shared_ptr<pcl::search::Search<PointT> > &tree,
pcl::PointCloud<PointT> &output)
{
// We're interested in a single nearest neighbor only
std::vector<int> nn_indices (1);
std::vector<float> nn_distances (1);
// The src indices that do not have a neighbor in tgt
std::vector<int> src_indices;
// Iterate through the source data set
for (int i = 0; i < static_cast<int> (src.points.size ()); ++i)
{
if (!isFinite (src.points[i]))
continue;
// Search for the closest point in the target data set (number of neighbors to find = 1)
if (!tree->nearestKSearch (src.points[i], 1, nn_indices, nn_distances))
{
PCL_WARN ("No neighbor found for point %zu (%f %f %f)!\n", i, src.points[i].x, src.points[i].y, src.points[i].z);
continue;
}
if (nn_distances[0] > threshold)
src_indices.push_back (i);
}
// Allocate enough space and copy the basics
output.points.resize (src_indices.size ());
output.header = src.header;
output.width = static_cast<uint32_t> (src_indices.size ());
output.height = 1;
//if (src.is_dense)
output.is_dense = true;
//else
// It's not necessarily true that is_dense is false if cloud_in.is_dense is false
// To verify this, we would need to iterate over all points and check for NaNs
//output.is_dense = false;
// Copy all the data fields from the input cloud to the output one
typedef typename pcl::traits::fieldList<PointT>::type FieldList;
// Iterate over each point
for (size_t i = 0; i < src_indices.size (); ++i)
// Iterate over each dimension
pcl::for_each_type <FieldList> (NdConcatenateFunctor <PointT, PointT> (src.points[src_indices[i]], output.points[i]));
}
template<typename PointT, typename LeafT, typename BranchT> void
pcl::octree::OctreePointCloudSearch<PointT, LeafT, BranchT>::approxNearestSearch (const PointT &p_q,
int &result_index,
float &sqr_distance)
{
assert(this->leafCount_>0);
assert (isFinite (p_q) && "Invalid (NaN, Inf) point coordinates given to nearestKSearch!");
OctreeKey key;
key.x = key.y = key.z = 0;
approxNearestSearchRecursive (p_q, this->rootNode_, key, 1, result_index, sqr_distance);
return;
}
template <typename PointInT> void
pcl16::NormalEstimationOMP<PointInT, Eigen::MatrixXf>::computeFeatureEigen (pcl16::PointCloud<Eigen::MatrixXf> &output)
{
float vpx, vpy, vpz;
getViewPoint (vpx, vpy, vpz);
output.is_dense = true;
// Resize the output dataset
output.points.resize (indices_->size (), 4);
// GCC 4.2.x seems to segfault with "internal compiler error" on MacOS X here
#if defined(_WIN32) || ((__GNUC__ > 4) && (__GNUC_MINOR__ > 2))
#pragma omp parallel for schedule (dynamic, threads_)
#endif
// Iterating over the entire index vector
for (int idx = 0; idx < static_cast<int> (indices_->size ()); ++idx)
{
// 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_);
if (!isFinite ((*input_)[(*indices_)[idx]]) ||
this->searchForNeighbors ((*indices_)[idx], search_parameter_, nn_indices, nn_dists) == 0)
{
output.points (idx, 0) = output.points (idx, 1) = output.points (idx, 2) = output.points (idx, 3) = std::numeric_limits<float>::quiet_NaN ();
output.is_dense = false;
continue;
}
// 16-bytes aligned placeholder for the XYZ centroid of a surface patch
Eigen::Vector4f xyz_centroid;
// Estimate the XYZ centroid
compute3DCentroid (*surface_, nn_indices, xyz_centroid);
// Placeholder for the 3x3 covariance matrix at each surface patch
EIGEN_ALIGN16 Eigen::Matrix3f covariance_matrix;
// Compute the 3x3 covariance matrix
computeCovarianceMatrix (*surface_, nn_indices, xyz_centroid, covariance_matrix);
// Get the plane normal and surface curvature
solvePlaneParameters (covariance_matrix,
output.points (idx, 0), output.points (idx, 1), output.points (idx, 2), output.points (idx, 3));
flipNormalTowardsViewpoint (input_->points[(*indices_)[idx]], vpx, vpy, vpz,
output.points (idx, 0), output.points (idx, 1), output.points (idx, 2));
}
}
template <typename PointT> int
pcl16::search::BruteForce<PointT>::nearestKSearch (
const PointT& point, int k, std::vector<int>& k_indices, std::vector<float>& k_distances) const
{
assert (isFinite (point) && "Invalid (NaN, Inf) point coordinates given to nearestKSearch!");
k_indices.clear ();
k_distances.clear ();
if (k < 1)
return 0;
if (input_->is_dense)
return denseKSearch (point, k, k_indices, k_distances);
else
return sparseKSearch (point, k, k_indices, k_distances);
}
double KolmogorovZurbenko::mavga1d(const QVector<double> &v, int start, int stop)
{
double s = 0;
int z = 0;
for(int i = start; i < stop; i++){
if(isFinite(v[i])){
z++;
s += v[i];
}
}
if(z == 0) return std::numeric_limits<double>::quiet_NaN();
return s / z;
}
template <typename PointT> int
pcl16::search::BruteForce<PointT>::radiusSearch (
const PointT& point, double radius, std::vector<int> &k_indices,
std::vector<float> &k_sqr_distances, unsigned int max_nn) const
{
assert (isFinite (point) && "Invalid (NaN, Inf) point coordinates given to nearestKSearch!");
k_indices.clear ();
k_sqr_distances.clear ();
if (radius <= 0)
return 0;
if (input_->is_dense)
return denseRadiusSearch (point, radius, k_indices, k_sqr_distances, max_nn);
else
return sparseRadiusSearch (point, radius, k_indices, k_sqr_distances, max_nn);
}
Real Vector4::normalize()
{
aproassert(isFinite(), "Vector4 not finite !");
Real lenghtSq = squaredLenght();
if(lenghtSq > 1e-6f)
{
Real l = Math::Sqrt(lenghtSq);
*this *= 1.f / l;
return l;
}
else
{
set(1.f, 0.f, 0.f, 0.f);
return 0;
}
}
template <typename PointT, typename Scalar> inline unsigned int
pcl::compute3DCentroid (const pcl::PointCloud<PointT> &cloud,
Eigen::Matrix<Scalar, 4, 1> ¢roid)
{
if (cloud.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.size (); ++i)
{
centroid[0] += cloud[i].x;
centroid[1] += cloud[i].y;
centroid[2] += cloud[i].z;
}
centroid[3] = 0;
centroid /= static_cast<Scalar> (cloud.size ());
return (static_cast<unsigned int> (cloud.size ()));
}
// NaN or Inf values could exist => check for them
else
{
unsigned cp = 0;
for (size_t i = 0; i < cloud.size (); ++i)
{
// Check if the point is invalid
if (!isFinite (cloud [i]))
continue;
centroid[0] += cloud[i].x;
centroid[1] += cloud[i].y;
centroid[2] += cloud[i].z;
++cp;
}
centroid[3] = 0;
centroid /= static_cast<Scalar> (cp);
return (cp);
}
}
size_t LLCoordFrame::readOrientation(const char *buffer)
{
memcpy(mOrigin.mV, buffer, 3*sizeof(F32)); /*Flawfinder: ignore */
buffer += 3*sizeof(F32);
memcpy(mXAxis.mV, buffer, 3*sizeof(F32)); /*Flawfinder: ignore */
buffer += 3*sizeof(F32);
memcpy(mYAxis.mV, buffer, 3*sizeof(F32)); /*Flawfinder: ignore */
buffer += 3*sizeof(F32);
memcpy(mZAxis.mV, buffer, 3*sizeof(F32)); /*Flawfinder: ignore */
if( !isFinite() )
{
reset();
llwarns << "Non Finite in LLCoordFrame::readOrientation()" << llendl;
}
return 12*sizeof(F32);
}
void LLCoordFrame::setAxes( const F32 *rotation_matrix )
{
mXAxis.mV[VX] = *(rotation_matrix + 3*VX + VX);
mXAxis.mV[VY] = *(rotation_matrix + 3*VX + VY);
mXAxis.mV[VZ] = *(rotation_matrix + 3*VX + VZ);
mYAxis.mV[VX] = *(rotation_matrix + 3*VY + VX);
mYAxis.mV[VY] = *(rotation_matrix + 3*VY + VY);
mYAxis.mV[VZ] = *(rotation_matrix + 3*VY + VZ);
mZAxis.mV[VX] = *(rotation_matrix + 3*VZ + VX);
mZAxis.mV[VY] = *(rotation_matrix + 3*VZ + VY);
mZAxis.mV[VZ] = *(rotation_matrix + 3*VZ + VZ);
if( !isFinite() )
{
reset();
llwarns << "Non Finite in LLCoordFrame::setAxes()" << llendl;
}
}
template<typename PointT, typename LeafT, typename BranchT> int
pcl::octree::OctreePointCloudSearch<PointT, LeafT, BranchT>::radiusSearch (const PointT &p_q, const double radius,
std::vector<int> &k_indices,
std::vector<float> &k_sqr_distances,
unsigned int max_nn) const
{
assert (isFinite (p_q) && "Invalid (NaN, Inf) point coordinates given to nearestKSearch!");
OctreeKey key;
key.x = key.y = key.z = 0;
k_indices.clear ();
k_sqr_distances.clear ();
getNeighborsWithinRadiusRecursive (p_q, radius * radius, this->rootNode_, key, 1, k_indices, k_sqr_distances,
max_nn);
return (static_cast<int> (k_indices.size ()));
}
template <typename PointInT> void
pcl::OrganizedFastMesh<PointInT>::performReconstruction (pcl::PolygonMesh &output)
{
reconstructPolygons (output.polygons);
// Get the field names
int x_idx = pcl::getFieldIndex (output.cloud, "x");
int y_idx = pcl::getFieldIndex (output.cloud, "y");
int z_idx = pcl::getFieldIndex (output.cloud, "z");
if (x_idx == -1 || y_idx == -1 || z_idx == -1)
return;
// correct all measurements,
// (running over complete image since some rows and columns are left out
// depending on triangle_pixel_size)
// avoid to do that here (only needed for ASCII mesh file output, e.g., in vtk files
for (unsigned int i = 0; i < input_->points.size (); ++i)
if (!isFinite (input_->points[i]))
resetPointData (i, output, 0.0f, x_idx, y_idx, z_idx);
}
template <typename PointInT, typename PointOutT> void
pcl16::NormalEstimationOMP<PointInT, PointOutT>::computeFeature (PointCloudOut &output)
{
float vpx, vpy, vpz;
getViewPoint (vpx, vpy, vpz);
output.is_dense = true;
// Iterating over the entire index vector
#pragma omp parallel for schedule (dynamic, threads_)
for (int idx = 0; idx < static_cast<int> (indices_->size ()); ++idx)
{
// 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_);
if (!isFinite ((*input_)[(*indices_)[idx]]) ||
this->searchForNeighbors ((*indices_)[idx], search_parameter_, nn_indices, nn_dists) == 0)
{
output.points[idx].normal[0] = output.points[idx].normal[1] = output.points[idx].normal[2] = output.points[idx].curvature = std::numeric_limits<float>::quiet_NaN ();
output.is_dense = false;
continue;
}
// 16-bytes aligned placeholder for the XYZ centroid of a surface patch
Eigen::Vector4f xyz_centroid;
// Estimate the XYZ centroid
compute3DCentroid (*surface_, nn_indices, xyz_centroid);
// Placeholder for the 3x3 covariance matrix at each surface patch
EIGEN_ALIGN16 Eigen::Matrix3f covariance_matrix;
// Compute the 3x3 covariance matrix
computeCovarianceMatrix (*surface_, nn_indices, xyz_centroid, covariance_matrix);
// Get the plane normal and surface curvature
solvePlaneParameters (covariance_matrix,
output.points[idx].normal[0], output.points[idx].normal[1], output.points[idx].normal[2], output.points[idx].curvature);
flipNormalTowardsViewpoint (input_->points[(*indices_)[idx]], vpx, vpy, vpz,
output.points[idx].normal[0], output.points[idx].normal[1], output.points[idx].normal[2]);
}
}
Plane CoordinateFrame::toWorldSpace(const Plane& plane) const {
// Since there is no scale factor, we don't have to
// worry about the inverse transpose of the normal.
Vector3 normal;
float d;
plane.getEquation(normal, d);
const Vector3& newNormal = rotation * normal;
if (isFinite(d)) {
d = (newNormal * -d + translation).dot(newNormal);
return Plane(newNormal, newNormal * d);
} else {
// When d is infinite, we can't multiply 0's by it without
// generating NaNs.
return Plane::fromEquation(newNormal.x, newNormal.y, newNormal.z, d);
}
}
template<typename PointT, typename LeafT, typename BranchT> int
pcl::octree::OctreePointCloudSearch<PointT, LeafT, BranchT>::nearestKSearch (const PointT &p_q, int k,
std::vector<int> &k_indices,
std::vector<float> &k_sqr_distances)
{
assert(this->leafCount_>0);
assert (isFinite (p_q) && "Invalid (NaN, Inf) point coordinates given to nearestKSearch!");
k_indices.clear ();
k_sqr_distances.clear ();
if (k < 1)
return 0;
unsigned int i;
unsigned int resultCount;
prioPointQueueEntry pointEntry;
std::vector<prioPointQueueEntry> pointCandidates;
OctreeKey key;
key.x = key.y = key.z = 0;
// initalize smallest point distance in search with high value
double smallestDist = numeric_limits<double>::max ();
getKNearestNeighborRecursive (p_q, k, this->rootNode_, key, 1, smallestDist, pointCandidates);
resultCount = static_cast<unsigned int> (pointCandidates.size ());
k_indices.resize (resultCount);
k_sqr_distances.resize (resultCount);
for (i = 0; i < resultCount; ++i)
{
k_indices [i] = pointCandidates [i].pointIdx_;
k_sqr_distances [i] = pointCandidates [i].pointDistance_;
}
return static_cast<int> (k_indices.size ());
}
void EffectPanel::typeChangedCallback(char* type) {
Monitor::removeCallback( (void*)(&effect->inst->original->depths[effect->timeline][oldType]), this );
Monitor::addCallback( &effect->inst->original->depths[effect->timeline][effect->type],
new Callback<unsigned char, EffectPanel>
(this, &EffectPanel::depthChangedCallback) );
oldType = effect->type;
effectTypeChoice->Select(effect->type);
if (isConstant == false) {
if (isFinite()) {
sliderBox->Show(true);
} else {
sliderBox->Show(false);
depthSlider->SetValue(effect->inst->getDepth(effect->type, effect->timeline));
}
}
wxSizer* parentSizer = GetParent()->GetContainingSizer();
wxWindow* parentWindow = GetParent()->GetParent();
parentWindow->SetVirtualSizeHints(0,0);
parentSizer->SetVirtualSizeHints(parentWindow);
}
BOOM::Vector<double>* Application::loadPosExamples(const BOOM::String &filename,
SignalSensor &model)
{
int consensusOffset=model.getConsensusOffset();
int windowLen=model.getContextWindowLength();
int consensusLen=model.getConsensusLength();
BOOM::Vector<double> *scores=new BOOM::Vector<double>;
BOOM::FastaReader reader(filename);
BOOM::String def, seqStr;
while(reader.nextSequence(def,seqStr))
{
Sequence seq(seqStr,DnaAlphabet::global());
int seqLen=seqStr.length();
int consensusBegin=(seqLen-consensusLen)/2;
int windowBegin=consensusBegin-consensusOffset;
double score=model.getLogP(seq,seqStr,windowBegin);
if(isFinite(score)) scores->push_back(score);
}
return scores;
}
template<typename PointT, typename LeafT, typename BranchT> bool
pcl::octree::OctreePointCloudSearch<PointT, LeafT, BranchT>::voxelSearch (const PointT& point,
std::vector<int>& pointIdx_data)
{
assert (isFinite (point) && "Invalid (NaN, Inf) point coordinates given to nearestKSearch!");
OctreeKey key;
bool b_success = false;
// generate key
this->genOctreeKeyforPoint (point, key);
LeafT* leaf = this->findLeaf (key);
if (leaf)
{
leaf->getData (pointIdx_data);
b_success = true;
}
return (b_success);
}
