Merge changes I7f3c1cc5,I9744c480,Id8ac2fae
* changes:
Make target selector use throttle instead of velocity
Tune localizer, remove error states
Add button to set line following mode
diff --git a/y2019/joystick_reader.cc b/y2019/joystick_reader.cc
index 11b8a77..b08d6b6 100644
--- a/y2019/joystick_reader.cc
+++ b/y2019/joystick_reader.cc
@@ -75,6 +75,12 @@
const ButtonLocation kRelease(2, 4);
const ButtonLocation kResetLocalizer(4, 3);
+const ButtonLocation kAutoPanel(3, 10);
+const ButtonLocation kAutoPanelIntermediate(4, 6);
+
+const ElevatorWristPosition kAutoPanelPos{0.0, -M_PI / 2.0};
+const ElevatorWristPosition kAutoPanelIntermediatePos{0.34, -M_PI / 2.0};
+
const ElevatorWristPosition kStowPos{0.36, 0.0};
const ElevatorWristPosition kPanelHPIntakeForwrdPos{0.04, M_PI / 2.0};
@@ -198,6 +204,12 @@
stilts_was_above_ = false;
}
+ if (data.IsPressed(kAutoPanel)) {
+ elevator_wrist_pos_ = kAutoPanelPos;
+ } else if (data.IsPressed(kAutoPanelIntermediate)) {
+ elevator_wrist_pos_ = kAutoPanelIntermediatePos;
+ }
+
if (switch_ball_) {
if (superstructure_queue.status->has_piece) {
new_superstructure_goal->wrist.profile_params.max_acceleration = 20;
diff --git a/y2019/vision/BUILD b/y2019/vision/BUILD
index 90c7dff..5cfc52b 100644
--- a/y2019/vision/BUILD
+++ b/y2019/vision/BUILD
@@ -74,7 +74,6 @@
gtk_dependent_cc_binary(
name = "debug_viewer",
srcs = ["debug_viewer.cc"],
- copts = ["-Wno-unused-variable"],
restricted_to = VISION_TARGETS,
deps = [
":target_finder",
@@ -83,6 +82,7 @@
"//aos/vision/blob:transpose",
"//aos/vision/debug:debug_framework",
"//aos/vision/math:vector",
+ "@com_github_gflags_gflags//:gflags",
],
)
diff --git a/y2019/vision/debug_viewer.cc b/y2019/vision/debug_viewer.cc
index 35f2af3..b282367 100644
--- a/y2019/vision/debug_viewer.cc
+++ b/y2019/vision/debug_viewer.cc
@@ -8,6 +8,7 @@
#include "aos/vision/blob/transpose.h"
#include "aos/vision/debug/debug_framework.h"
#include "aos/vision/math/vector.h"
+#include "gflags/gflags.h"
using aos::vision::ImageRange;
using aos::vision::ImageFormat;
@@ -18,6 +19,8 @@
using aos::vision::Segment;
using aos::vision::PixelRef;
+DEFINE_int32(camera, 10, "The camera to use the intrinsics for");
+
namespace y2019 {
namespace vision {
@@ -53,15 +56,18 @@
class FilterHarness : public aos::vision::FilterHarness {
public:
+ FilterHarness() {
+ *(target_finder_.mutable_intrinsics()) = GetCamera(FLAGS_camera)->intrinsics;
+ }
aos::vision::RangeImage Threshold(aos::vision::ImagePtr image) override {
- return finder_.Threshold(image);
+ return target_finder_.Threshold(image);
}
void InstallViewer(aos::vision::BlobStreamViewer *viewer) override {
viewer_ = viewer;
viewer_->SetScale(2.0);
overlays_.push_back(&overlay_);
- overlays_.push_back(finder_.GetOverlay());
+ overlays_.push_back(target_finder_.GetOverlay());
viewer_->view()->SetOverlays(&overlays_);
}
@@ -87,24 +93,25 @@
}
// Remove bad blobs.
- finder_.PreFilter(&imgs);
+ target_finder_.PreFilter(&imgs);
// Find polygons from blobs.
std::vector<std::vector<Segment<2>>> raw_polys;
for (const RangeImage &blob : imgs) {
// Convert blobs to contours in the corrected space.
- ContourNode* contour = finder_.GetContour(blob);
+ ContourNode *contour = target_finder_.GetContour(blob);
if (draw_contours_) {
DrawContour(contour, {255, 0, 0});
}
- finder_.UnWarpContour(contour);
+ const ::std::vector<::Eigen::Vector2f> unwarped_contour =
+ target_finder_.UnWarpContour(contour);
if (draw_contours_) {
- DrawContour(contour, {0, 0, 255});
+ DrawContour(unwarped_contour, {0, 0, 255});
}
// Process to polygons.
std::vector<Segment<2>> polygon =
- finder_.FillPolygon(contour, draw_raw_poly_);
+ target_finder_.FillPolygon(unwarped_contour, draw_raw_poly_);
if (polygon.empty()) {
if (!draw_contours_) {
DrawBlob(blob, {255, 0, 0});
@@ -117,12 +124,14 @@
if (draw_raw_poly_) {
std::vector<PixelRef> colors = GetNColors(polygon.size());
std::vector<Vector<2>> corners;
- for (size_t i = 0; i < 4; ++i) {
- corners.push_back(polygon[i].Intersect(polygon[(i + 1) % 4]));
+ for (size_t i = 0; i < polygon.size(); ++i) {
+ corners.push_back(
+ polygon[i].Intersect(polygon[(i + 1) % polygon.size()]));
}
- for (size_t i = 0; i < 4; ++i) {
- overlay_.AddLine(corners[i], corners[(i + 1) % 4], colors[i]);
+ for (size_t i = 0; i < polygon.size(); ++i) {
+ overlay_.AddLine(corners[i], corners[(i + 1) % polygon.size()],
+ colors[i]);
}
}
}
@@ -130,7 +139,7 @@
// Calculate each component side of a possible target.
std::vector<TargetComponent> target_component_list =
- finder_.FillTargetComponentList(raw_polys);
+ target_finder_.FillTargetComponentList(raw_polys);
if (draw_components_) {
for (const TargetComponent &comp : target_component_list) {
DrawComponent(comp, {0, 255, 255}, {0, 255, 255}, {255, 0, 0},
@@ -139,7 +148,7 @@
}
// Put the compenents together into targets.
- std::vector<Target> target_list = finder_.FindTargetsFromComponents(
+ std::vector<Target> target_list = target_finder_.FindTargetsFromComponents(
target_component_list, draw_raw_target_);
if (draw_raw_target_) {
for (const Target &target : target_list) {
@@ -150,12 +159,13 @@
// Use the solver to generate an intermediate version of our results.
std::vector<IntermediateResult> results;
for (const Target &target : target_list) {
- results.emplace_back(finder_.ProcessTargetToResult(target, draw_raw_IR_));
+ results.emplace_back(
+ target_finder_.ProcessTargetToResult(target, draw_raw_IR_));
if (draw_raw_IR_) DrawResult(results.back(), {255, 128, 0});
}
// Check that our current results match possible solutions.
- results = finder_.FilterResults(results, 0);
+ results = target_finder_.FilterResults(results, 0);
if (draw_results_) {
for (const IntermediateResult &res : results) {
DrawTarget(res, {0, 255, 0});
@@ -215,6 +225,18 @@
}
}
+ void DrawContour(const ::std::vector<::Eigen::Vector2f> &contour,
+ PixelRef color) {
+ if (viewer_) {
+ for (size_t i = 0; i < contour.size(); ++i) {
+ Vector<2> a(contour[i].x(), contour[i].y());
+ Vector<2> b(contour[(i + 1) % contour.size()].x(),
+ contour[(i + 1) % contour.size()].y());
+ overlay_.AddLine(a, b, color);
+ }
+ }
+ }
+
void DrawComponent(const TargetComponent &comp, PixelRef top_color,
PixelRef bot_color, PixelRef in_color,
PixelRef out_color) {
@@ -239,15 +261,15 @@
}
void DrawResult(const IntermediateResult &result, PixelRef color) {
- Target target =
- Project(finder_.GetTemplateTarget(), intrinsics(), result.extrinsics);
+ Target target = Project(target_finder_.GetTemplateTarget(), intrinsics(),
+ result.extrinsics);
DrawComponent(target.left, color, color, color, color);
DrawComponent(target.right, color, color, color, color);
}
void DrawTarget(const IntermediateResult &result, PixelRef color) {
- Target target =
- Project(finder_.GetTemplateTarget(), intrinsics(), result.extrinsics);
+ Target target = Project(target_finder_.GetTemplateTarget(), intrinsics(),
+ result.extrinsics);
Segment<2> leftAx((target.left.top + target.left.inside) * 0.5,
(target.left.bottom + target.left.outside) * 0.5);
leftAx.Set(leftAx.A() * 0.9 + leftAx.B() * 0.1,
@@ -278,11 +300,13 @@
overlay_.AddLine(p3 + leftAx.B(), p3 + rightAx.B(), {0, 255, 0});
}
- const IntrinsicParams &intrinsics() const { return finder_.intrinsics(); }
+ const IntrinsicParams &intrinsics() const {
+ return target_finder_.intrinsics();
+ }
private:
// implementation of the filter pipeline.
- TargetFinder finder_;
+ TargetFinder target_finder_;
aos::vision::BlobStreamViewer *viewer_ = nullptr;
aos::vision::PixelLinesOverlay overlay_;
std::vector<aos::vision::OverlayBase *> overlays_;
@@ -302,6 +326,7 @@
int main(int argc, char **argv) {
y2019::vision::FilterHarness filter_harness;
+ ::gflags::ParseCommandLineFlags(&argc, &argv, true);
aos::vision::DebugFrameworkMain(argc, argv, &filter_harness,
aos::vision::CameraParams());
}
diff --git a/y2019/vision/global_calibration.cc b/y2019/vision/global_calibration.cc
index d21e184..c9646fc 100644
--- a/y2019/vision/global_calibration.cc
+++ b/y2019/vision/global_calibration.cc
@@ -162,9 +162,10 @@
for (const RangeImage &blob : imgs) {
// Convert blobs to contours in the corrected space.
ContourNode *contour = target_finder.GetContour(blob);
- target_finder.UnWarpContour(contour);
+ const ::std::vector<::Eigen::Vector2f> unwarped_contour =
+ target_finder.UnWarpContour(contour);
const ::std::vector<Segment<2>> polygon =
- target_finder.FillPolygon(contour, verbose);
+ target_finder.FillPolygon(unwarped_contour, verbose);
if (!polygon.empty()) {
raw_polys.push_back(polygon);
}
diff --git a/y2019/vision/target_finder.cc b/y2019/vision/target_finder.cc
index 45678eb..1d991ff 100644
--- a/y2019/vision/target_finder.cc
+++ b/y2019/vision/target_finder.cc
@@ -59,7 +59,7 @@
}
-Point UnWarpPoint(const Point &point, int iterations) {
+::Eigen::Vector2f UnWarpPoint(const Point point) {
const double x0 = ((double)point.x - c_x) / f_x;
const double y0 = ((double)point.y - c_y) / f_y;
double x = x0;
@@ -71,94 +71,86 @@
x = x0 / coeff;
y = y0 / coeff;
}
- double nx = x * f_x_prime + c_x_prime;
- double ny = y * f_y_prime + c_y_prime;
- Point p = {static_cast<int>(nx), static_cast<int>(ny)};
- return p;
+ const double nx = x * f_x_prime + c_x_prime;
+ const double ny = y * f_y_prime + c_y_prime;
+ return ::Eigen::Vector2f(nx, ny);
}
-void TargetFinder::UnWarpContour(ContourNode *start) const {
+::std::vector<::Eigen::Vector2f> TargetFinder::UnWarpContour(
+ ContourNode *start) const {
+ ::std::vector<::Eigen::Vector2f> result;
ContourNode *node = start;
while (node->next != start) {
- node->set_point(UnWarpPoint(node->pt, iterations));
+ result.push_back(UnWarpPoint(node->pt));
node = node->next;
}
- node->set_point(UnWarpPoint(node->pt, iterations));
+ result.push_back(UnWarpPoint(node->pt));
+ return result;
}
// TODO: Try hierarchical merge for this.
// Convert blobs into polygons.
std::vector<aos::vision::Segment<2>> TargetFinder::FillPolygon(
- ContourNode* start, bool verbose) {
+ const ::std::vector<::Eigen::Vector2f> &contour, bool verbose) {
if (verbose) printf("Process Polygon.\n");
- struct Pt {
- float x;
- float y;
- };
- std::vector<Pt> points;
+ ::std::vector<::Eigen::Vector2f> slopes;
// Collect all slopes from the contour.
- Point previous_point = start->pt;
- for (ContourNode *node = start; node->next != start;) {
- node = node->next;
+ ::Eigen::Vector2f previous_point = contour[0];
+ for (size_t i = 0; i < contour.size(); ++i) {
+ ::Eigen::Vector2f next_point = contour[(i + 1) % contour.size()];
- Point current_point = node->pt;
+ slopes.push_back(next_point - previous_point);
- points.push_back({static_cast<float>(current_point.x - previous_point.x),
- static_cast<float>(current_point.y - previous_point.y)});
-
- previous_point = current_point;
+ previous_point = next_point;
}
- const int num_points = points.size();
- auto get_pt = [&points, num_points](int i) {
- return points[(i + num_points * 2) % num_points];
+ const int num_points = slopes.size();
+ auto get_pt = [&slopes, num_points](int i) {
+ return slopes[(i + num_points * 2) % num_points];
};
- std::vector<Pt> filtered_points = points;
+ ::std::vector<::Eigen::Vector2f> filtered_slopes = slopes;
// Three box filter makith a guassian?
// Run gaussian filter over the slopes 3 times. That'll get us pretty close
// to running a gausian over it.
for (int k = 0; k < 3; ++k) {
const int window_size = 2;
- for (size_t i = 0; i < points.size(); ++i) {
- Pt a{0.0, 0.0};
+ for (size_t i = 0; i < slopes.size(); ++i) {
+ ::Eigen::Vector2f a = ::Eigen::Vector2f::Zero();
for (int j = -window_size; j <= window_size; ++j) {
- Pt p = get_pt(j + i);
- a.x += p.x;
- a.y += p.y;
+ ::Eigen::Vector2f p = get_pt(j + i);
+ a += p;
}
- a.x /= (window_size * 2 + 1);
- a.y /= (window_size * 2 + 1);
+ a /= (window_size * 2 + 1);
- const float scale = 1.0 + (i / float(points.size() * 10));
- a.x *= scale;
- a.y *= scale;
- filtered_points[i] = a;
+ const float scale = 1.0 + (i / float(slopes.size() * 10));
+ a *= scale;
+ filtered_slopes[i] = a;
}
- points = filtered_points;
+ slopes = filtered_slopes;
}
// Heuristic which says if a particular slope is part of a corner.
auto is_corner = [&](size_t i) {
- const Pt a = get_pt(i - 3);
- const Pt b = get_pt(i + 3);
- const double dx = (a.x - b.x);
- const double dy = (a.y - b.y);
+ const ::Eigen::Vector2f a = get_pt(i - 3);
+ const ::Eigen::Vector2f b = get_pt(i + 3);
+ const double dx = (a.x() - b.x());
+ const double dy = (a.y() - b.y());
return dx * dx + dy * dy > 0.25;
};
bool prev_v = is_corner(-1);
// Find all centers of corners.
- // Because they round, multiple points may be a corner.
- std::vector<size_t> edges;
- size_t kBad = points.size() + 10;
+ // Because they round, multiple slopes may be a corner.
+ ::std::vector<size_t> edges;
+ const size_t kBad = slopes.size() + 10;
size_t prev_up = kBad;
size_t wrapped_n = prev_up;
- for (size_t i = 0; i < points.size(); ++i) {
+ for (size_t i = 0; i < slopes.size(); ++i) {
bool v = is_corner(i);
if (prev_v && !v) {
if (prev_up == kBad) {
@@ -174,40 +166,26 @@
}
if (wrapped_n != kBad) {
- edges.push_back(((prev_up + points.size() + wrapped_n - 1) / 2) % points.size());
+ edges.push_back(((prev_up + slopes.size() + wrapped_n - 1) / 2) % slopes.size());
}
if (verbose) printf("Edge Count (%zu).\n", edges.size());
- // Get all CountourNodes from the contour.
- using aos::vision::PixelRef;
- std::vector<ContourNode *> segments;
- {
- std::vector<ContourNode *> segments_all;
-
- for (ContourNode *node = start; node->next != start;) {
- node = node->next;
- segments_all.push_back(node);
- }
- for (size_t i : edges) {
- segments.push_back(segments_all[i]);
- }
- }
- if (verbose) printf("Segment Count (%zu).\n", segments.size());
-
// Run best-fits over each line segment.
- std::vector<Segment<2>> seg_list;
- if (segments.size() == 4) {
- for (size_t i = 0; i < segments.size(); ++i) {
- ContourNode *segment_end = segments[(i + 1) % segments.size()];
- ContourNode *segment_start = segments[i];
+ ::std::vector<Segment<2>> seg_list;
+ if (edges.size() == 4) {
+ for (size_t i = 0; i < edges.size(); ++i) {
+ // Include the corners in both line fits.
+ const size_t segment_start_index = edges[i];
+ const size_t segment_end_index =
+ (edges[(i + 1) % edges.size()] + 1) % contour.size();
float mx = 0.0;
float my = 0.0;
int n = 0;
- for (ContourNode *node = segment_start; node != segment_end;
- node = node->next) {
- mx += node->pt.x;
- my += node->pt.y;
+ for (size_t j = segment_start_index; j != segment_end_index;
+ (j = (j + 1) % contour.size())) {
+ mx += contour[j].x();
+ my += contour[j].y();
++n;
// (x - [x] / N) ** 2 = [x * x] - 2 * [x] * [x] / N + [x] * [x] / N / N;
}
@@ -217,10 +195,10 @@
float xx = 0.0;
float xy = 0.0;
float yy = 0.0;
- for (ContourNode *node = segment_start; node != segment_end;
- node = node->next) {
- const float x = node->pt.x - mx;
- const float y = node->pt.y - my;
+ for (size_t j = segment_start_index; j != segment_end_index;
+ (j = (j + 1) % contour.size())) {
+ const float x = contour[j].x() - mx;
+ const float y = contour[j].y() - my;
xx += x * x;
xy += x * y;
yy += y * y;
diff --git a/y2019/vision/target_finder.h b/y2019/vision/target_finder.h
index 1ba79ba..fcde358 100644
--- a/y2019/vision/target_finder.h
+++ b/y2019/vision/target_finder.h
@@ -30,12 +30,12 @@
// filter out obvious or durranged blobs.
void PreFilter(BlobList *imgs);
- ContourNode* GetContour(const RangeImage &blob);
- void UnWarpContour(ContourNode* start) const;
+ ContourNode *GetContour(const RangeImage &blob);
+ ::std::vector<::Eigen::Vector2f> UnWarpContour(ContourNode *start) const;
// Turn a blob into a polgygon.
- std::vector<aos::vision::Segment<2>> FillPolygon(ContourNode *start,
- bool verbose);
+ std::vector<aos::vision::Segment<2>> FillPolygon(
+ const ::std::vector<::Eigen::Vector2f> &contour, bool verbose);
// Turn a bloblist into components of a target.
std::vector<TargetComponent> FillTargetComponentList(
diff --git a/y2019/vision/target_sender.cc b/y2019/vision/target_sender.cc
index f322aeb..80a47e0 100644
--- a/y2019/vision/target_sender.cc
+++ b/y2019/vision/target_sender.cc
@@ -312,8 +312,10 @@
for (const RangeImage &blob : imgs) {
// Convert blobs to contours in the corrected space.
ContourNode* contour = finder_.GetContour(blob);
- finder_.UnWarpContour(contour);
- std::vector<Segment<2>> polygon = finder_.FillPolygon(contour, verbose);
+ const ::std::vector<::Eigen::Vector2f> unwarped_contour =
+ finder_.UnWarpContour(contour);
+ ::std::vector<Segment<2>> polygon =
+ finder_.FillPolygon(unwarped_contour, verbose);
if (!polygon.empty()) {
raw_polys.push_back(polygon);
}