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);
}
}
}
Eigen::Vector3f unproject(const Eigen::Vector3f &win,
const Eigen::Matrix4f &model,
const Eigen::Matrix4f &proj,
const Vector2i &viewportSize) {
Eigen::Matrix4f Inverse = (proj * model).inverse();
Eigen::Vector4f tmp;
tmp << win, 1;
tmp(0) = tmp(0) / viewportSize.x();
tmp(1) = tmp(1) / viewportSize.y();
tmp = tmp.array() * 2.0f - 1.0f;
Eigen::Vector4f obj = Inverse * tmp;
obj /= obj(3);
return obj.head(3);
}
Eigen::Vector3f project(const Eigen::Vector3f &obj,
const Eigen::Matrix4f &model,
const Eigen::Matrix4f &proj,
const Vector2i &viewportSize) {
Eigen::Vector4f tmp;
tmp << obj, 1;
tmp = model * tmp;
tmp = proj * tmp;
tmp = tmp.array() / tmp(3);
tmp = tmp.array() * 0.5f + 0.5f;
tmp(0) = tmp(0) * viewportSize.x();
tmp(1) = tmp(1) * viewportSize.y();
return tmp.head(3);
}
Eigen::Vector3f igl::unproject(
const Eigen::Vector3f& win,
const Eigen::Matrix4f& model,
const Eigen::Matrix4f& proj,
const Eigen::Vector4f& viewport)
{
Eigen::Matrix4f Inverse = (proj * model).inverse();
Eigen::Vector4f tmp;
tmp << win, 1;
tmp(0) = (tmp(0) - viewport(0)) / viewport(2);
tmp(1) = (tmp(1) - viewport(1)) / viewport(3);
tmp = tmp.array() * 2.0f - 1.0f;
Eigen::Vector4f obj = Inverse * tmp;
obj /= obj(3);
return obj.head(3);
}