commit 48dbc74694416825d271f34b19f83b4ed2ea7bf9
author Austin Schuh <austin.linux@gmail.com> Sat Mar 23 14:12:49 2019 -0700
committer Gerrit Code Review <gerrit@robotics.mvla.net> Sat Mar 23 14:12:49 2019 -0700
tree 824744891e871122593c77178c44a5403536d0bf
parent c32ada66d611aac0ec8f4198b6aec94667625000
parent acdabeb9e459276b8aaa4ad6084796f6f7e3eed4

Merge "Add a barebones websocket server for vision debug"

y2019/vision/BUILD

diff --git a/y2019/vision/BUILD b/y2019/vision/BUILD
index e19c8cd..186850c 100644
--- a/y2019/vision/BUILD
+++ b/y2019/vision/BUILD
@@ -67,6 +67,7 @@
     restricted_to = VISION_TARGETS,
     deps = [
         ":constants",
+        "//aos/util:math",
         "//aos/vision/blob:contour",
         "//aos/vision/blob:hierarchical_contour_merge",
         "//aos/vision/blob:region_alloc",

y2019/vision/target_geometry.cc

diff --git a/y2019/vision/target_geometry.cc b/y2019/vision/target_geometry.cc
index 6df1a27..91c8848 100644
--- a/y2019/vision/target_geometry.cc
+++ b/y2019/vision/target_geometry.cc
@@ -4,6 +4,8 @@
 
 #include <math.h>
 
+#include "aos/util/math.h"
+
 using ceres::NumericDiffCostFunction;
 using ceres::CENTRAL;
 using ceres::CostFunction;
@@ -306,6 +308,12 @@
   IR.backup_extrinsics = ExtrinsicParams::get(&params_4point[0]);
   IR.backup_solver_error = summary_4point2.final_cost;
 
+  // Normalize all angles to (-M_PI, M_PI]
+  IR.extrinsics.r1 = ::aos::math::NormalizeAngle(IR.extrinsics.r1);
+  IR.extrinsics.r2 = ::aos::math::NormalizeAngle(IR.extrinsics.r2);
+  IR.backup_extrinsics.r1 = ::aos::math::NormalizeAngle(IR.backup_extrinsics.r1);
+  IR.backup_extrinsics.r2 = ::aos::math::NormalizeAngle(IR.backup_extrinsics.r2);
+
   if (verbose) {
     std::cout << "rup = " << intrinsics_.mount_angle * 180 / M_PI << ";\n";
     std::cout << "fl = " << intrinsics_.focal_length << ";\n";