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); }