Removed duplicate copy of Normalize3d and added MakeHomogeneous function
to SE3