Merge "Allow resetting localizer via queue"
diff --git a/frc971/control_loops/c2d.h b/frc971/control_loops/c2d.h
index 1bf99a2..b6f4198 100644
--- a/frc971/control_loops/c2d.h
+++ b/frc971/control_loops/c2d.h
@@ -64,8 +64,60 @@
   Eigen::Matrix<Scalar, num_states, num_states> phi22 =
       phi.block(num_states, num_states, num_states, num_states);
 
-  *Q_d = phi22.transpose() * phi12;
-  *Q_d = (*Q_d + Q_d->transpose()) / static_cast<Scalar>(2.0);
+  Qtemp = phi22.transpose() * phi12;
+  *Q_d = (Qtemp + Qtemp.transpose()) / static_cast<Scalar>(2.0);
+}
+
+// Does a faster approximation for the discretizing A/Q, for if solving a 2Nx2N
+// matrix exponential is too expensive.
+// Basic reasoning/method:
+// The original algorithm does a matrix exponential on a 2N x 2N matrix (the
+// block matrix made of of A and Q). This is extremely expensive for larg-ish
+// matrices. This function takes advantage of the structure of the matrix
+// we are taking the exponential and notes that we care about two things:
+// 1) The exponential of A*t, which is only NxN and so is relatively cheap.
+// 2) The upper-right quarter of the 2Nx2N matrix, which we can approximate
+//    using a taylor series to several terms and still be substantially cheaper
+//    than taking the big exponential.
+template <typename Scalar, int num_states>
+void DiscretizeQAFast(
+    const Eigen::Matrix<Scalar, num_states, num_states> &Q_continuous,
+    const Eigen::Matrix<Scalar, num_states, num_states> &A_continuous,
+    ::std::chrono::nanoseconds dt,
+    Eigen::Matrix<Scalar, num_states, num_states> *Q_d,
+    Eigen::Matrix<Scalar, num_states, num_states> *A_d) {
+  Eigen::Matrix<Scalar, num_states, num_states> Qtemp =
+      (Q_continuous + Q_continuous.transpose()) / static_cast<Scalar>(2.0);
+  Scalar dt_d =
+      ::std::chrono::duration_cast<::std::chrono::duration<Scalar>>(dt).count();
+  Eigen::Matrix<Scalar, num_states, num_states> last_term = Qtemp;
+  double last_coeff = dt_d;
+  const Eigen::Matrix<Scalar, num_states, num_states> At =
+      A_continuous.transpose();
+  Eigen::Matrix<Scalar, num_states, num_states> Atn = At;
+  Eigen::Matrix<Scalar, num_states, num_states> phi12 = last_term * last_coeff;
+  Eigen::Matrix<Scalar, num_states, num_states> phi22 =
+      At.Identity() + Atn * last_coeff;
+  // TODO(james): Tune this once we have the robot up; ii < 6 is enough to get
+  // beyond any real numerical issues. 5 should be fine, but just happened to
+  // kick a test over to failing. 4 would probably work.
+  for (int ii = 2; ii < 6; ++ii) {
+    Eigen::Matrix<Scalar, num_states, num_states> next_term =
+        -A_continuous * last_term + Qtemp * Atn;
+    last_coeff *= dt_d / static_cast<Scalar>(ii);
+    phi12 += next_term * last_coeff;
+
+    last_term = next_term;
+
+    Atn *= At;
+    phi22 += last_coeff * Atn;
+  }
+  Eigen::Matrix<Scalar, num_states, num_states> phi22t =
+    phi22.transpose();
+
+  Qtemp = phi22t * phi12;
+  *Q_d = (Qtemp + Qtemp.transpose()) / static_cast<Scalar>(2.0);
+  *A_d = phi22t;
 }
 
 }  // namespace controls
diff --git a/frc971/control_loops/c2d_test.cc b/frc971/control_loops/c2d_test.cc
index dcdb048..a0c4deb 100644
--- a/frc971/control_loops/c2d_test.cc
+++ b/frc971/control_loops/c2d_test.cc
@@ -69,6 +69,21 @@
       << "\nQ_d_integrated:\n" << Q_d_integrated;
 }
 
+// Tests that the "fast" discretization produces nearly identical results.
+TEST_F(C2DTest, DiscretizeQAFast) {
+  Eigen::Matrix<double, 2, 2> Q_d;
+  Eigen::Matrix<double, 2, 2> Q_d_fast;
+  Eigen::Matrix<double, 2, 2> A_d;
+  Eigen::Matrix<double, 2, 2> A_d_fast;
+  Eigen::Matrix<double, 2, 1> B_d;
+  const auto dt = ::std::chrono::seconds(1);
+  DiscretizeQ(Q_continuous, A_continuous, dt, &Q_d);
+  C2D(A_continuous, B_continuous, dt, &A_d, &B_d);
+  DiscretizeQAFast(Q_continuous, A_continuous, dt, &Q_d_fast, &A_d_fast);
+  EXPECT_LT((Q_d - Q_d_fast).norm(), 1e-20);
+  EXPECT_LT((A_d - A_d_fast).norm(), 1e-20);
+}
+
 }  // namespace testing
 }  // namespace controls
 }  // namespace frc971
