Converted vision to interpolating angle based on capture timestamp.
Change-Id: I4885eb7eb1254af7266a5bd4c91d5d1b14b09eb2
diff --git a/y2016/vision/target_receiver.cc b/y2016/vision/target_receiver.cc
index b51e4ed..07c5ea0 100644
--- a/y2016/vision/target_receiver.cc
+++ b/y2016/vision/target_receiver.cc
@@ -19,7 +19,8 @@
namespace y2016 {
namespace vision {
-::aos::vision::Vector<2> CreateCenterFromTarget(double lx, double ly, double rx, double ry) {
+::aos::vision::Vector<2> CreateCenterFromTarget(double lx, double ly, double rx,
+ double ry) {
return ::aos::vision::Vector<2>((lx + rx) / 2.0, (ly + ry) / 2.0);
}
@@ -77,15 +78,16 @@
double good_ang = min_angle;
double min_ang_err = -1.0;
int right_index = -1;
- // Now pick the bottom edge angle from the right that lines up best with the left.
+ // Now pick the bottom edge angle from the right that lines up best with the
+ // left.
for (int j = 0; j < right_target.target_size(); j++) {
double wid2 = TargetWidth(right_target.target(j).left_corner_x(),
- right_target.target(j).left_corner_y(),
- right_target.target(j).right_corner_x(),
- right_target.target(j).right_corner_y());
+ right_target.target(j).left_corner_y(),
+ right_target.target(j).right_corner_x(),
+ right_target.target(j).right_corner_y());
h = right_target.target(j).left_corner_y() -
right_target.target(j).right_corner_y();
- double ang = h/ wid2;
+ double ang = h / wid2;
double ang_err = ::std::abs(good_ang - ang);
if (min_ang_err == -1.0 || min_ang_err > ang_err) {
min_ang_err = ang_err;
@@ -105,69 +107,137 @@
right_target.target(right_index).right_corner_y());
}
+class CameraHandler {
+ public:
+ void Received(const VisionData &target, ::aos::time::Time now) {
+ if (current_.received) {
+ last_ = current_;
+ }
+ current_.target = target;
+ current_.rx_time = now;
+ current_.capture_time =
+ now - ::aos::time::Time::InNS(target.send_timestamp() -
+ target.image_timestamp());
+ current_.received = true;
+ }
+
+ void CheckStale(::aos::time::Time now) {
+ if (now > current_.rx_time + ::aos::time::Time::InMS(50)) {
+ current_.received = false;
+ last_.received = false;
+ }
+ }
+
+ bool received_both() const { return current_.received && last_.received; }
+
+ bool is_valid() const {
+ return current_.target.target_size() > 0 && last_.target.target_size() > 0;
+ }
+
+ const VisionData &target() const { return current_.target; }
+ const VisionData &last_target() const { return last_.target; }
+
+ ::aos::time::Time capture_time() const { return current_.capture_time; }
+ ::aos::time::Time last_capture_time() const { return last_.capture_time; }
+
+ private:
+ struct TargetWithTimes {
+ VisionData target;
+ ::aos::time::Time rx_time{0, 0};
+ ::aos::time::Time capture_time{0, 0};
+ bool received = false;
+ };
+
+ TargetWithTimes current_;
+ TargetWithTimes last_;
+};
+
+::aos::vision::Vector<2> CalculateFiltered(
+ const CameraHandler &older, const CameraHandler &newer,
+ const ::aos::vision::Vector<2> &newer_center,
+ const ::aos::vision::Vector<2> &last_newer_center) {
+ const double age_ratio =
+ (older.capture_time() - newer.last_capture_time()).ToSeconds() /
+ (newer.capture_time() - newer.last_capture_time()).ToSeconds();
+ return ::aos::vision::Vector<2>(
+ newer_center.x() * age_ratio + (1 - age_ratio) * last_newer_center.x(),
+ newer_center.y() * age_ratio + (1 - age_ratio) * last_newer_center.y());
+}
void Main() {
StereoGeometry stereo(constants::GetValues().vision_name);
LOG(INFO, "calibration: %s\n",
stereo.calibration().ShortDebugString().c_str());
- VisionData left_target;
- aos::time::Time left_rx_time{0, 0};
-
- VisionData right_target;
- aos::time::Time right_rx_time{0, 0};
+ CameraHandler left;
+ CameraHandler right;
::aos::vision::RXUdpSocket recv(8080);
char rawData[65507];
- bool got_left = false;
- bool got_right = false;
while (true) {
// TODO(austin): Don't malloc.
VisionData target;
int size = recv.Recv(rawData, 65507);
- aos::time::Time now = aos::time::Time::Now();
+ ::aos::time::Time now = ::aos::time::Time::Now();
if (target.ParseFromArray(rawData, size)) {
if (target.camera_index() == 0) {
- left_target = target;
- left_rx_time = now;
- got_left = true;
+ left.Received(target, now);
} else {
- right_target = target;
- right_rx_time = now;
- got_right = true;
+ right.Received(target, now);
}
} else {
LOG(ERROR, "oh noes: parse error\n");
continue;
}
- if (now > left_rx_time + aos::time::Time::InMS(50)) {
- got_left = false;
- }
- if (now > right_rx_time + aos::time::Time::InMS(50)) {
- got_right = false;
- }
+ left.CheckStale(now);
+ right.CheckStale(now);
- if (got_left && got_right) {
- bool left_image_valid = left_target.target_size() > 0;
- bool right_image_valid = right_target.target_size() > 0;
+ if (left.received_both() && right.received_both()) {
+ const bool left_image_valid = left.is_valid();
+ const bool right_image_valid = right.is_valid();
auto new_vision_status = vision_status.MakeMessage();
new_vision_status->left_image_valid = left_image_valid;
new_vision_status->right_image_valid = right_image_valid;
if (left_image_valid && right_image_valid) {
- ::aos::vision::Vector<2> center0(0.0, 0.0);
- ::aos::vision::Vector<2> center1(0.0, 0.0);
- SelectTargets(left_target, right_target, ¢er0, ¢er1);
+ ::aos::vision::Vector<2> center_left(0.0, 0.0);
+ ::aos::vision::Vector<2> center_right(0.0, 0.0);
+ SelectTargets(left.target(), right.target(), ¢er_left,
+ ¢er_right);
+
+ // TODO(Ben): Remember this from last time instead of recalculating it
+ // each time.
+ ::aos::vision::Vector<2> last_center_left(0.0, 0.0);
+ ::aos::vision::Vector<2> last_center_right(0.0, 0.0);
+ SelectTargets(left.last_target(), right.last_target(),
+ &last_center_left, &last_center_right);
+
+ ::aos::vision::Vector<2> filtered_center_left(0.0, 0.0);
+ ::aos::vision::Vector<2> filtered_center_right(0.0, 0.0);
+ if (left.capture_time() < right.capture_time()) {
+ filtered_center_left = center_left;
+ new_vision_status->target_time = left.capture_time().ToNSec();
+ filtered_center_right =
+ CalculateFiltered(left, right, center_right, last_center_right);
+ } else {
+ filtered_center_right = center_right;
+ new_vision_status->target_time = right.capture_time().ToNSec();
+ filtered_center_right =
+ CalculateFiltered(right, left, center_left, last_center_left);
+ }
+
double distance, horizontal_angle, vertical_angle;
- stereo.Process(center0, center1, &distance, &horizontal_angle,
- &vertical_angle);
- new_vision_status->left_image_timestamp = left_target.image_timestamp();
- new_vision_status->right_image_timestamp = right_target.image_timestamp();
- new_vision_status->left_send_timestamp = left_target.send_timestamp();
- new_vision_status->right_send_timestamp = right_target.send_timestamp();
+ stereo.Process(filtered_center_left, filtered_center_right, &distance,
+ &horizontal_angle, &vertical_angle);
+ new_vision_status->left_image_timestamp =
+ left.target().image_timestamp();
+ new_vision_status->right_image_timestamp =
+ right.target().image_timestamp();
+ new_vision_status->left_send_timestamp = left.target().send_timestamp();
+ new_vision_status->right_send_timestamp = right.target().send_timestamp();
new_vision_status->horizontal_angle = horizontal_angle;
new_vision_status->vertical_angle = vertical_angle;
new_vision_status->distance = distance;
@@ -180,9 +250,10 @@
}
if (target.camera_index() == 0) {
- LOG(DEBUG, "left_target: %s\n", left_target.ShortDebugString().c_str());
+ LOG(DEBUG, "left_target: %s\n", left.target().ShortDebugString().c_str());
} else {
- LOG(DEBUG, "right_target: %s\n", right_target.ShortDebugString().c_str());
+ LOG(DEBUG, "right_target: %s\n",
+ right.target().ShortDebugString().c_str());
}
}
}