template <typename PointT, typename Scalar> inline PointT
pcl::transformPointWithNormal (const PointT &point,
const Eigen::Transform<Scalar, 3, Eigen::Affine> &transform)
{
PointT ret = point;
ret.getVector3fMap () = transform * point.getVector3fMap ();
ret.getNormalVector3fMap () = transform.rotation () * point.getNormalVector3fMap ();
return (ret);
}
/** \brief Calculate centroid of voxel.
* \param[out] centroid_arg the resultant centroid of the voxel
*/
void
getCentroid (PointT& centroid_arg) const
{
using namespace pcl::common;
if (point_counter_)
{
centroid_arg.getVector3fMap() = (pt_ / static_cast<double> (point_counter_)).cast<float>();
centroid_arg.getNormalVector3fMap() = n_.normalized().cast<float>();
centroid_arg.r = static_cast<unsigned char>(r_ / point_counter_);
centroid_arg.g = static_cast<unsigned char>(g_ / point_counter_);
centroid_arg.b = static_cast<unsigned char>(b_ / point_counter_);
}
else
{
centroid_arg.x = std::numeric_limits<float>::quiet_NaN();
centroid_arg.y = std::numeric_limits<float>::quiet_NaN();
centroid_arg.z = std::numeric_limits<float>::quiet_NaN();
centroid_arg.normal_x = std::numeric_limits<float>::quiet_NaN();
centroid_arg.normal_y = std::numeric_limits<float>::quiet_NaN();
centroid_arg.normal_z = std::numeric_limits<float>::quiet_NaN();
centroid_arg.r = 0;
centroid_arg.g = 0;
centroid_arg.b = 0;
}
}