Adding promised angle and distance computations.
Change-Id: Icd2c404da6d7484d3f58e109a6c6072684bf3850
diff --git a/y2017/vision/BUILD b/y2017/vision/BUILD
index c7b4b7e..14811d9 100644
--- a/y2017/vision/BUILD
+++ b/y2017/vision/BUILD
@@ -64,6 +64,7 @@
'//aos/vision/events:udp',
':vision_queue',
':vision_result',
+ ':target_finder',
'//aos/common:mutex',
],
)
diff --git a/y2017/vision/target_finder.cc b/y2017/vision/target_finder.cc
index 1f2c46d..22943ef 100644
--- a/y2017/vision/target_finder.cc
+++ b/y2017/vision/target_finder.cc
@@ -233,5 +233,49 @@
return true;
}
+namespace {
+
+constexpr double kInchesToMeters = 0.0254;
+
+} // namespace
+
+void RotateAngle(aos::vision::Vector<2> vec, double angle, double *rx,
+ double *ry) {
+ double cos_ang = std::cos(angle);
+ double sin_ang = std::sin(angle);
+ *rx = vec.x() * cos_ang - vec.y() * sin_ang;
+ *ry = vec.x() * sin_ang + vec.y() * cos_ang;
+}
+
+void TargetFinder::GetAngleDist(const aos::vision::Vector<2>& target,
+ double down_angle, double *dist,
+ double *angle) {
+ // TODO(ben): Will put all these numbers in a config file before
+ // the first competition. I hope.
+ double focal_length = 1418.6;
+ double mounted_angle_deg = 33.5;
+ double camera_angle = mounted_angle_deg * M_PI / 180.0 - down_angle;
+ double window_height = 960.0;
+ double window_width = 1280.0;
+
+ double target_height = 78.0;
+ double camera_height = 21.5;
+ double tape_width = 2;
+ double world_height = tape_width + target_height - camera_height;
+
+ double target_to_boiler = 9.5;
+ double robot_to_camera = 9.5;
+ double added_dist = target_to_boiler + robot_to_camera;
+
+ double px = target.x() - window_width / 2.0;
+ double py = target.y() - window_height / 2.0;
+ double pz = focal_length;
+ RotateAngle(aos::vision::Vector<2>(pz, -py), camera_angle, &pz, &py);
+ double pl = std::sqrt(pz * pz + px * px);
+
+ *dist = kInchesToMeters * (world_height * pl / py - added_dist);
+ *angle = std::atan2(px, pz);
+}
+
} // namespace vision
} // namespace y2017
diff --git a/y2017/vision/target_finder.h b/y2017/vision/target_finder.h
index 79417d1..5ee143d 100644
--- a/y2017/vision/target_finder.h
+++ b/y2017/vision/target_finder.h
@@ -70,6 +70,10 @@
// Get the local overlay for debug if we are doing that.
aos::vision::PixelLinesOverlay *GetOverlay() { return &overlay_; }
+ // Convert target location into meters and radians.
+ void GetAngleDist(const aos::vision::Vector<2>& target, double down_angle,
+ double *dist, double *angle);
+
private:
// Find a loosly connected target.
double DetectConnectedTarget(const RangeImage &img);
diff --git a/y2017/vision/target_receiver.cc b/y2017/vision/target_receiver.cc
index 851d3a5..f96a95e 100644
--- a/y2017/vision/target_receiver.cc
+++ b/y2017/vision/target_receiver.cc
@@ -5,28 +5,18 @@
#include "aos/common/time.h"
#include "aos/linux_code/init.h"
#include "aos/vision/events/udp.h"
+#include "y2017/vision/target_finder.h"
#include "y2017/vision/vision.q.h"
#include "y2017/vision/vision_result.pb.h"
using aos::monotonic_clock;
-namespace y2017 {
-namespace vision {
-
-void ComputeDistanceAngle(const TargetResult &target, double *distance,
- double *angle) {
- // TODO: fix this.
- *distance = target.y();
- *angle = target.x();
-}
-
-} // namespace vision
-} // namespace y2017
-
int main() {
using namespace y2017::vision;
::aos::events::RXUdpSocket recv(8080);
char raw_data[65507];
+ // TODO(parker): Have this pull in a config from somewhere.
+ TargetFinder finder;
while (true) {
// TODO(austin): Don't malloc.
@@ -52,8 +42,10 @@
target_time.time_since_epoch())
.count();
- ComputeDistanceAngle(target.target(), &new_vision_status->distance,
- &new_vision_status->angle);
+ finder.GetAngleDist(
+ aos::vision::Vector<2>(target.target().x(), target.target().y()),
+ /* TODO: Insert down estimate here in radians: */ 0.0,
+ &new_vision_status->distance, &new_vision_status->angle);
}
LOG_STRUCT(DEBUG, "vision", *new_vision_status);