Auto Exposure:
- Filter results down to the three best.
- Changing exposure based on distance to the target or at random.
- Plumb exposure through the debug framework.
- Refactor into target finder.
Change-Id: Ia083f56859938bf0825472fd15d248c545f6f2fc
diff --git a/y2019/jevois/camera/image_stream.h b/y2019/jevois/camera/image_stream.h
index 578e24f..dc52bd5 100644
--- a/y2019/jevois/camera/image_stream.h
+++ b/y2019/jevois/camera/image_stream.h
@@ -39,6 +39,11 @@
void ReadEvent() override { reader_->HandleFrame(); }
+ bool SetExposure(int abs_exp) {
+ return reader_->SetCameraControl(V4L2_CID_EXPOSURE_ABSOLUTE,
+ "V4L2_CID_EXPOSURE_ABSOLUTE", abs_exp);
+ }
+
private:
std::unique_ptr<Reader> reader_;
};
diff --git a/y2019/jevois/camera/reader.h b/y2019/jevois/camera/reader.h
index 53d83c7..c5498fe 100644
--- a/y2019/jevois/camera/reader.h
+++ b/y2019/jevois/camera/reader.h
@@ -33,10 +33,11 @@
}
int fd() { return fd_; }
+ bool SetCameraControl(uint32_t id, const char *name, int value);
+
private:
void QueueBuffer(v4l2_buffer *buf);
void InitMMap();
- bool SetCameraControl(uint32_t id, const char *name, int value);
void Init();
void Start();
void MMapBuffers();
diff --git a/y2019/vision/debug_viewer.cc b/y2019/vision/debug_viewer.cc
index 4f0e747..c8d8234 100644
--- a/y2019/vision/debug_viewer.cc
+++ b/y2019/vision/debug_viewer.cc
@@ -195,6 +195,12 @@
}
}
+ int desired_exposure;
+ if (target_finder_.TestExposure(results, &desired_exposure)) {
+ printf("Switching exposure to %d.\n", desired_exposure);
+ SetExposure(desired_exposure);
+ }
+
// If the target list is not empty then we found a target.
return !results.empty();
}
diff --git a/y2019/vision/target_finder.cc b/y2019/vision/target_finder.cc
index 5860e5c..f6e5ac4 100644
--- a/y2019/vision/target_finder.cc
+++ b/y2019/vision/target_finder.cc
@@ -555,6 +555,12 @@
filtered.emplace_back(updatable_result);
}
}
+
+ // Sort the target list so that the widest (ie closest) target is first.
+ sort(filtered.begin(), filtered.end(),
+ [](const IntermediateResult &a, const IntermediateResult &b)
+ -> bool { return a.target_width > b.target_width; });
+
frame_count_++;
if (!filtered.empty()) {
valid_result_count_++;
@@ -570,5 +576,61 @@
return filtered;
}
+bool TargetFinder::TestExposure(const std::vector<IntermediateResult> &results,
+ int *desired_exposure) {
+ // TODO(ben): Add these values to config file.
+ constexpr double low_dist = 0.8;
+ constexpr double high_dist = 2.0;
+ constexpr int low_exposure = 60;
+ constexpr int mid_exposure = 300;
+ constexpr int high_exposure = 500;
+
+ bool needs_update = false;
+ if (results.size() > 0) {
+ // We are seeing a target so lets use an exposure
+ // based on the distance to that target.
+ // First result should always be the closest target.
+ if (results[0].extrinsics.z < low_dist) {
+ *desired_exposure = low_exposure;
+ } else if (results[0].extrinsics.z > high_dist) {
+ *desired_exposure = high_exposure;
+ } else {
+ *desired_exposure = mid_exposure;
+ }
+ if (*desired_exposure != current_exposure_) {
+ needs_update = true;
+ current_exposure_ = *desired_exposure;
+ }
+ } else {
+ // We don't see a target, but part of the problem might
+ // be the exposure setting. Lets try changing it and see
+ // if things get better.
+ const int offset = std::rand() % 10;
+
+ // At random with 3/X probability try a higher or lower.
+ if (offset == 0) {
+ if (low_exposure != current_exposure_) {
+ needs_update = true;
+ current_exposure_ = low_exposure;
+ *desired_exposure = low_exposure;
+ }
+ } else if (offset == 1) {
+ if (mid_exposure != current_exposure_) {
+ needs_update = true;
+ current_exposure_ = mid_exposure;
+ *desired_exposure = mid_exposure;
+ }
+ } else if (offset == 2) {
+ if (high_exposure != current_exposure_) {
+ needs_update = true;
+ current_exposure_ = high_exposure;
+ *desired_exposure = high_exposure;
+ }
+ }
+ // If one of our cases is not hit don't change anything.
+ }
+ return needs_update;
+}
+
} // namespace vision
} // namespace y2019
diff --git a/y2019/vision/target_finder.h b/y2019/vision/target_finder.h
index ff4c3f7..0f1575c 100644
--- a/y2019/vision/target_finder.h
+++ b/y2019/vision/target_finder.h
@@ -71,6 +71,9 @@
const std::vector<IntermediateResult> &results, uint64_t print_rate,
bool verbose);
+ bool TestExposure(const std::vector<IntermediateResult> &results,
+ int *desired_exposure);
+
// Get the local overlay for debug if we are doing that.
aos::vision::PixelLinesOverlay *GetOverlay() { return &overlay_; }
@@ -104,6 +107,8 @@
// Counts for logging.
size_t frame_count_;
size_t valid_result_count_;
+
+ int current_exposure_ = 0;
};
} // namespace vision
diff --git a/y2019/vision/target_geometry.cc b/y2019/vision/target_geometry.cc
index de76acb..212759a 100644
--- a/y2019/vision/target_geometry.cc
+++ b/y2019/vision/target_geometry.cc
@@ -254,6 +254,7 @@
::aos::math::NormalizeAngle(IR.backup_extrinsics.r1);
IR.backup_extrinsics.r2 =
::aos::math::NormalizeAngle(IR.backup_extrinsics.r2);
+ IR.target_width = target.width();
// Ok, let's look at how perpendicular the corners are.
// Vector from the outside to inside along the top on the left.
diff --git a/y2019/vision/target_sender.cc b/y2019/vision/target_sender.cc
index 79e2ca4..db33419 100644
--- a/y2019/vision/target_sender.cc
+++ b/y2019/vision/target_sender.cc
@@ -262,7 +262,10 @@
results = finder.FilterResults(results, 30, verbose);
LOG(INFO) << "Results: " << results.size();
- // TODO: Select top 3 (randomly?)
+ int desired_exposure;
+ if (finder.TestExposure(results, &desired_exposure)) {
+ camera0->SetExposure(desired_exposure);
+ }
frc971::jevois::CameraFrame frame{};
diff --git a/y2019/vision/target_types.h b/y2019/vision/target_types.h
index aba9300..990debc 100644
--- a/y2019/vision/target_types.h
+++ b/y2019/vision/target_types.h
@@ -48,6 +48,8 @@
TargetComponent left;
TargetComponent right;
+ double width() const { return left.inside.DistanceTo(right.inside); }
+
// Returns a target. The resulting target is in meters with 0, 0 centered
// between the upper inner corners of the two pieces of tape, y being up and x
// being to the right.
@@ -109,6 +111,9 @@
struct IntermediateResult {
ExtrinsicParams extrinsics;
+ // Width of the target in pixels. Distance from inner most points.
+ double target_width;
+
// Error from solver calulations.
double solver_error;