commit 14279deaad08ccf00d7a45013565b2a36aa7f93d
author Milind Upadhyay <milind.upadhyay@gmail.com> Sat Feb 26 16:07:53 2022 -0800
committer Milind Upadhyay <milind.upadhyay@gmail.com> Sun Feb 27 20:43:55 2022 -0800
tree c117ff70d82c746ecc3fc7d69dd53f8f8c68e9a0
parent f61e148daa8bfdb5f34c9f7ff8443abe4e2764d0

Add confidence flag to TargetEstimate

Sigmoid function used to convert a final cost to a confidence from 0-1.

Signed-off-by: Milind Upadhyay <milind.upadhyay@gmail.com>
Change-Id: Ia71b8542c4f14a2a6678b68f0aff3cff41e65f8c

y2022/vision/target_estimator.cc

diff --git a/y2022/vision/target_estimator.cc b/y2022/vision/target_estimator.cc
index 3ebec95..6773631 100644
--- a/y2022/vision/target_estimator.cc
+++ b/y2022/vision/target_estimator.cc
@@ -140,6 +140,25 @@
             << std::chrono::duration<double, std::milli>(end - start).count()
             << " ms";
 
+  // Use a sigmoid to convert the final cost into a confidence for the
+  // localizer. Fit a sigmoid to the points of (0, 1) and two other reasonable
+  // residual-confidence combinations using
+  // https://www.desmos.com/calculator/jj7p8zk0w2
+  constexpr double kSigmoidCapacity = 1.206;
+  // Stretch the sigmoid out correctly.
+  // Currently, good estimates have final costs of around 1 to 2 pixels.
+  constexpr double kSigmoidScalar = 0.2061;
+  constexpr double kSigmoidGrowthRate = -0.1342;
+  if (centroids_.size() > 0) {
+    confidence_ = kSigmoidCapacity /
+                  (1.0 + kSigmoidScalar * std::exp(-kSigmoidGrowthRate *
+                                                   summary.final_cost));
+  } else {
+    // If we detected no blobs, the confidence should be zero and not depending
+    // on the final cost, which would be 0.
+    confidence_ = 0.0;
+  }
+
   if (FLAGS_solver_output) {
     LOG(INFO) << summary.FullReport();
 
@@ -150,6 +169,7 @@
     LOG(INFO) << "angle to camera (polar): " << angle_to_camera_;
     LOG(INFO) << "distance (polar): " << distance_;
     LOG(INFO) << "camera height: " << camera_height_;
+    LOG(INFO) << "confidence: " << confidence_;
   }
 }
 
@@ -293,9 +313,9 @@
 namespace {
 void DrawEstimateValues(double distance, double angle_to_target,
                         double angle_to_camera, double roll, double pitch,
-                        double yaw, cv::Mat view_image) {
+                        double yaw, double confidence, cv::Mat view_image) {
   constexpr int kTextX = 10;
-  int text_y = 330;
+  int text_y = 250;
   constexpr int kTextSpacing = 35;
 
   const auto kTextColor = cv::Scalar(0, 255, 255);
@@ -318,6 +338,10 @@
       absl::StrFormat("Roll: %.3f, pitch: %.3f, yaw: %.3f", roll, pitch, yaw),
       cv::Point(kTextX, text_y += kTextSpacing), cv::FONT_HERSHEY_DUPLEX,
       kFontScale, kTextColor, 2);
+
+  cv::putText(view_image, absl::StrFormat("Confidence: %.3f", confidence),
+              cv::Point(kTextX, text_y += kTextSpacing),
+              cv::FONT_HERSHEY_DUPLEX, kFontScale, kTextColor, 2);
 }
 }  // namespace
 
@@ -328,12 +352,13 @@
                      target_estimate.angle_to_camera(),
                      target_estimate.rotation_camera_hub()->roll(),
                      target_estimate.rotation_camera_hub()->pitch(),
-                     target_estimate.rotation_camera_hub()->yaw(), view_image);
+                     target_estimate.rotation_camera_hub()->yaw(),
+                     target_estimate.confidence(), view_image);
 }
 
 void TargetEstimator::DrawEstimate(cv::Mat view_image) const {
   DrawEstimateValues(distance_, angle_to_target(), angle_to_camera_, roll_,
-                     pitch_, yaw_, view_image);
+                     pitch_, yaw_, confidence_, view_image);
 }
 
 }  // namespace y2022::vision