Force the 4 point solution to contain the lowest point
Otherwise it may be optimal to have shorter targets. This results in
significant skew which is bad.
Change-Id: I813d7977966afacec657cd6d2b86b225dfe6e169
diff --git a/y2019/vision/debug_viewer.cc b/y2019/vision/debug_viewer.cc
index e217d53..a919bce 100644
--- a/y2019/vision/debug_viewer.cc
+++ b/y2019/vision/debug_viewer.cc
@@ -163,7 +163,12 @@
for (const Target &target : target_list) {
results.emplace_back(
target_finder_.ProcessTargetToResult(target, draw_raw_IR_));
- if (draw_raw_IR_) DrawResult(results.back(), {255, 128, 0});
+ if (draw_raw_IR_) {
+ IntermediateResult updatable_result = results.back();
+ target_finder_.MaybePickAndUpdateResult(&updatable_result,
+ draw_raw_IR_);
+ DrawResult(updatable_result, {255, 128, 0});
+ }
}
// Check that our current results match possible solutions.
diff --git a/y2019/vision/target_finder.cc b/y2019/vision/target_finder.cc
index b2b5a90..2cd5130 100644
--- a/y2019/vision/target_finder.cc
+++ b/y2019/vision/target_finder.cc
@@ -508,32 +508,42 @@
return target_list;
}
+bool TargetFinder::MaybePickAndUpdateResult(IntermediateResult *result,
+ bool verbose) {
+ // Based on a linear regression between error and distance to target.
+ // Closer targets can have a higher error because they are bigger.
+ const double acceptable_error =
+ std::max(2 * (21 - 12 * result->extrinsics.z), 50.0);
+ if (result->solver_error < acceptable_error) {
+ if (verbose) {
+ printf("Using an 8 point solve: %f < %f \n", result->solver_error,
+ acceptable_error);
+ }
+ return true;
+ } else if (result->backup_solver_error < acceptable_error) {
+ if (verbose) {
+ printf("Using a 4 point solve: %f < %f \n", result->backup_solver_error,
+ acceptable_error);
+ }
+ IntermediateResult backup;
+ result->extrinsics = result->backup_extrinsics;
+ result->solver_error = result->backup_solver_error;
+ return true;
+ } else if (verbose) {
+ printf("Rejecting a target with errors: (%f, %f) > %f \n",
+ result->solver_error, result->backup_solver_error, acceptable_error);
+ }
+ return false;
+}
+
std::vector<IntermediateResult> TargetFinder::FilterResults(
const std::vector<IntermediateResult> &results, uint64_t print_rate,
bool verbose) {
std::vector<IntermediateResult> filtered;
for (const IntermediateResult &res : results) {
- // 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);
+ IntermediateResult updatable_result = res;
+ if (MaybePickAndUpdateResult(&updatable_result, verbose)) {
+ filtered.emplace_back(updatable_result);
}
}
frame_count_++;
diff --git a/y2019/vision/target_finder.h b/y2019/vision/target_finder.h
index 0de5ad1..c7de67a 100644
--- a/y2019/vision/target_finder.h
+++ b/y2019/vision/target_finder.h
@@ -52,6 +52,10 @@
// Given a target solve for the transformation of the template target.
IntermediateResult ProcessTargetToResult(const Target &target, bool verbose);
+ // Returns true if a target is good, and false otherwise. Picks the 4 vs 8
+ // point solution depending on which one looks right.
+ bool MaybePickAndUpdateResult(IntermediateResult *result, bool verbose);
+
std::vector<IntermediateResult> FilterResults(
const std::vector<IntermediateResult> &results, uint64_t print_rate,
bool verbose);
diff --git a/y2019/vision/target_geometry.cc b/y2019/vision/target_geometry.cc
index 24ac087..8239dd6 100644
--- a/y2019/vision/target_geometry.cc
+++ b/y2019/vision/target_geometry.cc
@@ -46,6 +46,8 @@
}
std::array<Vector<2>, 8> Target::ToPointList() const {
+ // Note, the even points are fit with the line solver in the 4 point solution
+ // while the odds are fit with the point matcher.
return std::array<Vector<2>, 8>{{right.top, right.outside, right.inside,
right.bottom, left.top, left.outside,
left.inside, left.bottom}};
@@ -168,8 +170,8 @@
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);
+ const Vector<2> p1 = Project(template_seg.A(), intrinsics, extrinsics);
+ const Vector<2> 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();
@@ -185,6 +187,45 @@
IntrinsicParams intrinsics;
};
+// Find the distance that the bottom point is outside the target and penalize
+// that linearly.
+class BottomPointCostFunctor {
+ public:
+ BottomPointCostFunctor(::Eigen::Vector2f bottom_point,
+ Segment<2> template_seg, IntrinsicParams intrinsics)
+ : bottom_point_(bottom_point.x(), bottom_point.y()),
+ template_seg_(template_seg),
+ intrinsics_(intrinsics) {}
+
+ bool operator()(const double *const x, double *residual) const {
+ const ExtrinsicParams extrinsics = ExtrinsicParams::get(x);
+ const Vector<2> p1 = Project(template_seg_.A(), intrinsics_, extrinsics);
+ const Vector<2> p2 = Project(template_seg_.B(), intrinsics_, extrinsics);
+
+ // Construct a vector pointed perpendicular to the line. This vector is
+ // pointed down out the bottom of the target.
+ ::Eigen::Vector2d down_axis(-(p1.y() - p2.y()), p1.x() - p2.x());
+ down_axis.normalize();
+
+ // Positive means out.
+ const double component =
+ down_axis.transpose() * (bottom_point_ - p1.GetData().transpose());
+
+ if (component > 0) {
+ residual[0] = component * 1.0;
+ } else {
+ residual[0] = 0.0;
+ }
+ return true;
+ }
+
+ private:
+ ::Eigen::Vector2d bottom_point_;
+ Segment<2> template_seg_;
+
+ IntrinsicParams intrinsics_;
+};
+
IntermediateResult TargetFinder::ProcessTargetToResult(const Target &target,
bool verbose) {
// Memory for the ceres solver.
@@ -211,20 +252,38 @@
problem_4point.AddResidualBlock(
new NumericDiffCostFunction<LineCostFunctor, CENTRAL, 1, 4>(
new LineCostFunctor(b, line, intrinsics_)),
- NULL, ¶ms_8point[0]);
+ NULL, ¶ms_4point[0]);
} else {
problem_4point.AddResidualBlock(
new NumericDiffCostFunction<PointCostFunctor, CENTRAL, 2, 4>(
new PointCostFunctor(b, a, intrinsics_)),
- NULL, ¶ms_8point[0]);
+ NULL, ¶ms_4point[0]);
}
problem_8point.AddResidualBlock(
new NumericDiffCostFunction<PointCostFunctor, CENTRAL, 2, 4>(
new PointCostFunctor(b, a, intrinsics_)),
- NULL, ¶ms_4point[0]);
+ NULL, ¶ms_8point[0]);
}
+ // Now, add a large cost for the bottom point being below the bottom line.
+ problem_4point.AddResidualBlock(
+ new NumericDiffCostFunction<BottomPointCostFunctor, CENTRAL, 1, 4>(
+ new BottomPointCostFunctor(target.left.bottom_point,
+ Segment<2>(target_template_.left.outside,
+ target_template_.left.bottom),
+ intrinsics_)),
+ NULL, ¶ms_4point[0]);
+ // Make sure to point the segment the other way so when we do a -pi/2 rotation
+ // on the line, it points down in target space.
+ problem_4point.AddResidualBlock(
+ new NumericDiffCostFunction<BottomPointCostFunctor, CENTRAL, 1, 4>(
+ new BottomPointCostFunctor(target.right.bottom_point,
+ Segment<2>(target_template_.right.bottom,
+ target_template_.right.outside),
+ intrinsics_)),
+ NULL, ¶ms_4point[0]);
+
Solver::Options options;
options.minimizer_progress_to_stdout = false;
Solver::Summary summary_8point;