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/aos/vision/debug/aveugle-source.cc b/aos/vision/debug/aveugle-source.cc
index cbe3d0a..d03be14 100644
--- a/aos/vision/debug/aveugle-source.cc
+++ b/aos/vision/debug/aveugle-source.cc
@@ -46,6 +46,9 @@
++i_;
}
});
+ interface_->InstallSetExposure([this](int abs_exp) {
+ this->SetExposure(abs_exp);
+ });
}
void ProcessImage(DataRef data, aos::monotonic_clock::time_point) override {
prev_data_ = std::string(data);
diff --git a/aos/vision/debug/debug_framework.cc b/aos/vision/debug/debug_framework.cc
index 1d94217..7b3ad76 100644
--- a/aos/vision/debug/debug_framework.cc
+++ b/aos/vision/debug/debug_framework.cc
@@ -60,6 +60,11 @@
if (GetScreenHeight() < 1024) {
view_.SetScale(1.0);
}
+
+ // Pass along the set exposure so that users can acceess it.
+ filter->InstallSetExposure([this](uint32_t abs_exp) {
+ this->SetExposure(abs_exp);
+ });
}
// This the first stage in the pipeline that takes
diff --git a/aos/vision/debug/debug_framework.h b/aos/vision/debug/debug_framework.h
index a46812f..d8ed4a1 100644
--- a/aos/vision/debug/debug_framework.h
+++ b/aos/vision/debug/debug_framework.h
@@ -39,6 +39,16 @@
virtual std::function<void(uint32_t)> RegisterKeyPress() {
return std::function<void(uint32_t)>();
}
+
+ // The DebugFramework will tells us where to call to get the camera.
+ void InstallSetExposure(std::function<void(int)> set_exp) {
+ set_exposure_ = set_exp;
+ }
+ void SetExposure(int abs_exp) {
+ set_exposure_(abs_exp);
+ }
+ private:
+ std::function<void(int)> set_exposure_;
};
// For ImageSource implementations only. Allows registering key press events
@@ -51,6 +61,12 @@
key_press_events_.emplace_back(std::move(key_press_event));
}
+ // The camera will tell us where to call to set exposure.
+ void InstallSetExposure(std::function<void(int)> set_exp) {
+ set_exposure_ = std::move(set_exp);
+ }
+ void SetExposure(int abs_exp) { set_exposure_(abs_exp); }
+
// The return value bool here for all of these is
// if the frame is "interesting" ie has a target.
virtual bool NewJpeg(DataRef data) = 0;
@@ -76,6 +92,8 @@
private:
std::vector<std::function<void(uint32_t)>> key_press_events_;
+
+ std::function<void(int)> set_exposure_;
};
// Implemented by each source type. Will stream frames to
diff --git a/aos/vision/image/image_stream.h b/aos/vision/image/image_stream.h
index 57e06cb..8aab97d 100644
--- a/aos/vision/image/image_stream.h
+++ b/aos/vision/image/image_stream.h
@@ -37,6 +37,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:
void ProcessHelper(DataRef data, aos::monotonic_clock::time_point timestamp);
diff --git a/aos/vision/image/reader.cc b/aos/vision/image/reader.cc
index aaf9a10..23b58c2 100644
--- a/aos/vision/image/reader.cc
+++ b/aos/vision/image/reader.cc
@@ -85,17 +85,6 @@
}
--queued_;
- if (tick_id_ % 10 == 0) {
- if (!SetCameraControl(V4L2_CID_EXPOSURE_AUTO, "V4L2_CID_EXPOSURE_AUTO",
- V4L2_EXPOSURE_MANUAL)) {
- LOG(FATAL, "Failed to set exposure\n");
- }
-
- if (!SetCameraControl(V4L2_CID_EXPOSURE_ABSOLUTE,
- "V4L2_CID_EXPOSURE_ABSOLUTE", params_.exposure())) {
- LOG(FATAL, "Failed to set exposure\n");
- }
- }
++tick_id_;
// Get a timestamp now as proxy for when the image was taken
// TODO(ben): the image should come with a timestamp, parker
@@ -171,8 +160,6 @@
struct v4l2_control setArg = {id, value};
r = xioctl(fd_, VIDIOC_S_CTRL, &setArg);
if (r == 0) {
- LOG(DEBUG, "Set camera control %s from %d to %d\n", name, getArg.value,
- value);
return true;
}
diff --git a/aos/vision/image/reader.h b/aos/vision/image/reader.h
index adb3a3c..25fc0bd 100644
--- a/aos/vision/image/reader.h
+++ b/aos/vision/image/reader.h
@@ -32,10 +32,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/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;