Update Bundle Utility classes for LiDAR work (#4716)

* Initial utilities hand merge

* Clean up for tests

* Fixed disclaimer for new files

* Removed BundleConstraint

* Moved Constraint into utilities folder

* Fixed header and SpciePosition test

* Moved BundleResults unit test to GTest

* Moved BundleSolutionInfo test to gtest

* BundleControlPoint gtest

* Added constrained point test

* updated BundleResults saving and test

* Updated BundleSolutionInfo Test