add rotation matrix to rodrigues function

essentially ceres' code