diff --git a/frc971/control_loops/drivetrain/hybrid_ekf.h b/frc971/control_loops/drivetrain/hybrid_ekf.h
index 01a49a9..82f409c 100644
--- a/frc971/control_loops/drivetrain/hybrid_ekf.h
+++ b/frc971/control_loops/drivetrain/hybrid_ekf.h
@@ -251,12 +251,8 @@
                    StateSquare *P) {
     StateSquare A_c = AForState(*state);
     StateSquare A_d;
-    Eigen::Matrix<Scalar, kNStates, 0> dummy_B;
-    controls::C2D<Scalar, kNStates, 0>(
-        A_c, Eigen::Matrix<Scalar, kNStates, 0>::Zero(), dt, &A_d, &dummy_B);
     StateSquare Q_d;
-    controls::DiscretizeQ(Q_continuous_, A_c, dt, &Q_d);
-    *P = A_d * *P * A_d.transpose() + Q_d;
+    controls::DiscretizeQAFast(Q_continuous_, A_c, dt, &Q_d, &A_d);
 
     *state = RungeKuttaU(
         [this](const State &X,
@@ -264,6 +260,9 @@
         *state, U,
         ::std::chrono::duration_cast<::std::chrono::duration<double>>(dt)
             .count());
+
+    StateSquare Ptemp = A_d * *P * A_d.transpose() + Q_d;
+    *P = Ptemp;
   }
 
   void CorrectImpl(const Eigen::Matrix<Scalar, kNOutputs, kNOutputs> &R,
@@ -274,8 +273,9 @@
     Eigen::Matrix<Scalar, kNStates, kNOutputs> PH = *P * H.transpose();
     Eigen::Matrix<Scalar, kNOutputs, kNOutputs> S = H * PH + R;
     Eigen::Matrix<Scalar, kNStates, kNOutputs> K = PH * S.inverse();
-    *state = *state + K * err;
-    *P = (StateSquare::Identity() - K * H) * *P;
+    *state += K * err;
+    StateSquare Ptemp = (StateSquare::Identity() - K * H) * *P;
+    *P = Ptemp;
   }
 
   void ProcessObservation(Observation *obs, const std::chrono::nanoseconds dt,
diff --git a/y2019/vision/debug_viewer.cc b/y2019/vision/debug_viewer.cc
index 1db8e33..35f2af3 100644
--- a/y2019/vision/debug_viewer.cc
+++ b/y2019/vision/debug_viewer.cc
@@ -59,7 +59,7 @@
 
   void InstallViewer(aos::vision::BlobStreamViewer *viewer) override {
     viewer_ = viewer;
-    viewer_->SetScale(0.75);
+    viewer_->SetScale(2.0);
     overlays_.push_back(&overlay_);
     overlays_.push_back(finder_.GetOverlay());
     viewer_->view()->SetOverlays(&overlays_);
diff --git a/y2019/vision/global_calibration.cc b/y2019/vision/global_calibration.cc
index 186e025..c962fc9 100644
--- a/y2019/vision/global_calibration.cc
+++ b/y2019/vision/global_calibration.cc
@@ -114,7 +114,7 @@
   Solver::Summary summary;
 
   std::cout << summary.BriefReport() << "\n";
-  auto intrinsics_ = IntrinsicParams::get(&intrinsics[0]);
+  IntrinsicParams intrinsics_ = IntrinsicParams::get(&intrinsics[0]);
   std::cout << "rup = " << intrinsics_.mount_angle * 180 / M_PI << ";\n";
   std::cout << "fl = " << intrinsics_.focal_length << ";\n";
   std::cout << "error = " << summary.final_cost << ";\n";
@@ -125,8 +125,9 @@
                                        std::to_string(i) + ".yuyv");
 
     aos::vision::ImageFormat fmt{640, 480};
-    aos::vision::BlobList imgs = aos::vision::FindBlobs(
-        aos::vision::DoThresholdYUYV(fmt, frame.data.data(), 120));
+    aos::vision::BlobList imgs =
+        aos::vision::FindBlobs(aos::vision::DoThresholdYUYV(
+            fmt, frame.data.data(), TargetFinder::GetThresholdValue()));
     finder_.PreFilter(&imgs);
 
     bool verbose = false;
@@ -153,16 +154,19 @@
     // Use the solver to generate an intermediate version of our results.
     std::vector<IntermediateResult> results;
     for (const Target &target : target_list) {
-      auto target_value = target.toPointList();
-      auto template_value = finder_.GetTemplateTarget().toPointList();
+      ::std::array<aos::vision::Vector<2>, 8> target_value =
+          target.ToPointList();
+      ::std::array<aos::vision::Vector<2>, 8> template_value =
+          finder_.GetTemplateTarget().ToPointList();
 
-      auto *extrinsics = new double[ExtrinsicParams::kNumParams];
+      // TODO(austin): Memory leak below, fix.
+      double *extrinsics = new double[ExtrinsicParams::kNumParams];
       ExtrinsicParams().set(extrinsics);
       all_extrinsics.push_back({std::unique_ptr<double[]>(extrinsics), i});
 
       for (size_t j = 0; j < 8; ++j) {
-        auto temp = template_value[j];
-        auto targ = target_value[j];
+        aos::vision::Vector<2> temp = template_value[j];
+        aos::vision::Vector<2> targ = target_value[j];
 
         auto ftor = [temp, targ, i](const double *const intrinsics,
                                     const double *const extrinsics,
@@ -184,7 +188,7 @@
 
       auto ftor = [&info, i](const double *const extrinsics,
                              const double *const geometry, double *residual) {
-        auto err = GetError(info, extrinsics, geometry, i);
+        std::array<double, 3> err = GetError(info, extrinsics, geometry, i);
         residual[0] = 32.0 * err[0];
         residual[1] = 32.0 * err[1];
         residual[2] = 32.0 * err[2];
@@ -206,8 +210,8 @@
 
   {
     std::cout << summary.BriefReport() << "\n";
-    auto intrinsics_ = IntrinsicParams::get(&intrinsics[0]);
-    auto geometry_ = CameraGeometry::get(&geometry[0]);
+    IntrinsicParams intrinsics_ = IntrinsicParams::get(&intrinsics[0]);
+    CameraGeometry geometry_ = CameraGeometry::get(&geometry[0]);
     std::cout << "rup = " << intrinsics_.mount_angle * 180 / M_PI << ";\n";
     std::cout << "fl = " << intrinsics_.focal_length << ";\n";
     std::cout << "error = " << summary.final_cost << ";\n";
@@ -222,11 +226,11 @@
     std::cout << "camera_barrel = " << intrinsics_.barrel_mount * 180.0 / M_PI
               << "\n";
 
-    for (auto &sample : all_extrinsics) {
+    for (const Sample &sample : all_extrinsics) {
       int i = sample.i;
       double *data = sample.extrinsics.get();
 
-      auto extn = ExtrinsicParams::get(data);
+      ExtrinsicParams extn = ExtrinsicParams::get(data);
 
       auto err = GetError(info, data, &geometry[0], i);
 
diff --git a/y2019/vision/target_finder.cc b/y2019/vision/target_finder.cc
index be2e262..d39764a 100644
--- a/y2019/vision/target_finder.cc
+++ b/y2019/vision/target_finder.cc
@@ -268,10 +268,19 @@
   TargetComponent new_target;
   for (const std::vector<Segment<2>> &poly : seg_list) {
     // Reject missized pollygons for now. Maybe rectify them here in the future;
-    if (poly.size() != 4) continue;
+    if (poly.size() != 4) {
+      continue;
+    }
     std::vector<Vector<2>> corners;
     for (size_t i = 0; i < 4; ++i) {
-      corners.push_back(poly[i].Intersect(poly[(i + 1) % 4]));
+      Vector<2> corner = poly[i].Intersect(poly[(i + 1) % 4]);
+      if (::std::isnan(corner.x()) || ::std::isnan(corner.y())) {
+        break;
+      }
+      corners.push_back(corner);
+    }
+    if (corners.size() != 4) {
+      continue;
     }
 
     // Select the closest two points. Short side of the rectangle.
diff --git a/y2019/vision/target_finder.h b/y2019/vision/target_finder.h
index 4583690..666ae2f 100644
--- a/y2019/vision/target_finder.h
+++ b/y2019/vision/target_finder.h
@@ -25,7 +25,7 @@
   aos::vision::RangeImage Threshold(aos::vision::ImagePtr image);
 
   // Value against which we threshold.
-  uint8_t GetThresholdValue() { return 120; }
+  static uint8_t GetThresholdValue() { return 100; }
 
   // filter out obvious or durranged blobs.
   void PreFilter(BlobList *imgs);
diff --git a/y2019/vision/target_geometry.cc b/y2019/vision/target_geometry.cc
index 1c1fb99..c8b2d84 100644
--- a/y2019/vision/target_geometry.cc
+++ b/y2019/vision/target_geometry.cc
@@ -45,7 +45,7 @@
   return out;
 }
 
-std::array<Vector<2>, 8> Target::toPointList() const {
+std::array<Vector<2>, 8> Target::ToPointList() const {
   return std::array<Vector<2>, 8>{{right.top, right.inside, right.bottom,
                                    right.outside, left.top, left.inside,
                                    left.bottom, left.outside}};
@@ -53,20 +53,20 @@
 
 Vector<2> Project(Vector<2> pt, const IntrinsicParams &intrinsics,
                   const ExtrinsicParams &extrinsics) {
-  double y = extrinsics.y;
-  double z = extrinsics.z;
-  double r1 = extrinsics.r1;
-  double r2 = extrinsics.r2;
-  double rup = intrinsics.mount_angle;
-  double rbarrel = intrinsics.barrel_mount;
-  double fl = intrinsics.focal_length;
+  const double y = extrinsics.y;
+  const double z = extrinsics.z;
+  const double r1 = extrinsics.r1;
+  const double r2 = extrinsics.r2;
+  const double rup = intrinsics.mount_angle;
+  const double rbarrel = intrinsics.barrel_mount;
+  const double fl = intrinsics.focal_length;
 
   ::Eigen::Matrix<double, 1, 3> pts{pt.x(), pt.y() + y, 0.0};
 
   {
-    double theta = r1;
-    double s = sin(theta);
-    double c = cos(theta);
+    const double theta = r1;
+    const double s = sin(theta);
+    const double c = cos(theta);
     pts = (::Eigen::Matrix<double, 3, 3>() << c, 0, -s, 0, 1, 0, s, 0,
            c).finished() *
           pts.transpose();
@@ -75,9 +75,9 @@
   pts(2) += z;
 
   {
-    double theta = r2;
-    double s = sin(theta);
-    double c = cos(theta);
+    const double theta = r2;
+    const double s = sin(theta);
+    const double c = cos(theta);
     pts = (::Eigen::Matrix<double, 3, 3>() << c, 0, -s, 0, 1, 0, s, 0,
            c).finished() *
           pts.transpose();
@@ -85,9 +85,9 @@
 
   // TODO: Apply 15 degree downward rotation.
   {
-    double theta = rup;
-    double s = sin(theta);
-    double c = cos(theta);
+    const double theta = rup;
+    const double s = sin(theta);
+    const double c = cos(theta);
 
     pts = (::Eigen::Matrix<double, 3, 3>() << 1, 0, 0, 0, c, -s, 0, s,
            c).finished() *
@@ -100,9 +100,9 @@
         pts.transpose();
 
   // TODO: Final image projection.
-  ::Eigen::Matrix<double, 1, 3> res = pts;
+  const ::Eigen::Matrix<double, 1, 3> res = pts;
 
-  float scale = fl / res.z();
+  const float scale = fl / res.z();
   return Vector<2>(res.x() * scale + 320.0, 240.0 - res.y() * scale);
 }
 
@@ -153,12 +153,13 @@
 
   Problem problem;
 
-  auto target_value = target.toPointList();
-  auto template_value = target_template_.toPointList();
+  ::std::array<aos::vision::Vector<2>, 8> target_value = target.ToPointList();
+  ::std::array<aos::vision::Vector<2>, 8> template_value =
+      target_template_.ToPointList();
 
   for (size_t i = 0; i < 8; ++i) {
-    auto a = template_value[i];
-    auto b = target_value[i];
+    aos::vision::Vector<2> a = template_value[i];
+    aos::vision::Vector<2> b = target_value[i];
 
     problem.AddResidualBlock(
         new NumericDiffCostFunction<RuntimeCostFunctor, CENTRAL, 2, 4>(
diff --git a/y2019/vision/target_sender.cc b/y2019/vision/target_sender.cc
index 06235ff..f322aeb 100644
--- a/y2019/vision/target_sender.cc
+++ b/y2019/vision/target_sender.cc
@@ -382,6 +382,7 @@
       char data[kBufferSize];
       ssize_t n = read(itsDev, &data[0], kBufferSize);
       if (n >= 1) {
+        LOG(INFO, "Serial bytes: %zd", n);
         cobs.ParseData(gsl::span<const char>(&data[0], n));
         auto packet = cobs.received_packet();
         if (!packet.empty()) {
diff --git a/y2019/vision/target_types.h b/y2019/vision/target_types.h
index a20ecf7..dea9ba6 100644
--- a/y2019/vision/target_types.h
+++ b/y2019/vision/target_types.h
@@ -40,7 +40,7 @@
 
   static Target MakeTemplate();
   // Get the points in some order (will match against the template).
-  std::array<aos::vision::Vector<2>, 8> toPointList() const;
+  std::array<aos::vision::Vector<2>, 8> ToPointList() const;
 };
 
 struct ExtrinsicParams {