commit b6f334d74c3fb493f3163bb311dcc9713173c2ab
author Alex Perry <alex.perry96@gmail.com> Sat Mar 23 13:10:45 2019 -0700
committer Alex Perry <alex.perry96@gmail.com> Sat Mar 23 13:10:45 2019 -0700
tree b02b4b122f1270273161f325c585f7c9797e3ced
parent f9202e8a1a08a8847997efa20bd6d5174a1d81b8

normalize angles on the jevois

Change-Id: I051ebc9929512385654d422f3925a973ecae82d1

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";