Implemented pose inference from scale invariant world points

Also implemented matlab like matrix Replicate and got rid of rids used
to make up for the lack of it.