Merge "Convert things from AOS logging to glog"
diff --git a/aos/vision/blob/threshold.cc b/aos/vision/blob/threshold.cc
index 36dcafe..46221b5 100644
--- a/aos/vision/blob/threshold.cc
+++ b/aos/vision/blob/threshold.cc
@@ -30,6 +30,7 @@
// The per-channel (YUYV) values in the current chunk.
uint8_t chunk_channels[2 * kChunkSize];
memcpy(&chunk_channels[0], current_row + x * kChunkSize * 2, 2 * kChunkSize);
+ __builtin_prefetch(current_row + (x + 1) * kChunkSize * 2);
for (int i = 0; i < kChunkSize; ++i) {
if ((chunk_channels[i * 2] > value) != in_range) {
@@ -51,5 +52,74 @@
return RangeImage(0, std::move(result));
}
+FastYuyvYPooledThresholder::FastYuyvYPooledThresholder() {
+ states_.fill(ThreadState::kWaitingForInputData);
+ for (int i = 0; i < kThreads; ++i) {
+ threads_[i] = std::thread([this, i]() { RunThread(i); });
+ }
+}
+
+FastYuyvYPooledThresholder::~FastYuyvYPooledThresholder() {
+ {
+ std::unique_lock<std::mutex> locker(mutex_);
+ quit_ = true;
+ condition_variable_.notify_all();
+ }
+ for (int i = 0; i < kThreads; ++i) {
+ threads_[i].join();
+ }
+}
+
+RangeImage FastYuyvYPooledThresholder::Threshold(ImageFormat fmt,
+ const char *data,
+ uint8_t value) {
+ input_format_ = fmt;
+ input_data_ = data;
+ input_value_ = value;
+ {
+ std::unique_lock<std::mutex> locker(mutex_);
+ for (int i = 0; i < kThreads; ++i) {
+ states_[i] = ThreadState::kProcessing;
+ }
+ condition_variable_.notify_all();
+ while (!AllThreadsDone()) {
+ condition_variable_.wait(locker);
+ }
+ }
+ std::vector<std::vector<ImageRange>> result;
+ result.reserve(fmt.h);
+ for (int i = 0; i < kThreads; ++i) {
+ result.insert(result.end(), outputs_[i].begin(), outputs_[i].end());
+ }
+ return RangeImage(0, std::move(result));
+}
+
+void FastYuyvYPooledThresholder::RunThread(int i) {
+ while (true) {
+ {
+ std::unique_lock<std::mutex> locker(mutex_);
+ while (states_[i] == ThreadState::kWaitingForInputData) {
+ if (quit_) {
+ return;
+ }
+ condition_variable_.wait(locker);
+ }
+ }
+
+ ImageFormat shard_format = input_format_;
+ CHECK_EQ(shard_format.h % kThreads, 0);
+ shard_format.h /= kThreads;
+
+ outputs_[i] = FastYuyvYThreshold(
+ shard_format, input_data_ + shard_format.w * 2 * shard_format.h * i,
+ input_value_);
+ {
+ std::unique_lock<std::mutex> locker(mutex_);
+ states_[i] = ThreadState::kWaitingForInputData;
+ condition_variable_.notify_all();
+ }
+ }
+}
+
} // namespace vision
} // namespace aos
diff --git a/aos/vision/blob/threshold.h b/aos/vision/blob/threshold.h
index 9891722..8251b3a 100644
--- a/aos/vision/blob/threshold.h
+++ b/aos/vision/blob/threshold.h
@@ -1,6 +1,10 @@
#ifndef AOS_VISION_BLOB_THRESHOLD_H_
#define AOS_VISION_BLOB_THRESHOLD_H_
+#include <condition_variable>
+#include <mutex>
+#include <thread>
+
#include "aos/vision/blob/range_image.h"
#include "aos/vision/image/image_types.h"
@@ -80,6 +84,58 @@
// This is implemented via some tricky bit shuffling that goes fast.
RangeImage FastYuyvYThreshold(ImageFormat fmt, const char *data, uint8_t value);
+// Manages a pool of threads which do sharded thresholding.
+class FastYuyvYPooledThresholder {
+ public:
+ // The number of threads we'll use.
+ static constexpr int kThreads = 4;
+
+ FastYuyvYPooledThresholder();
+ // Shuts down and joins the threads.
+ ~FastYuyvYPooledThresholder();
+
+ // Actually does a threshold, merges the result, and returns it.
+ RangeImage Threshold(ImageFormat fmt, const char *data, uint8_t value);
+
+ private:
+ enum class ThreadState {
+ // Each thread moves itself into this state once it's done processing the
+ // previous input data.
+ kWaitingForInputData,
+ // The main thread moves all the threads into this state once it has
+ // finished setting up new input data.
+ kProcessing,
+ };
+
+ // The main function for a thread.
+ void RunThread(int index);
+
+ // Returns true if all threads are currently done.
+ bool AllThreadsDone() const {
+ for (ThreadState state : states_) {
+ if (state != ThreadState::kWaitingForInputData) {
+ return false;
+ }
+ }
+ return true;
+ }
+
+ std::array<std::thread, kThreads> threads_;
+ // Protects access to the states_ and coordinates with condition_variable_.
+ std::mutex mutex_;
+ // Signals changes to states_ and quit_.
+ std::condition_variable condition_variable_;
+ bool quit_ = false;
+
+ std::array<ThreadState, kThreads> states_;
+
+ // Access to these is protected by coordination via states_.
+ ImageFormat input_format_;
+ const char *input_data_;
+ uint8_t input_value_;
+ std::array<RangeImage, kThreads> outputs_;
+};
+
} // namespace vision
} // namespace aos
diff --git a/frc971/control_loops/drivetrain/drivetrain.cc b/frc971/control_loops/drivetrain/drivetrain.cc
index 6a2ea76..2af6765 100644
--- a/frc971/control_loops/drivetrain/drivetrain.cc
+++ b/frc971/control_loops/drivetrain/drivetrain.cc
@@ -245,7 +245,8 @@
LOG_STRUCT(DEBUG, "localizer_control", *localizer_control_fetcher_);
localizer_->ResetPosition(monotonic_now, localizer_control_fetcher_->x,
localizer_control_fetcher_->y,
- localizer_control_fetcher_->theta);
+ localizer_control_fetcher_->theta,
+ localizer_control_fetcher_->theta_uncertainty);
}
localizer_->Update({last_last_left_voltage_, last_last_right_voltage_},
monotonic_now, position->left_encoder,
diff --git a/frc971/control_loops/drivetrain/hybrid_ekf.h b/frc971/control_loops/drivetrain/hybrid_ekf.h
index 42dc67a..f006bb4 100644
--- a/frc971/control_loops/drivetrain/hybrid_ekf.h
+++ b/frc971/control_loops/drivetrain/hybrid_ekf.h
@@ -484,7 +484,7 @@
dt_config_.make_kf_drivetrain_loop().observer().coefficients().R;
// TODO(james): The multipliers here are hand-waving things that I put in when
// tuning things. I haven't yet tried messing with these values again.
- encoder_noise_ = 0.05 * R_kf_drivetrain(0, 0);
+ encoder_noise_ = 0.5 * R_kf_drivetrain(0, 0);
gyro_noise_ = 0.1 * R_kf_drivetrain(2, 2);
}
diff --git a/frc971/control_loops/drivetrain/localizer.h b/frc971/control_loops/drivetrain/localizer.h
index 9866803..bd48315 100644
--- a/frc971/control_loops/drivetrain/localizer.h
+++ b/frc971/control_loops/drivetrain/localizer.h
@@ -50,7 +50,8 @@
double gyro_rate, double longitudinal_accelerometer) = 0;
// Reset the absolute position of the estimator.
virtual void ResetPosition(::aos::monotonic_clock::time_point t, double x,
- double y, double theta) = 0;
+ double y, double theta,
+ double theta_uncertainty) = 0;
// There are several subtly different norms floating around for state
// matrices. In order to avoid that mess, we jus tprovide direct accessors for
// the values that most people care about.
@@ -109,7 +110,7 @@
}
void ResetPosition(::aos::monotonic_clock::time_point t, double x, double y,
- double theta) override {
+ double theta, double /*theta_override*/) override {
const double left_encoder = ekf_.X_hat(StateIdx::kLeftEncoder);
const double right_encoder = ekf_.X_hat(StateIdx::kRightEncoder);
ekf_.ResetInitialState(t, (Ekf::State() << x, y, theta, left_encoder, 0,
diff --git a/frc971/control_loops/drivetrain/localizer.q b/frc971/control_loops/drivetrain/localizer.q
index 1ae0adc..8fef686 100644
--- a/frc971/control_loops/drivetrain/localizer.q
+++ b/frc971/control_loops/drivetrain/localizer.q
@@ -6,6 +6,7 @@
float x; // X position, meters
float y; // Y position, meters
float theta; // heading, radians
+ double theta_uncertainty; // Uncertainty in theta.
};
queue LocalizerControl localizer_control;
diff --git a/y2016/vision/BUILD b/y2016/vision/BUILD
index 33ffbbc..99c50fd 100644
--- a/y2016/vision/BUILD
+++ b/y2016/vision/BUILD
@@ -145,6 +145,10 @@
gtk_dependent_cc_binary(
name = "debug_receiver",
srcs = ["debug_receiver.cc"],
+ restricted_to = [
+ "//tools:k8",
+ "//tools:armhf-debian",
+ ],
visibility = ["//visibility:public"],
deps = [
":blob_filters",
diff --git a/y2016/vision/tools/BUILD b/y2016/vision/tools/BUILD
index 763328d..edbfca5 100644
--- a/y2016/vision/tools/BUILD
+++ b/y2016/vision/tools/BUILD
@@ -1,17 +1,22 @@
-load('//tools/build_rules:gtk_dependent.bzl', 'gtk_dependent_cc_binary', 'gtk_dependent_cc_library')
+load("//tools/build_rules:gtk_dependent.bzl", "gtk_dependent_cc_binary", "gtk_dependent_cc_library")
-gtk_dependent_cc_binary(name = "blob_stream_replay",
- srcs = ["blob_stream_replay.cc"],
- deps = [
- "//aos/vision/image:reader",
- "//aos/vision/image:jpeg_routines",
- "//aos/vision/image:image_stream",
- "//aos/vision/events:epoll_events",
- "//aos/vision/events:gtk_event",
- "//aos/vision/events:tcp_server",
- "//aos/vision/debug:debug_window",
- "//aos/vision/blob:range_image",
- "//aos/vision/blob:stream_view",
- "//y2016/vision:blob_filters",
- ],
+gtk_dependent_cc_binary(
+ name = "blob_stream_replay",
+ srcs = ["blob_stream_replay.cc"],
+ restricted_to = [
+ "//tools:k8",
+ "//tools:armhf-debian",
+ ],
+ deps = [
+ "//aos/vision/blob:range_image",
+ "//aos/vision/blob:stream_view",
+ "//aos/vision/debug:debug_window",
+ "//aos/vision/events:epoll_events",
+ "//aos/vision/events:gtk_event",
+ "//aos/vision/events:tcp_server",
+ "//aos/vision/image:image_stream",
+ "//aos/vision/image:jpeg_routines",
+ "//aos/vision/image:reader",
+ "//y2016/vision:blob_filters",
+ ],
)
diff --git a/y2019/actors/autonomous_actor.cc b/y2019/actors/autonomous_actor.cc
index 85d516c..1647f04 100644
--- a/y2019/actors/autonomous_actor.cc
+++ b/y2019/actors/autonomous_actor.cc
@@ -60,6 +60,7 @@
localizer_resetter->x = 1.0;
localizer_resetter->y = 1.5 * turn_scalar;
localizer_resetter->theta = M_PI;
+ localizer_resetter->theta_uncertainty = 0.0000001;
if (!localizer_resetter.Send()) {
LOG(ERROR, "Failed to reset localizer.\n");
}
diff --git a/y2019/control_loops/drivetrain/event_loop_localizer.cc b/y2019/control_loops/drivetrain/event_loop_localizer.cc
index f33f4e5..282ca9e 100644
--- a/y2019/control_loops/drivetrain/event_loop_localizer.cc
+++ b/y2019/control_loops/drivetrain/event_loop_localizer.cc
@@ -34,14 +34,19 @@
target_selector_(event_loop) {
localizer_.ResetInitialState(::aos::monotonic_clock::now(),
Localizer::State::Zero(), localizer_.P());
- ResetPosition(::aos::monotonic_clock::now(), 0.5, 3.4, 0.0);
+ ResetPosition(::aos::monotonic_clock::now(), 0.5, 3.4, 0.0, 0.0);
frame_fetcher_ = event_loop_->MakeFetcher<CameraFrame>(
".y2019.control_loops.drivetrain.camera_frames");
}
void EventLoopLocalizer::Reset(::aos::monotonic_clock::time_point now,
- const Localizer::State &state) {
- localizer_.ResetInitialState(now, state, localizer_.P());
+ const Localizer::State &state,
+ double theta_uncertainty) {
+ Localizer::StateSquare newP = localizer_.P();
+ if (theta_uncertainty > 0.0) {
+ newP(StateIdx::kTheta, StateIdx::kTheta) = theta_uncertainty;
+ }
+ localizer_.ResetInitialState(now, state, newP);
}
void EventLoopLocalizer::Update(
diff --git a/y2019/control_loops/drivetrain/event_loop_localizer.h b/y2019/control_loops/drivetrain/event_loop_localizer.h
index 6d5ca29..9255109 100644
--- a/y2019/control_loops/drivetrain/event_loop_localizer.h
+++ b/y2019/control_loops/drivetrain/event_loop_localizer.h
@@ -34,9 +34,9 @@
::aos::EventLoop *event_loop);
void Reset(::aos::monotonic_clock::time_point t,
- const Localizer::State &state);
+ const Localizer::State &state, double theta_uncertainty);
void ResetPosition(::aos::monotonic_clock::time_point t, double x, double y,
- double theta) override {
+ double theta, double theta_uncertainty) override {
// When we reset the state, we want to keep the encoder positions intact, so
// we copy from the original state and reset everything else.
Localizer::State new_state = localizer_.X_hat();
@@ -50,7 +50,8 @@
new_state(7, 0) = 0.0;
new_state(8, 0) = 0.0;
new_state(9, 0) = 0.0;
- Reset(t, new_state);
+
+ Reset(t, new_state, theta_uncertainty);
}
void Update(const ::Eigen::Matrix<double, 2, 1> &U,
diff --git a/y2019/control_loops/drivetrain/localized_drivetrain_test.cc b/y2019/control_loops/drivetrain/localized_drivetrain_test.cc
index b405588..c6928e0 100644
--- a/y2019/control_loops/drivetrain/localized_drivetrain_test.cc
+++ b/y2019/control_loops/drivetrain/localized_drivetrain_test.cc
@@ -71,7 +71,7 @@
localizer_state;
localizer_state.setZero();
localizer_state.block<3, 1>(0, 0) = xytheta;
- localizer_.Reset(monotonic_clock::now(), localizer_state);
+ localizer_.Reset(monotonic_clock::now(), localizer_state, 0.0);
}
void RunIteration() {
diff --git a/y2019/control_loops/drivetrain/localizer_test.cc b/y2019/control_loops/drivetrain/localizer_test.cc
index 281abdc..b53e9d7 100644
--- a/y2019/control_loops/drivetrain/localizer_test.cc
+++ b/y2019/control_loops/drivetrain/localizer_test.cc
@@ -614,7 +614,7 @@
.finished(),
/*noisify=*/true,
/*disturb=*/false,
- /*estimate_tolerance=*/0.25,
+ /*estimate_tolerance=*/0.3,
/*goal_tolerance=*/0.7,
})));
diff --git a/y2019/joystick_reader.cc b/y2019/joystick_reader.cc
index 7b714be..9d7117b 100644
--- a/y2019/joystick_reader.cc
+++ b/y2019/joystick_reader.cc
@@ -106,11 +106,11 @@
const ElevatorWristPosition kPanelCargoForwardPos{0.0, M_PI / 2.0};
const ElevatorWristPosition kPanelCargoBackwardPos{0.0, -M_PI / 2.0};
-const ElevatorWristPosition kBallForwardLowerPos{0.42, M_PI / 2.0};
-const ElevatorWristPosition kBallBackwardLowerPos{0.14, -M_PI / 2.0};
+const ElevatorWristPosition kBallForwardLowerPos{0.21, 1.27};
+const ElevatorWristPosition kBallBackwardLowerPos{0.40, -1.99};
-const ElevatorWristPosition kBallForwardMiddlePos{1.16, 1.546};
-const ElevatorWristPosition kBallBackwardMiddlePos{0.90, -1.546};
+const ElevatorWristPosition kBallForwardMiddlePos{0.90, 1.21};
+const ElevatorWristPosition kBallBackwardMiddlePos{1.17, -1.98};
const ElevatorWristPosition kBallForwardUpperPos{1.51, 0.961};
const ElevatorWristPosition kBallBackwardUpperPos{1.44, -1.217};
@@ -356,8 +356,10 @@
new_superstructure_goal->stilts.profile_params.max_acceleration = 2.0;
}
- if (superstructure_queue.status->stilts.position > 0.3) {
+ if (superstructure_queue.status->stilts.position > 0.1) {
elevator_wrist_pos_ = kClimbPos;
+ new_superstructure_goal->wrist.profile_params.max_acceleration = 10;
+ new_superstructure_goal->elevator.profile_params.max_acceleration = 6;
}
if (superstructure_queue.status->stilts.position > kDeployStiltPosition &&
diff --git a/y2019/vision/debug_viewer.cc b/y2019/vision/debug_viewer.cc
index 3c174b4..4f0e747 100644
--- a/y2019/vision/debug_viewer.cc
+++ b/y2019/vision/debug_viewer.cc
@@ -80,6 +80,7 @@
}
bool HandleBlobs(BlobList imgs, ImageFormat fmt) override {
+ const CameraGeometry camera_geometry = GetCamera(FLAGS_camera)->geometry;
imgs_last_ = imgs;
fmt_last_ = fmt;
// reset for next drawing cycle
@@ -174,8 +175,23 @@
// Check that our current results match possible solutions.
results = target_finder_.FilterResults(results, 0, draw_results_);
if (draw_results_) {
- for (const IntermediateResult &res : results) {
- DrawTarget(res, {0, 255, 0});
+ for (const IntermediateResult &result : results) {
+ ::std::cout << "Found target x: "
+ << camera_geometry.location[0] +
+ ::std::cos(camera_geometry.heading +
+ result.extrinsics.r2) *
+ result.extrinsics.z
+ << ::std::endl;
+ ::std::cout << "Found target y: "
+ << camera_geometry.location[1] +
+ ::std::sin(camera_geometry.heading +
+ result.extrinsics.r2) *
+ result.extrinsics.z
+ << ::std::endl;
+ ::std::cout << "Found target z: "
+ << camera_geometry.location[2] + result.extrinsics.y
+ << ::std::endl;
+ DrawTarget(result, {0, 255, 0});
}
}
@@ -320,11 +336,11 @@
BlobList imgs_last_;
ImageFormat fmt_last_;
bool draw_select_blob_ = false;
- bool draw_contours_ = false;
- bool draw_raw_poly_ = false;
+ bool draw_contours_ = true;
+ bool draw_raw_poly_ = true;
bool draw_components_ = false;
bool draw_raw_target_ = false;
- bool draw_raw_IR_ = false;
+ bool draw_raw_IR_ = true;
bool draw_results_ = true;
};
diff --git a/y2019/vision/server/www/field.ts b/y2019/vision/server/www/field.ts
index 8cb1282..b52eee8 100644
--- a/y2019/vision/server/www/field.ts
+++ b/y2019/vision/server/www/field.ts
@@ -29,17 +29,37 @@
const HP_Y = ((26 * 12 + 10.5) / 2 - 25.9) * IN_TO_M;
const HP_THETA = Math.PI;
+const HAB_LENGTH = 4 * FT_TO_M;
+const HALF_HAB_3_WIDTH = 2 * FT_TO_M;
+const HAB_2_WIDTH = (3 * 12 + 4) * IN_TO_M;
+const HALF_HAB_1_WIDTH = (6 * 12 + 3.25) * IN_TO_M;
+const HAB_1_LENGTH = (3 * 12 + 11.25) * IN_TO_M;
+
+const DEPOT_WIDTH = (12 + 10.625) * IN_TO_M;
+
export function drawField(ctx : CanvasRenderingContext2D) : void {
drawTargets(ctx);
+ drawHab(ctx);
}
function drawHab(ctx : CanvasRenderingContext2D) : void {
- ctx.fillstyle = 'rgb(100,100,100)';
- const habLeft = 5 * FT_TO_M;
- const habWidth = 5 * FT_TO_M;
- const habTop = -5 * FT_TO_M;
- const habHeight = 10 * FT_TO_M;
- ctx.fillRect(habLeft,habTop,habWidth,habHeight);
+ drawHalfHab(ctx);
+ ctx.save();
+
+ ctx.scale(1, -1);
+ drawHalfHab(ctx);
+
+ ctx.restore();
+}
+
+function drawHalfHab(ctx : CanvasRenderingContext2D) : void {
+ ctx.fillStyle = 'rgb(50, 50, 50)';
+ ctx.fillRect(0, 0, HAB_LENGTH, HALF_HAB_3_WIDTH);
+ ctx.fillStyle = 'rgb(100, 100, 100)';
+ ctx.fillRect(0, HALF_HAB_3_WIDTH, HAB_LENGTH, HAB_2_WIDTH);
+ ctx.fillStyle = 'rgb(200, 200, 200)';
+ ctx.fillRect(HAB_LENGTH, 0, HAB_1_LENGTH, HALF_HAB_1_WIDTH);
+ ctx.strokeRect(0, HALF_HAB_3_WIDTH + HAB_2_WIDTH, HAB_LENGTH, DEPOT_WIDTH);
}
function drawTargets(ctx : CanvasRenderingContext2D) : void {
diff --git a/y2019/vision/server/www/main.ts b/y2019/vision/server/www/main.ts
index 8dc0ea9..17b7139 100644
--- a/y2019/vision/server/www/main.ts
+++ b/y2019/vision/server/www/main.ts
@@ -2,8 +2,6 @@
import {drawField, drawTarget} from './field';
import {drawRobot} from './robot';
-const FIELD_WIDTH = 27 * FT_TO_M;
-
function main(): void {
const vis = new Visualiser();
}
@@ -14,9 +12,10 @@
private theta = 0;
private drawLocked = false;
- private lockedX = 0;
- private lockedY = 0;
- private lockedTheta = 0;
+ private targetLocked = false;
+ private targetX = 0;
+ private targetY = 0;
+ private targetTheta = 0;
constructor() {
const canvas = <HTMLCanvasElement>document.getElementById('field');
diff --git a/y2019/vision/target_finder.cc b/y2019/vision/target_finder.cc
index 77806d5..5860e5c 100644
--- a/y2019/vision/target_finder.cc
+++ b/y2019/vision/target_finder.cc
@@ -518,7 +518,12 @@
// Closer targets can have a higher error because they are bigger.
const double acceptable_error =
std::max(2 * (75 - 12 * result->extrinsics.z), 75.0);
- if (result->solver_error < acceptable_error) {
+ if (!result->good_corners) {
+ if (verbose) {
+ printf("Rejecting a target with bad corners: (%f, %f)\n",
+ result->solver_error, result->backup_solver_error);
+ }
+ } else if (result->solver_error < acceptable_error) {
if (verbose) {
printf("Using an 8 point solve: %f < %f \n", result->solver_error,
acceptable_error);
diff --git a/y2019/vision/target_geometry.cc b/y2019/vision/target_geometry.cc
index e647f1f..00375c1 100644
--- a/y2019/vision/target_geometry.cc
+++ b/y2019/vision/target_geometry.cc
@@ -313,8 +313,67 @@
// 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);
+ IR.backup_extrinsics.r1 =
+ ::aos::math::NormalizeAngle(IR.backup_extrinsics.r1);
+ IR.backup_extrinsics.r2 =
+ ::aos::math::NormalizeAngle(IR.backup_extrinsics.r2);
+
+ // Ok, let's look at how perpendicular the corners are.
+ // Vector from the outside to inside along the top on the left.
+ const ::Eigen::Vector2d top_left_vector =
+ (target.left.top.GetData() - target.left.inside.GetData())
+ .transpose()
+ .normalized();
+ // Vector up the outside of the left target.
+ const ::Eigen::Vector2d outer_left_vector =
+ (target.left.top.GetData() - target.left.outside.GetData())
+ .transpose()
+ .normalized();
+ // Vector up the inside of the left target.
+ const ::Eigen::Vector2d inner_left_vector =
+ (target.left.inside.GetData() - target.left.bottom.GetData())
+ .transpose()
+ .normalized();
+
+ // Vector from the outside to inside along the top on the right.
+ const ::Eigen::Vector2d top_right_vector =
+ (target.right.top.GetData() - target.right.inside.GetData())
+ .transpose()
+ .normalized();
+ // Vector up the outside of the right target.
+ const ::Eigen::Vector2d outer_right_vector =
+ (target.right.top.GetData() - target.right.outside.GetData())
+ .transpose()
+ .normalized();
+ // Vector up the inside of the right target.
+ const ::Eigen::Vector2d inner_right_vector =
+ (target.right.inside.GetData() - target.right.bottom.GetData())
+ .transpose()
+ .normalized();
+
+ // Now dot the vectors and use that to compute angles.
+ // Left side, outside corner.
+ const double left_outer_corner_dot =
+ (outer_left_vector.transpose() * top_left_vector)(0);
+ // Left side, inside corner.
+ const double left_inner_corner_dot =
+ (inner_left_vector.transpose() * top_left_vector)(0);
+ // Right side, outside corner.
+ const double right_outer_corner_dot =
+ (outer_right_vector.transpose() * top_right_vector)(0);
+ // Right side, inside corner.
+ const double right_inner_corner_dot =
+ (inner_right_vector.transpose() * top_right_vector)(0);
+
+ constexpr double kCornerThreshold = 0.35;
+ if (::std::abs(left_outer_corner_dot) < kCornerThreshold &&
+ ::std::abs(left_inner_corner_dot) < kCornerThreshold &&
+ ::std::abs(right_outer_corner_dot) < kCornerThreshold &&
+ ::std::abs(right_inner_corner_dot) < kCornerThreshold) {
+ IR.good_corners = true;
+ } else {
+ IR.good_corners = false;
+ }
if (verbose) {
std::cout << "rup = " << intrinsics_.mount_angle * 180 / M_PI << ";\n";
@@ -336,6 +395,21 @@
std::cout << "z = " << IR.backup_extrinsics.z / kInchesToMeters << ";\n";
std::cout << "r1 = " << IR.backup_extrinsics.r1 * 180 / M_PI << ";\n";
std::cout << "r2 = " << IR.backup_extrinsics.r2 * 180 / M_PI << ";\n";
+
+
+ printf("left upper outer corner angle: %f, top (%f, %f), outer (%f, %f)\n",
+ (outer_left_vector.transpose() * top_left_vector)(0),
+ top_left_vector(0, 0), top_left_vector(1, 0),
+ outer_left_vector(0, 0), outer_left_vector(1, 0));
+ printf("left upper inner corner angle: %f\n",
+ (inner_left_vector.transpose() * top_left_vector)(0));
+
+ printf("right upper outer corner angle: %f, top (%f, %f), outer (%f, %f)\n",
+ (outer_right_vector.transpose() * top_right_vector)(0),
+ top_right_vector(0, 0), top_right_vector(1, 0),
+ outer_right_vector(0, 0), outer_right_vector(1, 0));
+ printf("right upper inner corner angle: %f\n",
+ (inner_right_vector.transpose() * top_right_vector)(0));
}
return IR;
}
diff --git a/y2019/vision/target_sender.cc b/y2019/vision/target_sender.cc
index 03c236e..12e0e09 100644
--- a/y2019/vision/target_sender.cc
+++ b/y2019/vision/target_sender.cc
@@ -91,13 +91,15 @@
params0.set_fps(15);
params0.set_height(480);
+ aos::vision::FastYuyvYPooledThresholder thresholder;
+
::std::unique_ptr<CameraStream> camera0(
new CameraStream(params0, "/dev/video0"));
camera0->set_on_frame([&](DataRef data,
monotonic_clock::time_point monotonic_now) {
aos::vision::ImageFormat fmt{640, 480};
- aos::vision::BlobList imgs = aos::vision::FindBlobs(
- aos::vision::FastYuyvYThreshold(fmt, data.data(), 120));
+ aos::vision::BlobList imgs =
+ aos::vision::FindBlobs(thresholder.Threshold(fmt, data.data(), 120));
finder_.PreFilter(&imgs);
LOG(INFO) << "Blobs: " << imgs.size();
diff --git a/y2019/vision/target_types.h b/y2019/vision/target_types.h
index 8ee1f4c..e97c051 100644
--- a/y2019/vision/target_types.h
+++ b/y2019/vision/target_types.h
@@ -105,6 +105,8 @@
ExtrinsicParams backup_extrinsics;
double backup_solver_error;
+
+ bool good_corners;
};
// Final foramtting ready for output on the wire.