Fixed opencv2 issues. Finished up the SAP Ndof function.