static void getCentroid3D(const pcl::PointCloud<PointT> &pcl_cloud_input, Eigen::Vector4f ¢roid)
{
// Initialize to 0
centroid.setZero();
if(pcl_cloud_input.points.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(pcl_cloud_input.is_dense)
{
for(size_t i = 0; i < pcl_cloud_input.points.size(); ++i)
centroid += pcl_cloud_input.points[i].getVector4fMap();
centroid[3] = 0;
centroid /= pcl_cloud_input.points.size();
}
// NaN or Inf values could exist => check for them
else
{
for(size_t i = 0; i < pcl_cloud_input.points.size(); ++i)
{
// Check if the point is invalid
if(!pcl_isfinite(pcl_cloud_input.points[i].x) || !pcl_isfinite(pcl_cloud_input.points[i].y) || !pcl_isfinite(pcl_cloud_input.points[i].z))
continue;
centroid += pcl_cloud_input.points[i].getVector4fMap();
cp++;
}
centroid[3] = 0;
centroid /= cp;
}
}
void AbsoluteEulerAngleDecoder::Decode(array_view<DirectX::Quaternion> rots, const VectorType & y)
{
int n = rots.size();
Eigen::Vector4f qs;
XMVECTOR q;
qs.setZero();
for (int i = 0; i < n; i++)
{
qs.segment<3>(0) = y.segment<3>(i * 3).cast<float>();
q = XMLoadFloat4A(qs.data());
q = XMQuaternionRotationRollPitchYawFromVector(q); // revert the log map
XMStoreA(rots[i], q);
}
}
void AbsoluteLnQuaternionDecoder::Decode(array_view<DirectX::Quaternion> rots, const VectorType & x)
{
int n = rots.size();
Eigen::Vector4f qs;
XMVECTOR q;
qs.setZero();
for (int i = 0; i < n; i++)
{
qs.segment<3>(0) = x.segment<3>(i * 3).cast<float>();
q = XMLoadFloat4A(qs.data());
q = XMQuaternionExp(q); // revert the log map
XMStoreA(rots[i], q);
}
}
void AbsoluteEulerAngleDecoder::Encode(array_view<const DirectX::Quaternion> rots, VectorType & x)
{
int n = rots.size();
Eigen::Vector4f qs;
XMVECTOR q;
qs.setZero();
x.resize(n * 3);
for (int i = 0; i < n; i++)
{
q = XMLoad(rots[i]);
q = XMQuaternionEulerAngleYawPitchRoll(q); // Decompsoe in to euler angle
XMStoreFloat4(qs.data(), q);
x.segment<3>(i * 3) = qs.head<3>();
}
}
void RelativeEulerAngleDecoder::Decode(array_view<DirectX::Quaternion> rots, const VectorType & x)
{
int n = rots.size();
Eigen::Vector4f qs;
XMVECTOR q, qb;
qs.setZero();
for (int i = 0; i < n; i++)
{
qs.segment<3>(0) = x.segment<3>(i * 3).cast<float>();
q = XMLoadFloat4A(qs.data());
q = XMQuaternionRotationRollPitchYawFromVector(q); // revert the log map
qb = XMLoadA(bases[i]);
q = XMQuaternionMultiply(qb, q);
XMStoreA(rots[i], q);
}
}
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);
}
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);
}
}
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;
}
}