Reset target estimate when unable to solve
Prevents us from getting stuck
Signed-off-by: Milind Upadhyay <milind.upadhyay@gmail.com>
Change-Id: I45fbd9cb3478033c74abda84b4e2294a37d01850
diff --git a/y2022/vision/target_estimator.cc b/y2022/vision/target_estimator.cc
index 2526355..26d9700 100644
--- a/y2022/vision/target_estimator.cc
+++ b/y2022/vision/target_estimator.cc
@@ -98,6 +98,12 @@
const std::array<cv::Point3d, 4> TargetEstimator::kMiddleTapePiecePoints =
ComputeMiddleTapePiecePoints();
+namespace {
+constexpr double kDefaultDistance = 3.0;
+constexpr double kDefaultYaw = M_PI;
+constexpr double kDefaultAngleToCamera = 0.0;
+} // namespace
+
TargetEstimator::TargetEstimator(cv::Mat intrinsics, cv::Mat extrinsics)
: blob_stats_(),
middle_blob_index_(0),
@@ -105,9 +111,9 @@
image_(std::nullopt),
roll_(0.0),
pitch_(0.0),
- yaw_(M_PI),
- distance_(3.0),
- angle_to_camera_(0.0),
+ yaw_(kDefaultYaw),
+ distance_(kDefaultDistance),
+ angle_to_camera_(kDefaultAngleToCamera),
// Seed camera height
camera_height_(extrinsics.at<double>(2, 3) +
constants::Values::kImuHeight()) {
@@ -209,6 +215,19 @@
// TODO(milind): seed with localizer output as well
+ // If we didn't solve well last time, seed everything at the defaults so we
+ // don't get stuck in a bad state.
+ // Copied from localizer.cc
+ constexpr double kMinConfidence = 0.75;
+ if (confidence_ < kMinConfidence) {
+ roll_ = roll_seed;
+ pitch_ = pitch_seed;
+ yaw_ = kDefaultYaw;
+ distance_ = kDefaultDistance;
+ angle_to_camera_ = kDefaultAngleToCamera;
+ camera_height_ = extrinsics_(2, 3) + constants::Values::kImuHeight();
+ }
+
// Constrain the rotation to be around the localizer's, otherwise there can be
// multiple solutions. There shouldn't be too much roll or pitch
if (FLAGS_freeze_roll) {
diff --git a/y2022/vision/viewer.cc b/y2022/vision/viewer.cc
index a21c09f..446f1f6 100644
--- a/y2022/vision/viewer.cc
+++ b/y2022/vision/viewer.cc
@@ -274,15 +274,14 @@
blob_result.filtered_blobs.size()
<< ")";
+ estimator.Solve(blob_result.filtered_stats,
+ FLAGS_display_estimation ? std::make_optional(ret_image)
+ : std::nullopt);
if (blob_result.filtered_blobs.size() > 0) {
- estimator.Solve(blob_result.filtered_stats,
- FLAGS_display_estimation ? std::make_optional(ret_image)
- : std::nullopt);
estimator.DrawEstimate(ret_image);
LOG(INFO) << "Read file " << (it - file_list.begin()) << ": " << *it;
}
-
cv::imshow("image", image_mat);
cv::imshow("mask", blob_result.binarized_image);
cv::imshow("blobs", ret_image);