/*
* You can use GPU for that
* */
void
Transform_Cloud ( matrix <Point_XYZI> & In_Cloud,
matrix <Point_XYZI> & Out_Cloud,
Eigen::Affine3f & Transform )
{
if ( & In_Cloud != & Out_Cloud )
Out_Cloud.Set_Dimensions ( In_Cloud.Get_Dimensions() );
// Dataset might contain NaNs and Infs, so check for them first,
// otherwise we get errors during the multiplication (?)
for (size_t i = 0; i < In_Cloud.Get_Num_Of_Elem (); ++i) {
XYZI cur_point = In_Cloud.data[i];
if ( ! isfinite (cur_point.x) ||
! isfinite (cur_point.y) ||
! isfinite (cur_point.z) )
continue;
// FIXME - rewrite -
// too complex
// use SSE ???
Eigen::Matrix<float, 3, 1> pt ( cur_point.x, cur_point.y, cur_point.z );
XYZI result;
result.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));
result.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));
result.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));
result.w = cur_point.w;
Out_Cloud.data[i] = result;
}
}
void
convert_to_point_cloud (
matrix <Point_XYZI_Color> & Point_Cloud,
pcl::PointCloud < pcl::PointXYZ> & pcl_cloud
)
{
for (unsigned int z = 0; z < Point_Cloud.Get_Num_Of_Elem(); z++)
{
pcl::PointXYZ one_more;
XYZI_Color cur_point = Point_Cloud.data[z];
one_more.x = cur_point.vertex.x;
one_more.y = cur_point.vertex.y;
one_more.y = cur_point.vertex.z;
pcl_cloud.push_back (one_more);
}
}
void
convert_to_point_cloud (
matrix <Point_XYZI> & Vertex_Point_Cloud,
pcl::PointCloud <pcl::PointXYZI> & pcl_cloud
)
{
for (unsigned int z = 0; z < Vertex_Point_Cloud.Get_Num_Of_Elem(); z++)
{
pcl::PointXYZI one_more;
XYZI cur = Vertex_Point_Cloud[z];
one_more.x = cur.x;
one_more.y = cur.y;
one_more.z = cur.z;
one_more.intensity = cur.w;
pcl_cloud.push_back (one_more);
}
}
