Added a condition requiring the homography to be non-singular and fixed matrix sizing issues