Finish roboRIO-side vision code
It all builds with --cpu=roborio now, and successfully starts listening
for data when run on my laptop. I think it should work...
Change-Id: I5cb107f61ad7a63b2e0926a92c3238893719c5f5
diff --git a/y2016/vision/stereo_geometry.cc b/y2016/vision/stereo_geometry.cc
new file mode 100644
index 0000000..b58cb82
--- /dev/null
+++ b/y2016/vision/stereo_geometry.cc
@@ -0,0 +1,18 @@
+#include "y2016/vision/stereo_geometry.h"
+
+namespace y2016 {
+namespace vision {
+
+Calibration FindCalibrationForRobotOrDie(
+ const ::std::string &robot_name, const CalibrationFile &calibration_file) {
+ for (const RobotCalibration &calibration : calibration_file.calibration()) {
+ if (calibration.robot() == robot_name) {
+ return calibration.calibration();
+ }
+ }
+ LOG(FATAL, "no calibration for %s found in %s\n", robot_name.c_str(),
+ calibration_file.ShortDebugString().c_str());
+}
+
+} // namespace vision
+} // namespace y2016