void VisMotCoord::bodyFiltering()
{
Vector3f camz;
Eigen::Matrix4f eigen_transform;
transformAsMatrix (camPos_updated, eigen_transform);
camz[0] = eigen_transform(0,2);
camz[1] = eigen_transform(1,2);
camz[2] = eigen_transform(2,2);
model.numFilteredFaces = 0;
for(int i=0;i<model.numPart();i++){
model.filteringPart(i, camz);
}
}
Transformation3 eigen2Hogman(const Eigen::Matrix4f eigen_mat) {
std::clock_t starttime=std::clock();
Eigen::Affine3f eigen_transform(eigen_mat);
Eigen::Quaternionf eigen_quat(eigen_transform.rotation());
Vector3 translation(eigen_mat(0, 3), eigen_mat(1, 3), eigen_mat(2, 3));
Quaternion rotation(eigen_quat.x(), eigen_quat.y(), eigen_quat.z(),
eigen_quat.w());
Transformation3 result(translation, rotation);
ROS_INFO_STREAM_COND_NAMED(( (std::clock()-starttime) / (double)CLOCKS_PER_SEC) > global_min_time_reported, "timings", __FUNCTION__ << " runtime: "<< ( std::clock() - starttime ) / (double)CLOCKS_PER_SEC <<"sec");
return result;
}
