Require target side and top edges to be perpendicular
Weird target from the sun wasn't rectangular. We can reject things that
aren't rectangular ish now, and that fixes it.
Change-Id: Iac1218c0fc1286758ec7602c29ac5c38c24ab831
diff --git a/y2019/vision/debug_viewer.cc b/y2019/vision/debug_viewer.cc
index 3c174b4..9f1d4dd 100644
--- a/y2019/vision/debug_viewer.cc
+++ b/y2019/vision/debug_viewer.cc
@@ -320,11 +320,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/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_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.