Make the vision solver use a less strict problem.
Solve a 4 point with distance to lines and use if the 8 point problem
isn't solvable.
Change-Id: Ie0e4f5983b35cbe2fc64af8230c7e1e1cbd53aaa
diff --git a/y2019/vision/debug_viewer.cc b/y2019/vision/debug_viewer.cc
index 14063c2..8ef67fa 100644
--- a/y2019/vision/debug_viewer.cc
+++ b/y2019/vision/debug_viewer.cc
@@ -166,7 +166,7 @@
}
// Check that our current results match possible solutions.
- results = target_finder_.FilterResults(results, 0);
+ results = target_finder_.FilterResults(results, 0, draw_results_);
if (draw_results_) {
for (const IntermediateResult &res : results) {
DrawTarget(res, {0, 255, 0});
diff --git a/y2019/vision/target_finder.cc b/y2019/vision/target_finder.cc
index e340702..0f25765 100644
--- a/y2019/vision/target_finder.cc
+++ b/y2019/vision/target_finder.cc
@@ -452,16 +452,6 @@
Target new_target;
const TargetComponent &b = component_list[j];
- // Reject targets that are too far off vertically.
- Vector<2> a_center = a.major_axis.Center();
- if (a_center.y() > b.bottom.y() || a_center.y() < b.top.y()) {
- continue;
- }
- Vector<2> b_center = b.major_axis.Center();
- if (b_center.y() > a.bottom.y() || b_center.y() < a.top.y()) {
- continue;
- }
-
if (a.is_right && !b.is_right) {
if (a.top.x() > b.top.x()) {
new_target.right = a;
@@ -474,6 +464,9 @@
new_target.left = a;
target_valid = true;
}
+ } else if (verbose) {
+ printf("Found same side components: %s.\n",
+ a.is_right ? "right" : "left");
}
if (target_valid) {
target_list.emplace_back(new_target);
@@ -485,11 +478,31 @@
}
std::vector<IntermediateResult> TargetFinder::FilterResults(
- const std::vector<IntermediateResult> &results, uint64_t print_rate) {
+ const std::vector<IntermediateResult> &results, uint64_t print_rate,
+ bool verbose) {
std::vector<IntermediateResult> filtered;
for (const IntermediateResult &res : results) {
- if (res.solver_error < 75.0) {
+ // Based on a linear regression between error and distance to target.
+ // Closer targets can have a higher error because they are bigger.
+ double acceptable_error = std::max(2 * (21 - 12 * res.extrinsics.z), 50.0);
+ if (res.solver_error < acceptable_error) {
+ if (verbose) {
+ printf("Using an 8 point solve: %f < %f \n", res.solver_error,
+ acceptable_error);
+ }
filtered.emplace_back(res);
+ } else if (res.backup_solver_error < acceptable_error) {
+ if (verbose) {
+ printf("Using a 4 point solve: %f < %f \n", res.backup_solver_error,
+ acceptable_error);
+ }
+ IntermediateResult backup;
+ backup.extrinsics = res.backup_extrinsics;
+ backup.solver_error= res.backup_solver_error;
+ filtered.emplace_back(backup);
+ } else if (verbose) {
+ printf("Rejecting a target with errors: (%f, %f) > %f \n",
+ res.solver_error, res.backup_solver_error, acceptable_error);
}
}
frame_count_++;
diff --git a/y2019/vision/target_finder.h b/y2019/vision/target_finder.h
index ebae3b4..0de5ad1 100644
--- a/y2019/vision/target_finder.h
+++ b/y2019/vision/target_finder.h
@@ -53,7 +53,8 @@
IntermediateResult ProcessTargetToResult(const Target &target, bool verbose);
std::vector<IntermediateResult> FilterResults(
- const std::vector<IntermediateResult> &results, uint64_t print_rate);
+ const std::vector<IntermediateResult> &results, uint64_t print_rate,
+ bool verbose);
// Get the local overlay for debug if we are doing that.
aos::vision::PixelLinesOverlay *GetOverlay() { return &overlay_; }
diff --git a/y2019/vision/target_geometry.cc b/y2019/vision/target_geometry.cc
index d4b8a54..24ac087 100644
--- a/y2019/vision/target_geometry.cc
+++ b/y2019/vision/target_geometry.cc
@@ -46,9 +46,9 @@
}
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}};
+ return std::array<Vector<2>, 8>{{right.top, right.outside, right.inside,
+ right.bottom, left.top, left.outside,
+ left.inside, left.bottom}};
}
Vector<2> Project(Vector<2> pt, const IntrinsicParams &intrinsics,
@@ -141,8 +141,8 @@
}
// Used at runtime on a single image given camera parameters.
-struct RuntimeCostFunctor {
- RuntimeCostFunctor(Vector<2> result, Vector<2> template_pt,
+struct PointCostFunctor {
+ PointCostFunctor(Vector<2> result, Vector<2> template_pt,
IntrinsicParams intrinsics)
: result(result), template_pt(template_pt), intrinsics(intrinsics) {}
@@ -159,13 +159,42 @@
IntrinsicParams intrinsics;
};
+// Find the distance from a lower target point to the 'vertical' line it should
+// be on.
+struct LineCostFunctor {
+ LineCostFunctor(Vector<2> result, Segment<2> template_seg,
+ IntrinsicParams intrinsics)
+ : result(result), template_seg(template_seg), intrinsics(intrinsics) {}
+
+ bool operator()(const double *const x, double *residual) const {
+ const auto extrinsics = ExtrinsicParams::get(x);
+ const auto p1 = Project(template_seg.A(), intrinsics, extrinsics);
+ const auto p2 = Project(template_seg.B(), intrinsics, extrinsics);
+ // distance from line (P1, P2) to point result
+ double dx = p2.x() - p1.x();
+ double dy = p2.y() - p1.y();
+ double denom = p2.DistanceTo(p1);
+ residual[0] = ::std::abs(dy * result.x() - dx * result.y() +
+ p2.x() * p1.y() - p2.y() * p1.x()) /
+ denom;
+ return true;
+ }
+
+ Vector<2> result;
+ Segment<2> template_seg;
+ IntrinsicParams intrinsics;
+};
+
IntermediateResult TargetFinder::ProcessTargetToResult(const Target &target,
bool verbose) {
// Memory for the ceres solver.
- double params[ExtrinsicParams::kNumParams];
- default_extrinsics_.set(¶ms[0]);
+ double params_8point[ExtrinsicParams::kNumParams];
+ default_extrinsics_.set(¶ms_8point[0]);
+ double params_4point[ExtrinsicParams::kNumParams];
+ default_extrinsics_.set(¶ms_4point[0]);
- Problem problem;
+ Problem problem_8point;
+ Problem problem_4point;
::std::array<aos::vision::Vector<2>, 8> target_value = target.ToPointList();
::std::array<aos::vision::Vector<2>, 8> template_value =
@@ -175,30 +204,57 @@
aos::vision::Vector<2> a = template_value[i];
aos::vision::Vector<2> b = target_value[i];
- problem.AddResidualBlock(
- new NumericDiffCostFunction<RuntimeCostFunctor, CENTRAL, 2, 4>(
- new RuntimeCostFunctor(b, a, intrinsics_)),
- NULL, ¶ms[0]);
+ if (i % 2 == 1) {
+ aos::vision::Vector<2> a2 = template_value[i-1];
+ aos::vision::Segment<2> line = Segment<2>(a, a2);
+
+ problem_4point.AddResidualBlock(
+ new NumericDiffCostFunction<LineCostFunctor, CENTRAL, 1, 4>(
+ new LineCostFunctor(b, line, intrinsics_)),
+ NULL, ¶ms_8point[0]);
+ } else {
+ problem_4point.AddResidualBlock(
+ new NumericDiffCostFunction<PointCostFunctor, CENTRAL, 2, 4>(
+ new PointCostFunctor(b, a, intrinsics_)),
+ NULL, ¶ms_8point[0]);
+ }
+
+ problem_8point.AddResidualBlock(
+ new NumericDiffCostFunction<PointCostFunctor, CENTRAL, 2, 4>(
+ new PointCostFunctor(b, a, intrinsics_)),
+ NULL, ¶ms_4point[0]);
}
Solver::Options options;
options.minimizer_progress_to_stdout = false;
- Solver::Summary summary;
- Solve(options, &problem, &summary);
+ Solver::Summary summary_8point;
+ Solve(options, &problem_8point, &summary_8point);
+ Solver::Summary summary_4point;
+ Solve(options, &problem_4point, &summary_4point);
IntermediateResult IR;
- IR.extrinsics = ExtrinsicParams::get(¶ms[0]);
- IR.solver_error = summary.final_cost;
+ IR.extrinsics = ExtrinsicParams::get(¶ms_8point[0]);
+ IR.solver_error = summary_8point.final_cost;
+ IR.backup_extrinsics = ExtrinsicParams::get(¶ms_4point[0]);
+ IR.backup_solver_error = summary_4point.final_cost;
if (verbose) {
- std::cout << summary.BriefReport() << "\n";
+ std::cout << "rup = " << intrinsics_.mount_angle * 180 / M_PI << ";\n";
+ std::cout << "fl = " << intrinsics_.focal_length << ";\n";
+ std::cout << "8 points:\n";
+ std::cout << summary_8point.BriefReport() << "\n";
+ std::cout << "error = " << summary_8point.final_cost << ";\n";
std::cout << "y = " << IR.extrinsics.y / kInchesToMeters << ";\n";
std::cout << "z = " << IR.extrinsics.z / kInchesToMeters << ";\n";
std::cout << "r1 = " << IR.extrinsics.r1 * 180 / M_PI << ";\n";
std::cout << "r2 = " << IR.extrinsics.r2 * 180 / M_PI << ";\n";
- std::cout << "rup = " << intrinsics_.mount_angle * 180 / M_PI << ";\n";
- std::cout << "fl = " << intrinsics_.focal_length << ";\n";
- std::cout << "error = " << summary.final_cost << ";\n";
+ std::cout << "4 points:\n";
+ std::cout << summary_4point.BriefReport() << "\n";
+ std::cout << "error = " << summary_4point.final_cost << ";\n\n";
+ std::cout << "y = " << IR.backup_extrinsics.y / kInchesToMeters << ";\n";
+ 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";
}
return IR;
}
diff --git a/y2019/vision/target_sender.cc b/y2019/vision/target_sender.cc
index 6dcf6c0..472baf3 100644
--- a/y2019/vision/target_sender.cc
+++ b/y2019/vision/target_sender.cc
@@ -339,7 +339,7 @@
}
LOG(INFO, "Raw Results: (%zu).\n", results.size());
- results = finder_.FilterResults(results, 30);
+ results = finder_.FilterResults(results, 30, verbose);
LOG(INFO, "Results: (%zu).\n", results.size());
// TODO: Select top 3 (randomly?)
diff --git a/y2019/vision/target_types.h b/y2019/vision/target_types.h
index 3174b90..8467da8 100644
--- a/y2019/vision/target_types.h
+++ b/y2019/vision/target_types.h
@@ -92,6 +92,11 @@
// Error from solver calulations.
double solver_error;
+
+ // extrinsics and error from a more relaxed problem.
+ ExtrinsicParams backup_extrinsics;
+
+ double backup_solver_error;
};
// Final foramtting ready for output on the wire.