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 {