Exemple #1
0
void PoseRT::computeMeanPose(const std::vector<PoseRT> &poses, PoseRT &meanPose)
{
  meanPose = PoseRT();
  if (poses.empty())
  {
    return;
  }

  Mat meanTvec = meanPose.tvec;
  Mat meanRotationMatrix = meanPose.getRotationMatrix();
  for (size_t i = 0; i < poses.size(); ++i)
  {
    meanTvec += poses[i].tvec;
    meanRotationMatrix += poses[i].getRotationMatrix();
  }
  meanTvec /= poses.size();
  meanRotationMatrix /= poses.size();

  SVD svd;
  Mat w, u, vt;
  svd.compute(meanRotationMatrix, w, u, vt, SVD::FULL_UV);
  Mat meanOrthogonalRotationMatrix = u * vt;

  meanPose.tvec = meanTvec;
  meanPose.setRotation(meanOrthogonalRotationMatrix);
}