Wrote exceptions, factorgraph unit tests and several other improvements