Rigid trans

Could you review this code. Thanks!
We usually rename the template types ''typename TPoint Point'