| 45 | { |
| 46 | |
| 47 | Eigen::Vector3d |
| 48 | SkeletonDetection::averageOverValidJoints( |
| 49 | const std::vector<rtpose_wrapper::Joint3DMsg>& joints) |
| 50 | { |
| 51 | Eigen::Vector3d v(0.0,0.0,0.0); |
| 52 | auto acc_lambda = [](Eigen::Vector3d m, |
| 53 | const rtpose_wrapper::Joint3DMsg& m1) |
| 54 | { |
| 55 | return std::isfinite(m1.x) and std::isfinite(m1.y) and std::isfinite(m1.z)? |
| 56 | m + Eigen::Vector3d(m1.x,m1.y,m1.z): |
| 57 | m; |
| 58 | }; |
| 59 | auto count_lambda = [](const rtpose_wrapper::Joint3DMsg& m) { |
| 60 | return not ((fabs(m.x) < 0.01 and fabs(m.y) < 0.01 and fabs(m.z) < 0.01) |
| 61 | or not (std::isfinite(m.x) and std::isfinite(m.y) |
| 62 | and std::isfinite(m.z)) |
| 63 | );}; |
| 64 | v = std::accumulate(joints.begin(), |
| 65 | joints.end(), |
| 66 | Eigen::Vector3d(0.0,0.0,0.0), |
| 67 | acc_lambda); |
| 68 | uint count = std::count_if(joints.begin(), |
| 69 | joints.end(), |
| 70 | count_lambda); |
| 71 | return count == 0 ? Eigen::Vector3d(0.0,0.0,0.0) : Eigen::Vector3d(v / count); |
| 72 | |
| 73 | } |
| 74 | |
| 75 | double |
| 76 | SkeletonDetection::varianceOverValidJoints( |