int GetBestPoseFromCandidates(List <int> candidateList)
{
int bestPoseID = -1;
float lowestCost = float.MaxValue;
foreach (var i in candidateList)
{
float cost = Pose.ComparePoses(currentPose, _poses[i], lFootPosWeight, rFootPosWeight, neckPosWeight,
lFootVelWeight, rFootVelWeight, neckVelWeight, poseToVelocityRatio);
if (cost < lowestCost)
{
lowestCost = cost;
bestPoseID = i;
}
}
return(bestPoseID);
}
