Add unit tests to the Vision subsystem

[jkleiber]

Summary After the initial commands are added in #15, unit tests need to be made for the Vision subsystem

Work

• Add unit tests for the public functions of the Vision subsystem

Verification

• CI passes