math: Clean code in HomographyFunctor

There was an absurd amount of std containers being allocated here. I
think young Zack did all this.