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
diff --git a/y2022/vision/camera_reader.cc b/y2022/vision/camera_reader.cc
index 1d1f13e..637ad0a 100644
--- a/y2022/vision/camera_reader.cc
+++ b/y2022/vision/camera_reader.cc
@@ -129,6 +129,7 @@
target_estimate_builder.add_angle_to_camera(
target_estimator_.angle_to_camera());
target_estimate_builder.add_rotation_camera_hub(&rotation);
+ target_estimate_builder.add_confidence(target_estimator_.confidence());
target_estimate_builder.add_blob_result(blob_result_offset);
target_estimate_builder.add_camera_calibration(camera_calibration_offset);
target_estimate_builder.add_image_monotonic_timestamp_ns(
diff --git a/y2022/vision/target_estimate.fbs b/y2022/vision/target_estimate.fbs
index f7dc93a..42a012d 100644
--- a/y2022/vision/target_estimate.fbs
+++ b/y2022/vision/target_estimate.fbs
@@ -52,6 +52,9 @@
angle_to_camera:double (id: 3);
// Rotation of the camera in the hub's reference frame
rotation_camera_hub:Rotation (id: 4);
+ // Confidence in the estimate from 0 to 1, based on the final solver cost.
+ // Good estimates currently have confidences of around 0.9 or greater.
+ confidence:double (id: 7);
blob_result:BlobResultFbs (id: 2);
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
diff --git a/y2022/vision/target_estimator.h b/y2022/vision/target_estimator.h
index 4303b24..5d338c4 100644
--- a/y2022/vision/target_estimator.h
+++ b/y2022/vision/target_estimator.h
@@ -10,6 +10,7 @@
#include "y2022/vision/target_estimate_generated.h"
namespace y2022::vision {
+
// Class to estimate the polar coordinates and rotation from the camera to the
// target.
class TargetEstimator {
@@ -45,6 +46,8 @@
inline double angle_to_target() const { return M_PI - yaw_; }
inline double camera_height() const { return camera_height_; }
+ inline double confidence() const { return confidence_; }
+
// Draws the distance, angle, and rotation on the given image
static void DrawEstimate(const TargetEstimate &target_estimate,
cv::Mat view_image);
@@ -79,6 +82,7 @@
double distance_;
double angle_to_camera_;
double camera_height_;
+ double confidence_;
};
} // namespace y2022::vision