commit 2ccf8c58e261654d5f05c06c2528067107eb45c8
author Brian Silverman <brians> Tue Mar 15 00:22:26 2016 -0400
committer Brian Silverman <brians> Tue Mar 15 19:13:08 2016 -0400
tree 7cc37a7d37737a5ce0248e2ebbe30a67eb29e5fd
parent 50c77b51b81d465dce696b795dbc484193df6d4e

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

y2016/vision/stereo_geometry.h

diff --git a/y2016/vision/stereo_geometry.h b/y2016/vision/stereo_geometry.h
new file mode 100644
index 0000000..93f3892
--- /dev/null
+++ b/y2016/vision/stereo_geometry.h
@@ -0,0 +1,68 @@
+#ifndef Y2016_VISION_STEREO_GEOMETRY_H_
+#define Y2016_VISION_STEREO_GEOMETRY_H_
+
+#include <string>
+
+#include "aos/vision/math/vector.h"
+#include "aos/common/logging/logging.h"
+
+#include "y2016/vision/calibration.pb.h"
+
+namespace y2016 {
+namespace vision {
+
+// Returns the contents of the calibration file which are embedded into the
+// code.
+CalibrationFile EmbeddedCalibrationFile();
+
+// Returns the embedded Calibration for the given robot_name or LOG(FATAL)s.
+Calibration FindCalibrationForRobotOrDie(
+    const ::std::string &robot_name, const CalibrationFile &calibration_file);
+
+class StereoGeometry {
+ public:
+  StereoGeometry(const ::std::string &robot_name)
+      : calibration_(FindCalibrationForRobotOrDie(robot_name,
+                                                  EmbeddedCalibrationFile())) {}
+
+  // Returns the forward world distance in meters corresponding to the given
+  // distance in pixels between the two cameras.
+  double PixelToWorldDistance(double pixel_distance) {
+    return (calibration_.center_center_dist() *
+            calibration_.camera_image_width() * calibration_.focal_length() /
+            (pixel_distance + calibration_.center_center_skew())) +
+           calibration_.measure_camera_offset();
+  }
+
+  // Converts locations on both cameras to the distance between the locations
+  // and both angles.
+  // Returns (via pointers) the distance to the target in meters, the horizontal
+  // angle to the target in radians, and the vertical angle to the target in
+  // radians.
+  // TODO(Brian): Figure out which way is positive on the angles.
+  void Process(const ::aos::vision::Vector<2> &center0,
+               const ::aos::vision::Vector<2> &center1, double *distance,
+               double *horizontal_angle, double *vertical_angle) {
+    *distance = PixelToWorldDistance(center1.x() - center0.x());
+    const double average_x = (center0.x() + center1.x()) / 2.0 -
+                             calibration_.camera_image_width() / 2.0;
+    const double average_y = (center0.y() + center1.y()) / 2.0 -
+                             calibration_.camera_image_height() / 2.0;
+
+    *horizontal_angle =
+        std::atan2(average_x, calibration_.camera_image_width() *
+                                  calibration_.focal_length());
+    *vertical_angle = std::atan2(average_y, calibration_.camera_image_width() *
+                                                calibration_.focal_length());
+  }
+
+  const Calibration &calibration() const { return calibration_; }
+
+ private:
+  Calibration calibration_;
+};
+
+}  // namespace vision
+}  // namespace y2016
+
+#endif  // Y2016_VISION_STEREO_GEOMETRY_H_