Merge "Reset the websocket when the server disconnects."
diff --git a/aos/vision/math/BUILD b/aos/vision/math/BUILD
index 3233bc9..0fbdf7a 100644
--- a/aos/vision/math/BUILD
+++ b/aos/vision/math/BUILD
@@ -13,6 +13,7 @@
],
deps = [
"//third_party/eigen",
+ "@com_google_ceres_solver//:ceres",
],
)
diff --git a/frc971/control_loops/drivetrain/drivetrain_lib_test.cc b/frc971/control_loops/drivetrain/drivetrain_lib_test.cc
index d4c765f..05889e4 100644
--- a/frc971/control_loops/drivetrain/drivetrain_lib_test.cc
+++ b/frc971/control_loops/drivetrain/drivetrain_lib_test.cc
@@ -106,6 +106,13 @@
drivetrain_motor_plant_.GetRightPosition(), 1e-3);
}
+ void VerifyNearPosition(double x, double y) {
+ my_drivetrain_queue_.status.FetchLatest();
+ auto actual = drivetrain_motor_plant_.GetPosition();
+ EXPECT_NEAR(actual(0), x, 1e-2);
+ EXPECT_NEAR(actual(1), y, 1e-2);
+ }
+
void VerifyNearSplineGoal() {
my_drivetrain_queue_.status.FetchLatest();
double expected_x = my_drivetrain_queue_.status->trajectory_logging.x;
@@ -720,6 +727,29 @@
VerifyNearSplineGoal();
}
+//Tests that a trajectory can be executed after it is planned.
+TEST_F(DrivetrainTest, SplineExecuteAfterPlan) {
+ ::aos::ScopedMessagePtr<::frc971::control_loops::DrivetrainQueue::Goal> goal =
+ my_drivetrain_queue_.goal.MakeMessage();
+ goal->controller_type = 2;
+ goal->spline.spline_idx = 1;
+ goal->spline.spline_count = 1;
+ goal->spline.spline_x = {{0.0, 0.25, 0.5, 0.5, 0.75, 1.0}};
+ goal->spline.spline_y = {{0.0, 0.0, 0.25, 0.75, 1.0, 1.0}};
+ goal.Send();
+ WaitForTrajectoryPlan();
+ RunForTime(chrono::milliseconds(2000));
+
+ ::aos::ScopedMessagePtr<::frc971::control_loops::DrivetrainQueue::Goal>
+ start_goal = my_drivetrain_queue_.goal.MakeMessage();
+ start_goal->controller_type = 2;
+ start_goal->spline_handle = 1;
+ start_goal.Send();
+ RunForTime(chrono::milliseconds(2000));
+
+ VerifyNearPosition(1.0, 1.0);
+}
+
// The LineFollowDrivetrain logic is tested in line_follow_drivetrain_test. This
// tests that the integration with drivetrain_lib worked properly.
TEST_F(DrivetrainTest, BasicLineFollow) {
diff --git a/frc971/control_loops/drivetrain/splinedrivetrain.cc b/frc971/control_loops/drivetrain/splinedrivetrain.cc
index 0b6026d..a37ca0e 100644
--- a/frc971/control_loops/drivetrain/splinedrivetrain.cc
+++ b/frc971/control_loops/drivetrain/splinedrivetrain.cc
@@ -93,7 +93,7 @@
const ::frc971::control_loops::DrivetrainQueue::Goal &goal) {
current_spline_handle_ = goal.spline_handle;
// If told to stop, set the executing spline to an invalid index.
- if (current_spline_handle_ == 0) {
+ if (current_spline_handle_ == 0 && has_started_execution_) {
current_spline_idx_ = -1;
}
@@ -119,6 +119,7 @@
// Reset internal state to a trajectory start position.
current_xva_ = current_trajectory_->FFAcceleration(0);
current_xva_(1) = 0.0;
+ has_started_execution_ = false;
}
mutex_.Unlock();
}
@@ -133,6 +134,7 @@
::Eigen::Matrix<double, 2, 1> U_ff = ::Eigen::Matrix<double, 2, 1>::Zero();
if (!IsAtEnd() &&
current_spline_handle_ == current_spline_idx_) {
+ has_started_execution_ = true;
// TODO(alex): It takes about a cycle for the outputs to propagate to the
// motors. Consider delaying the output by a cycle.
U_ff = current_trajectory_->FFVoltage(current_xva_(0));
@@ -187,7 +189,7 @@
status->trajectory_logging.right_velocity = goal_state(4);
}
status->trajectory_logging.planning_state = static_cast<int8_t>(plan_state_.load());
- status->trajectory_logging.is_executing = !IsAtEnd();
+ status->trajectory_logging.is_executing = !IsAtEnd() && has_started_execution_;
status->trajectory_logging.goal_spline_handle = current_spline_handle_;
status->trajectory_logging.current_spline_idx = current_spline_idx_;
diff --git a/frc971/control_loops/drivetrain/splinedrivetrain.h b/frc971/control_loops/drivetrain/splinedrivetrain.h
index 3e47f5a..d70cb6d 100644
--- a/frc971/control_loops/drivetrain/splinedrivetrain.h
+++ b/frc971/control_loops/drivetrain/splinedrivetrain.h
@@ -70,6 +70,7 @@
int32_t current_spline_handle_ = 0; // Current spline told to excecute.
int32_t current_spline_idx_ = 0; // Current executing spline.
+ bool has_started_execution_ = false;
::std::unique_ptr<DistanceSpline> current_distance_spline_;
::std::unique_ptr<Trajectory> current_trajectory_;
diff --git a/y2019/constants.cc b/y2019/constants.cc
index 89fc009..ae6cb96 100644
--- a/y2019/constants.cc
+++ b/y2019/constants.cc
@@ -194,8 +194,8 @@
break;
case kPracticeTeamNumber:
- elevator_params->zeroing_constants.measured_absolute_position = 0.132216;
- elevator->potentiometer_offset = -0.022320 + 0.020567 - 0.022355 - 0.006497 + 0.019690 + 0.009151;
+ elevator_params->zeroing_constants.measured_absolute_position = 0.141526;
+ elevator->potentiometer_offset = -0.022320 + 0.020567 - 0.022355 - 0.006497 + 0.019690 + 0.009151 - 0.007513;
intake->zeroing_constants.measured_absolute_position = 1.756847;
diff --git a/y2019/control_loops/drivetrain/localizer.h b/y2019/control_loops/drivetrain/localizer.h
index 58b6901..1571fec 100644
--- a/y2019/control_loops/drivetrain/localizer.h
+++ b/y2019/control_loops/drivetrain/localizer.h
@@ -100,7 +100,7 @@
// The threshold to use for completely rejecting potentially bad target
// matches.
// TODO(james): Tune
- static constexpr Scalar kRejectionScore = 1000000.0;
+ static constexpr Scalar kRejectionScore = 1.0;
// Checks that the targets coming in make some sense--mostly to prevent NaNs
// or the such from propagating.
diff --git a/y2019/jevois/structures.h b/y2019/jevois/structures.h
index 9c74b11..fcf1ebe 100644
--- a/y2019/jevois/structures.h
+++ b/y2019/jevois/structures.h
@@ -144,6 +144,8 @@
// Send As, which triggers the bootstrap script to drop directly into USB
// mode.
kAs,
+ // Log camera images
+ kLog,
};
// This is all the information sent from the Teensy to each camera.
diff --git a/y2019/jevois/teensy.cc b/y2019/jevois/teensy.cc
index 0f9f94d..58d1570 100644
--- a/y2019/jevois/teensy.cc
+++ b/y2019/jevois/teensy.cc
@@ -774,6 +774,10 @@
printf("Sending USB mode\n");
stdin_camera_command = CameraCommand::kUsb;
break;
+ case 'l':
+ printf("Log mode\n");
+ stdin_camera_command = CameraCommand::kLog;
+ break;
case 'n':
printf("Sending normal mode\n");
stdin_camera_command = CameraCommand::kNormal;
@@ -798,6 +802,7 @@
printf("UART board commands:\n");
printf(" p: Send passthrough mode\n");
printf(" u: Send USB mode\n");
+ printf(" l: Send Log mode\n");
printf(" n: Send normal mode\n");
printf(" a: Send all-'a' mode\n");
printf(" c: Dump camera configuration\n");
diff --git a/y2019/joystick_reader.cc b/y2019/joystick_reader.cc
index b403913..645582d 100644
--- a/y2019/joystick_reader.cc
+++ b/y2019/joystick_reader.cc
@@ -88,6 +88,8 @@
const ButtonLocation kMidCargoHint(3, 16);
const ButtonLocation kFarCargoHint(4, 2);
+const ButtonLocation kCameraLog(3, 7);
+
const ElevatorWristPosition kStowPos{0.36, 0.0};
const ElevatorWristPosition kClimbPos{0.0, M_PI / 4.0};
@@ -107,21 +109,21 @@
const ElevatorWristPosition kPanelCargoBackwardPos{0.0, -M_PI / 2.0};
const ElevatorWristPosition kBallForwardLowerPos{0.21, 1.27};
-const ElevatorWristPosition kBallBackwardLowerPos{0.40, -1.99};
+const ElevatorWristPosition kBallBackwardLowerPos{0.43, -1.99};
-const ElevatorWristPosition kBallForwardMiddlePos{0.90, 1.21};
-const ElevatorWristPosition kBallBackwardMiddlePos{1.17, -1.98};
+const ElevatorWristPosition kBallForwardMiddlePos{0.925, 1.21};
+const ElevatorWristPosition kBallBackwardMiddlePos{1.19, -1.98};
const ElevatorWristPosition kBallForwardUpperPos{1.51, 0.961};
const ElevatorWristPosition kBallBackwardUpperPos{1.44, -1.217};
-const ElevatorWristPosition kBallCargoForwardPos{0.739044, 1.353};
-const ElevatorWristPosition kBallCargoBackwardPos{0.868265, -1.999};
+const ElevatorWristPosition kBallCargoForwardPos{0.59, 1.2};
+const ElevatorWristPosition kBallCargoBackwardPos{0.868265, -2.1};
const ElevatorWristPosition kBallHPIntakeForwardPos{0.55, 1.097};
const ElevatorWristPosition kBallHPIntakeBackwardPos{0.89, -2.018};
-const ElevatorWristPosition kBallIntakePos{0.29, 2.14};
+const ElevatorWristPosition kBallIntakePos{0.309, 2.13};
class Reader : public ::aos::input::ActionJoystickInput {
public:
@@ -319,7 +321,7 @@
if (switch_ball_) {
if (kDoBallOutake ||
(kDoBallIntake &&
- monotonic_now < last_not_has_piece_ + chrono::milliseconds(100))) {
+ monotonic_now < last_not_has_piece_ + chrono::milliseconds(200))) {
new_superstructure_goal->intake.unsafe_goal = 0.959327;
}
@@ -397,6 +399,13 @@
video_tx_->Send(vision_control_);
last_vision_control_ = monotonic_now;
}
+
+ {
+ auto camera_log_message = camera_log.MakeMessage();
+ camera_log_message->log = data.IsPressed(kCameraLog);
+ LOG_STRUCT(DEBUG, "camera_log", *camera_log_message);
+ camera_log_message.Send();
+ }
}
private:
diff --git a/y2019/status_light.q b/y2019/status_light.q
index f84ed28..c26de88 100644
--- a/y2019/status_light.q
+++ b/y2019/status_light.q
@@ -8,3 +8,9 @@
};
queue StatusLight status_light;
+
+message CameraLog {
+ bool log;
+};
+
+queue CameraLog camera_log;
diff --git a/y2019/vision/BUILD b/y2019/vision/BUILD
index bf71116..b87e2ce 100644
--- a/y2019/vision/BUILD
+++ b/y2019/vision/BUILD
@@ -34,6 +34,16 @@
tools = [":constants_formatting"],
)
+cc_library(
+ name = "image_writer",
+ srcs = ["image_writer.cc"],
+ hdrs = ["image_writer.h"],
+ deps = [
+ "//aos/logging",
+ "//aos/vision/image:image_types",
+ ],
+)
+
sh_test(
name = "constants_formatting_test",
srcs = ["constants_formatting_test.sh"],
@@ -92,6 +102,7 @@
srcs = ["target_sender.cc"],
restricted_to = VISION_TARGETS,
deps = [
+ ":image_writer",
":target_finder",
"//aos/vision/blob:codec",
"//aos/vision/blob:find_blob",
diff --git a/y2019/vision/calibrate.sh b/y2019/vision/calibrate.sh
index 24118ed..7b7ba4f 100755
--- a/y2019/vision/calibrate.sh
+++ b/y2019/vision/calibrate.sh
@@ -21,9 +21,9 @@
--tape_direction_x=-1.0 \
--tape_direction_y=0.0 \
--beginning_tape_measure_reading=11 \
- --image_count=75 \
+ --image_count=69 \
--constants=constants.cc \
- --prefix data/cam10_0/debug_viewer_jpeg_
+ --prefix data/cam10_1/debug_viewer_jpeg_
calibration \
--camera_id 6 \
diff --git a/y2019/vision/constants.cc b/y2019/vision/constants.cc
index 36694f8..fabe75c 100644
--- a/y2019/vision/constants.cc
+++ b/y2019/vision/constants.cc
@@ -133,20 +133,20 @@
CameraCalibration camera_10 = {
{
- -0.190556 / 180.0 * M_PI, 345.022, 0.468494 / 180.0 * M_PI,
+ -0.305107 / 180.0 * M_PI, 336.952, -0.0804389 / 180.0 * M_PI,
},
{
- {{-4.83005 * kInchesToMeters, 2.95565 * kInchesToMeters,
- 33.3624 * kInchesToMeters}},
- 182.204 / 180.0 * M_PI,
+ {{-5.64467 * kInchesToMeters, 2.41348 * kInchesToMeters,
+ 33.1669 * kInchesToMeters}},
+ 181.598 / 180.0 * M_PI,
},
{
10,
{{-12.5 * kInchesToMeters, 0.0}},
{{-1 * kInchesToMeters, 0.0}},
11,
- "data/cam10_0/debug_viewer_jpeg_",
- 75,
+ "data/cam10_1/debug_viewer_jpeg_",
+ 69,
}};
CameraCalibration camera_14 = {
diff --git a/y2019/vision/image_writer.cc b/y2019/vision/image_writer.cc
new file mode 100644
index 0000000..ac35114
--- /dev/null
+++ b/y2019/vision/image_writer.cc
@@ -0,0 +1,40 @@
+#include <fstream>
+#include <sys/stat.h>
+
+#include "aos/logging/logging.h"
+#include "y2019/vision/image_writer.h"
+
+namespace y2019 {
+namespace vision {
+
+ImageWriter::ImageWriter() {
+ LOG(INFO, "Initializing image writer\n");
+ SetDirPath();
+}
+
+void ImageWriter::WriteImage(::aos::vision::DataRef data) {
+ LOG(INFO, "Writing image %d", image_count_);
+ std::ofstream ofs(
+ dir_path_ + file_prefix_ + std::to_string(image_count_) + ".yuyv",
+ std::ofstream::out);
+ ofs << data;
+ ofs.close();
+ ++image_count_;
+}
+
+void ImageWriter::SetDirPath() {
+ ::std::string base_path = "/jevois/data/run_";
+ for (int i = 0;; ++i) {
+ struct stat st;
+ std::string option = base_path + std::to_string(i);
+ if (stat(option.c_str(), &st) != 0) {
+ file_prefix_ = option + "/";
+ LOG(INFO, "Writing to %s\n", file_prefix_.c_str());
+ mkdir(file_prefix_.c_str(), 0777);
+ break;
+ }
+ }
+}
+
+} // namespace vision
+} // namespace y2019
diff --git a/y2019/vision/image_writer.h b/y2019/vision/image_writer.h
new file mode 100644
index 0000000..f33cca7
--- /dev/null
+++ b/y2019/vision/image_writer.h
@@ -0,0 +1,29 @@
+#ifndef Y2019_VISION_IMAGE_WRITER_H_
+#define Y2019_VISION_IMAGE_WRITER_H_
+
+#include <string>
+
+#include "aos/vision/image/image_types.h"
+
+namespace y2019 {
+namespace vision {
+
+class ImageWriter {
+ public:
+ ImageWriter();
+
+ void WriteImage(::aos::vision::DataRef data);
+
+ private:
+ void SetDirPath();
+
+ ::std::string file_prefix_ = std::string("debug_viewer_jpeg_");
+ ::std::string dir_path_;
+
+ unsigned int image_count_ = 0;
+};
+
+} // namespace vision
+} // namespace y2017
+
+#endif // Y2019_VISION_IMAGE_WRITER_H_
diff --git a/y2019/vision/target_finder.h b/y2019/vision/target_finder.h
index 3da5373..ff4c3f7 100644
--- a/y2019/vision/target_finder.h
+++ b/y2019/vision/target_finder.h
@@ -59,6 +59,8 @@
const std::vector<TargetComponent> component_list, bool verbose);
// Given a target solve for the transformation of the template target.
+ //
+ // This is safe to call concurrently.
IntermediateResult ProcessTargetToResult(const Target &target, bool verbose);
// Returns true if a target is good, and false otherwise. Picks the 4 vs 8
diff --git a/y2019/vision/target_geometry.cc b/y2019/vision/target_geometry.cc
index 00375c1..efa2def 100644
--- a/y2019/vision/target_geometry.cc
+++ b/y2019/vision/target_geometry.cc
@@ -6,7 +6,6 @@
#include "aos/util/math.h"
-using ceres::NumericDiffCostFunction;
using ceres::CENTRAL;
using ceres::CostFunction;
using ceres::Problem;
@@ -55,77 +54,6 @@
left.inside, left.bottom}};
}
-Vector<2> Project(Vector<2> pt, const IntrinsicParams &intrinsics,
- const ExtrinsicParams &extrinsics) {
- const double y = extrinsics.y; // height
- const double z = extrinsics.z; // distance
- const double r1 = extrinsics.r1; // skew
- const double r2 = extrinsics.r2; // heading
- const double rup = intrinsics.mount_angle;
- const double rbarrel = intrinsics.barrel_mount;
- const double fl = intrinsics.focal_length;
-
- // Start by translating point in target-space to be at correct height.
- ::Eigen::Matrix<double, 1, 3> pts{pt.x(), pt.y() + y, 0.0};
-
- {
- // Rotate to compensate for skew angle, to get into a frame still at the
- // same (x, y) position as the target but rotated to be facing straight
- // towards the camera.
- const double theta = r1;
- const double s = sin(theta);
- const double c = cos(theta);
- pts = (::Eigen::Matrix<double, 3, 3>() << c, 0, -s, 0, 1, 0, s, 0,
- c).finished() *
- pts.transpose();
- }
-
- // Translate the coordinate frame to have (x, y) centered at the camera, but
- // still oriented to be facing along the line from the camera to the target.
- pts(2) += z;
-
- {
- // Rotate out the heading so that the frame is oriented to line up with the
- // camera's viewpoint in the yaw-axis.
- const double theta = r2;
- const double s = sin(theta);
- const double c = cos(theta);
- pts = (::Eigen::Matrix<double, 3, 3>() << c, 0, -s, 0, 1, 0, s, 0,
- c).finished() *
- pts.transpose();
- }
-
- // TODO: Apply 15 degree downward rotation.
- {
- // Compensate for rotation in the pitch of the camera up/down to get into
- // the coordinate frame lined up with the plane of the camera sensor.
- const double theta = rup;
- const double s = sin(theta);
- const double c = cos(theta);
-
- pts = (::Eigen::Matrix<double, 3, 3>() << 1, 0, 0, 0, c, -s, 0, s,
- c).finished() *
- pts.transpose();
- }
-
- // Compensate for rotation of the barrel of the camera, i.e. about the axis
- // that points straight out from the camera lense, using an AngleAxis instead
- // of manually constructing the rotation matrices because once you get into
- // this frame you no longer need to be masochistic.
- // TODO: Maybe barrel should be extrinsics to allow rocking?
- // Also, in this case, barrel should go above the rotation above?
- pts = ::Eigen::AngleAxis<double>(rbarrel, ::Eigen::Vector3d(0.0, 0.0, 1.0)) *
- pts.transpose();
-
- // TODO: Final image projection.
- const ::Eigen::Matrix<double, 1, 3> res = pts;
-
- // Finally, scale to account for focal length and translate to get into
- // pixel-space.
- const float scale = fl / res.z();
- return Vector<2>(res.x() * scale + 320.0, 240.0 - res.y() * scale);
-}
-
Target Project(const Target &target, const IntrinsicParams &intrinsics,
const ExtrinsicParams &extrinsics) {
Target new_targ;
@@ -150,11 +78,13 @@
IntrinsicParams intrinsics)
: result(result), template_pt(template_pt), intrinsics(intrinsics) {}
- bool operator()(const double *const x, double *residual) const {
- auto extrinsics = ExtrinsicParams::get(x);
- auto pt = result - Project(template_pt, intrinsics, extrinsics);
- residual[0] = pt.x();
- residual[1] = pt.y();
+ template <typename T>
+ bool operator()(const T *const x, T *residual) const {
+ const auto extrinsics = TemplatedExtrinsicParams<T>::get(x);
+ ::Eigen::Matrix<T, 2, 1> pt =
+ Project<T>(ToEigenMatrix<T>(template_pt), intrinsics, extrinsics);
+ residual[0] = result.x() - pt(0, 0);
+ residual[1] = result.y() - pt(0, 1);
return true;
}
@@ -170,15 +100,18 @@
IntrinsicParams intrinsics)
: result(result), template_seg(template_seg), intrinsics(intrinsics) {}
- bool operator()(const double *const x, double *residual) const {
- const auto extrinsics = ExtrinsicParams::get(x);
- const Vector<2> p1 = Project(template_seg.A(), intrinsics, extrinsics);
- const Vector<2> p2 = Project(template_seg.B(), intrinsics, extrinsics);
- // distance from line (P1, P2) to point result
- double dx = p2.x() - p1.x();
- double dy = p2.y() - p1.y();
- double denom = p2.DistanceTo(p1);
- residual[0] = ::std::abs(dy * result.x() - dx * result.y() +
+ template <typename T>
+ bool operator()(const T *const x, T *residual) const {
+ const auto extrinsics = TemplatedExtrinsicParams<T>::get(x);
+ const ::Eigen::Matrix<T, 2, 1> p1 =
+ Project<T>(ToEigenMatrix<T>(template_seg.A()), intrinsics, extrinsics);
+ const ::Eigen::Matrix<T, 2, 1> p2 =
+ Project<T>(ToEigenMatrix<T>(template_seg.B()), intrinsics, extrinsics);
+ // Distance from line(P1, P2) to point result
+ T dx = p2.x() - p1.x();
+ T dy = p2.y() - p1.y();
+ T denom = (p2-p1).norm();
+ residual[0] = ceres::abs(dy * result.x() - dx * result.y() +
p2.x() * p1.y() - p2.y() * p1.x()) /
denom;
return true;
@@ -199,24 +132,28 @@
template_seg_(template_seg),
intrinsics_(intrinsics) {}
- bool operator()(const double *const x, double *residual) const {
- const ExtrinsicParams extrinsics = ExtrinsicParams::get(x);
- const Vector<2> p1 = Project(template_seg_.A(), intrinsics_, extrinsics);
- const Vector<2> p2 = Project(template_seg_.B(), intrinsics_, extrinsics);
+ template <typename T>
+ bool operator()(const T *const x, T *residual) const {
+ const auto extrinsics = TemplatedExtrinsicParams<T>::get(x);
+ const ::Eigen::Matrix<T, 2, 1> p1 = Project<T>(
+ ToEigenMatrix<T>(template_seg_.A()), intrinsics_, extrinsics);
+ const ::Eigen::Matrix<T, 2, 1> p2 = Project<T>(
+ ToEigenMatrix<T>(template_seg_.B()), intrinsics_, extrinsics);
// Construct a vector pointed perpendicular to the line. This vector is
// pointed down out the bottom of the target.
- ::Eigen::Vector2d down_axis(-(p1.y() - p2.y()), p1.x() - p2.x());
+ ::Eigen::Matrix<T, 2, 1> down_axis;
+ down_axis << -(p1.y() - p2.y()), p1.x() - p2.x();
down_axis.normalize();
// Positive means out.
- const double component =
- down_axis.transpose() * (bottom_point_ - p1.GetData().transpose());
+ const T component =
+ down_axis.transpose() * (bottom_point_ - p1);
- if (component > 0) {
+ if (component > T(0)) {
residual[0] = component * 1.0;
} else {
- residual[0] = 0.0;
+ residual[0] = T(0);
}
return true;
}
@@ -254,18 +191,18 @@
aos::vision::Segment<2> line = Segment<2>(a, a2);
problem_4point.AddResidualBlock(
- new NumericDiffCostFunction<LineCostFunctor, CENTRAL, 1, 4>(
+ new ceres::AutoDiffCostFunction<LineCostFunctor, 1, 4>(
new LineCostFunctor(b, line, intrinsics_)),
NULL, ¶ms_4point[0]);
} else {
problem_4point.AddResidualBlock(
- new NumericDiffCostFunction<PointCostFunctor, CENTRAL, 2, 4>(
+ new ceres::AutoDiffCostFunction<PointCostFunctor, 2, 4>(
new PointCostFunctor(b, a, intrinsics_)),
NULL, ¶ms_4point[0]);
}
problem_8point.AddResidualBlock(
- new NumericDiffCostFunction<PointCostFunctor, CENTRAL, 2, 4>(
+ new ceres::AutoDiffCostFunction<PointCostFunctor, 2, 4>(
new PointCostFunctor(b, a, intrinsics_)),
NULL, ¶ms_8point[0]);
}
@@ -284,7 +221,7 @@
// Now, add a large cost for the bottom point being below the bottom line.
problem_4point.AddResidualBlock(
- new NumericDiffCostFunction<BottomPointCostFunctor, CENTRAL, 1, 4>(
+ new ceres::AutoDiffCostFunction<BottomPointCostFunctor, 1, 4>(
new BottomPointCostFunctor(target.left.bottom_point,
Segment<2>(target_template_.left.outside,
target_template_.left.bottom),
@@ -293,7 +230,7 @@
// Make sure to point the segment the other way so when we do a -pi/2 rotation
// on the line, it points down in target space.
problem_4point.AddResidualBlock(
- new NumericDiffCostFunction<BottomPointCostFunctor, CENTRAL, 1, 4>(
+ new ceres::AutoDiffCostFunction<BottomPointCostFunctor, 1, 4>(
new BottomPointCostFunctor(target.right.bottom_point,
Segment<2>(target_template_.right.bottom,
target_template_.right.outside),
diff --git a/y2019/vision/target_sender.cc b/y2019/vision/target_sender.cc
index 12e0e09..79e2ca4 100644
--- a/y2019/vision/target_sender.cc
+++ b/y2019/vision/target_sender.cc
@@ -1,6 +1,10 @@
#include "y2019/vision/target_finder.h"
+#include <condition_variable>
#include <fstream>
+#include <mutex>
+#include <random>
+#include <thread>
#include "aos/vision/blob/codec.h"
#include "aos/vision/blob/find_blob.h"
@@ -8,10 +12,10 @@
#include "aos/vision/events/udp.h"
#include "y2019/jevois/camera/image_stream.h"
#include "y2019/jevois/camera/reader.h"
-
#include "y2019/jevois/serial.h"
#include "y2019/jevois/structures.h"
#include "y2019/jevois/uart.h"
+#include "y2019/vision/image_writer.h"
// This has to be last to preserve compatibility with other headers using AOS
// logging.
@@ -66,6 +70,96 @@
using aos::vision::ImageRange;
using aos::vision::RangeImage;
using aos::vision::ImageFormat;
+using y2019::vision::TargetFinder;
+using y2019::vision::IntermediateResult;
+using y2019::vision::Target;
+
+class TargetProcessPool {
+ public:
+ // The number of threads we'll use.
+ static constexpr int kThreads = 4;
+
+ TargetProcessPool(TargetFinder *finder);
+ ~TargetProcessPool();
+
+ std::vector<IntermediateResult> Process(std::vector<const Target *> &&inputs,
+ bool verbose);
+
+ private:
+ // The main function for a thread.
+ void RunThread();
+
+ std::array<std::thread, kThreads> threads_;
+ // Coordinates access to results_/inputs_ and coordinates with
+ // condition_variable_.
+ std::mutex mutex_;
+ // Signals changes to results_/inputs_ and quit_.
+ std::condition_variable condition_variable_;
+ bool quit_ = false;
+
+ bool verbose_ = false;
+ std::vector<const Target *> inputs_;
+ std::vector<IntermediateResult> results_;
+
+ TargetFinder *const finder_;
+};
+
+TargetProcessPool::TargetProcessPool(TargetFinder *finder) : finder_(finder) {
+ for (int i = 0; i < kThreads; ++i) {
+ threads_[i] = std::thread([this]() { RunThread(); });
+ }
+}
+
+TargetProcessPool::~TargetProcessPool() {
+ {
+ std::unique_lock<std::mutex> locker(mutex_);
+ quit_ = true;
+ condition_variable_.notify_all();
+ }
+ for (int i = 0; i < kThreads; ++i) {
+ threads_[i].join();
+ }
+}
+
+std::vector<IntermediateResult> TargetProcessPool::Process(
+ std::vector<const Target *> &&inputs, bool verbose) {
+ inputs_ = std::move(inputs);
+ results_.clear();
+ verbose_ = verbose;
+ const size_t number_targets = inputs_.size();
+ {
+ std::unique_lock<std::mutex> locker(mutex_);
+ condition_variable_.notify_all();
+ while (results_.size() < number_targets) {
+ condition_variable_.wait(locker);
+ }
+ }
+ return std::move(results_);
+}
+
+void TargetProcessPool::RunThread() {
+ while (true) {
+ const Target *my_input;
+ {
+ std::unique_lock<std::mutex> locker(mutex_);
+ while (inputs_.empty()) {
+ if (quit_) {
+ return;
+ }
+ condition_variable_.wait(locker);
+ }
+ my_input = inputs_.back();
+ inputs_.pop_back();
+ }
+ IntermediateResult my_output =
+ finder_->ProcessTargetToResult(*my_input, false);
+ {
+ std::unique_lock<std::mutex> locker(mutex_);
+ results_.emplace_back(std::move(my_output));
+ condition_variable_.notify_all();
+ }
+ }
+}
int main(int argc, char **argv) {
(void)argc;
@@ -82,17 +176,25 @@
// dup2(itsDev, 1);
// dup2(itsDev, 2);
- TargetFinder finder_;
+ TargetFinder finder;
+ TargetProcessPool process_pool(&finder);
+ ImageWriter writer;
+ uint32_t image_count = 0;
+ bool log_images = false;
aos::vision::CameraParams params0;
- params0.set_exposure(50);
+ params0.set_exposure(60);
params0.set_brightness(40);
params0.set_width(640);
- params0.set_fps(15);
+ params0.set_fps(25);
params0.set_height(480);
aos::vision::FastYuyvYPooledThresholder thresholder;
+ // A source of psuedorandom numbers which gives different numbers each time we
+ // need to drop targets.
+ std::minstd_rand random_engine;
+
::std::unique_ptr<CameraStream> camera0(
new CameraStream(params0, "/dev/video0"));
camera0->set_on_frame([&](DataRef data,
@@ -100,18 +202,18 @@
aos::vision::ImageFormat fmt{640, 480};
aos::vision::BlobList imgs =
aos::vision::FindBlobs(thresholder.Threshold(fmt, data.data(), 120));
- finder_.PreFilter(&imgs);
+ finder.PreFilter(&imgs);
LOG(INFO) << "Blobs: " << imgs.size();
constexpr bool verbose = false;
::std::vector<Polygon> raw_polys;
for (const RangeImage &blob : imgs) {
// Convert blobs to contours in the corrected space.
- ContourNode *contour = finder_.GetContour(blob);
+ ContourNode *contour = finder.GetContour(blob);
::std::vector<::Eigen::Vector2f> unwarped_contour =
- finder_.UnWarpContour(contour);
+ finder.UnWarpContour(contour);
const Polygon polygon =
- finder_.FindPolygon(::std::move(unwarped_contour), verbose);
+ finder.FindPolygon(::std::move(unwarped_contour), verbose);
if (!polygon.segments.empty()) {
raw_polys.push_back(polygon);
}
@@ -120,22 +222,44 @@
// Calculate each component side of a possible target.
::std::vector<TargetComponent> target_component_list =
- finder_.FillTargetComponentList(raw_polys, verbose);
+ finder.FillTargetComponentList(raw_polys, verbose);
LOG(INFO) << "Components: " << target_component_list.size();
// Put the compenents together into targets.
::std::vector<Target> target_list =
- finder_.FindTargetsFromComponents(target_component_list, verbose);
- LOG(INFO) << "Potential Target: " << target_list.size();
+ finder.FindTargetsFromComponents(target_component_list, verbose);
+ static constexpr size_t kMaximumPotentialTargets = 8;
+ LOG(INFO) << "Potential Targets (will filter to "
+ << kMaximumPotentialTargets << "): " << target_list.size();
+
+ // A list of all the indices into target_list which we're going to actually
+ // use.
+ std::vector<int> target_list_indices;
+ target_list_indices.resize(target_list.size());
+ for (size_t i = 0; i < target_list.size(); ++i) {
+ target_list_indices[i] = i;
+ }
+ // Drop random elements until we get sufficiently few of them. We drop
+ // different elements each time to ensure we will see different valid
+ // targets on successive frames, which provides more useful information to
+ // the localization.
+ while (target_list_indices.size() > kMaximumPotentialTargets) {
+ std::uniform_int_distribution<size_t> distribution(
+ 0, target_list_indices.size() - 1);
+ const size_t index = distribution(random_engine);
+ target_list_indices.erase(target_list_indices.begin() + index);
+ }
// Use the solver to generate an intermediate version of our results.
- ::std::vector<IntermediateResult> results;
- for (const Target &target : target_list) {
- results.emplace_back(finder_.ProcessTargetToResult(target, verbose));
+ std::vector<const Target *> inputs;
+ for (size_t index : target_list_indices) {
+ inputs.push_back(&target_list[index]);
}
+ std::vector<IntermediateResult> results =
+ process_pool.Process(std::move(inputs), verbose);
LOG(INFO) << "Raw Results: " << results.size();
- results = finder_.FilterResults(results, 30, verbose);
+ results = finder.FilterResults(results, 30, verbose);
LOG(INFO) << "Results: " << results.size();
// TODO: Select top 3 (randomly?)
@@ -167,6 +291,13 @@
LOG(INFO) << "Some problem happened";
}
}
+
+ if (log_images) {
+ if ((image_count % 5) == 0) {
+ writer.WriteImage(data);
+ }
+ ++image_count;
+ }
});
aos::events::EpollLoop loop;
@@ -187,7 +318,7 @@
frc971::jevois::UartUnpackToCamera(packet);
if (calibration_question) {
const auto &calibration = *calibration_question;
- IntrinsicParams *intrinsics = finder_.mutable_intrinsics();
+ IntrinsicParams *intrinsics = finder.mutable_intrinsics();
intrinsics->mount_angle = calibration.calibration(0, 0);
intrinsics->focal_length = calibration.calibration(0, 1);
intrinsics->barrel_mount = calibration.calibration(0, 2);
@@ -195,6 +326,10 @@
switch (calibration.camera_command) {
case CameraCommand::kNormal:
case CameraCommand::kAs:
+ log_images = false;
+ break;
+ case CameraCommand::kLog:
+ log_images = true;
break;
case CameraCommand::kUsb:
return 0;
diff --git a/y2019/vision/target_types.h b/y2019/vision/target_types.h
index e97c051..aba9300 100644
--- a/y2019/vision/target_types.h
+++ b/y2019/vision/target_types.h
@@ -1,6 +1,8 @@
#ifndef _Y2019_VISION_TARGET_TYPES_H_
#define _Y2019_VISION_TARGET_TYPES_H_
+#include "Eigen/Dense"
+
#include "aos/vision/math/segment.h"
#include "aos/vision/math/vector.h"
#include "y2019/vision/constants.h"
@@ -54,28 +56,29 @@
std::array<aos::vision::Vector<2>, 8> ToPointList() const;
};
-struct ExtrinsicParams {
+template <typename Scalar>
+struct TemplatedExtrinsicParams {
static constexpr size_t kNumParams = 4;
// Height of the target
- double y = 18.0 * 0.0254;
+ Scalar y = Scalar(18.0 * 0.0254);
// Distance to the target
- double z = 23.0 * 0.0254;
+ Scalar z = Scalar(23.0 * 0.0254);
// Skew of the target relative to the line-of-sight from the camera to the
// target.
- double r1 = 1.0 / 180 * M_PI;
+ Scalar r1 = Scalar(1.0 / 180 * M_PI);
// Heading from the camera to the target, relative to the center of the view
// from the camera.
- double r2 = -1.0 / 180 * M_PI;
+ Scalar r2 = Scalar(-1.0 / 180 * M_PI);
- void set(double *data) {
+ void set(Scalar *data) {
data[0] = y;
data[1] = z;
data[2] = r1;
data[3] = r2;
}
- static ExtrinsicParams get(const double *data) {
- ExtrinsicParams out;
+ static TemplatedExtrinsicParams get(const Scalar *data) {
+ TemplatedExtrinsicParams out;
out.y = data[0];
out.z = data[1];
out.r1 = data[2];
@@ -84,10 +87,18 @@
}
};
+using ExtrinsicParams = TemplatedExtrinsicParams<double>;
+
// Projects a point from idealized template space to camera space.
+template <typename Extrinsics>
aos::vision::Vector<2> Project(aos::vision::Vector<2> pt,
- const IntrinsicParams &intrinsics,
- const ExtrinsicParams &extrinsics);
+ const IntrinsicParams &intrinsics,
+ const Extrinsics &extrinsics);
+
+template <typename T, typename Extrinsics>
+::Eigen::Matrix<T, 2, 1> Project(::Eigen::Matrix<T, 2, 1> pt,
+ const IntrinsicParams &intrinsics,
+ const Extrinsics &extrinsics);
Target Project(const Target &target, const IntrinsicParams &intrinsics,
const ExtrinsicParams &extrinsics);
@@ -130,6 +141,96 @@
float skew;
};
+template<typename T>
+::Eigen::Matrix<T, 2, 1> ToEigenMatrix(aos::vision::Vector<2> pt) {
+ return (::Eigen::Matrix<T, 2, 1>() << T(pt.x()), T(pt.y())).finished();
+}
+
+template <typename Extrinsics>
+aos::vision::Vector<2> Project(aos::vision::Vector<2> pt,
+ const IntrinsicParams &intrinsics,
+ const Extrinsics &extrinsics) {
+ const ::Eigen::Matrix<double, 2, 1> eigen_pt = ToEigenMatrix<double>(pt);
+ const ::Eigen::Matrix<double, 2, 1> res =
+ Project(eigen_pt, intrinsics, extrinsics);
+ return aos::vision::Vector<2>(res(0, 0), res(1, 0));
+}
+
+template <typename T, typename Extrinsics>
+::Eigen::Matrix<T, 2, 1> Project(::Eigen::Matrix<T, 2, 1> pt,
+ const IntrinsicParams &intrinsics,
+ const Extrinsics &extrinsics) {
+ const T y = extrinsics.y; // height
+ const T z = extrinsics.z; // distance
+ const T r1 = extrinsics.r1; // skew
+ const T r2 = extrinsics.r2; // heading
+ const double rup = intrinsics.mount_angle;
+ const double rbarrel = intrinsics.barrel_mount;
+ const double fl = intrinsics.focal_length;
+
+ // Start by translating point in target-space to be at correct height.
+ ::Eigen::Matrix<T, 3, 1> pts{pt(0, 0), pt(1, 0) + y, T(0.0)};
+
+ {
+ // Rotate to compensate for skew angle, to get into a frame still at the
+ // same (x, y) position as the target but rotated to be facing straight
+ // towards the camera.
+ const T theta = r1;
+ const T s = sin(theta);
+ const T c = cos(theta);
+ pts = (::Eigen::Matrix<T, 3, 3>() << c, T(0), -s, T(0), T(1), T(0), s, T(0),
+ c).finished() *
+ pts;
+ }
+
+ // Translate the coordinate frame to have (x, y) centered at the camera, but
+ // still oriented to be facing along the line from the camera to the target.
+ pts(2) += z;
+
+ {
+ // Rotate out the heading so that the frame is oriented to line up with the
+ // camera's viewpoint in the yaw-axis.
+ const T theta = r2;
+ const T s = sin(theta);
+ const T c = cos(theta);
+ pts = (::Eigen::Matrix<T, 3, 3>() << c, T(0), -s, T(0), T(1), T(0), s, T(0),
+ c).finished() *
+ pts;
+ }
+
+ // TODO: Apply 15 degree downward rotation.
+ {
+ // Compensate for rotation in the pitch of the camera up/down to get into
+ // the coordinate frame lined up with the plane of the camera sensor.
+ const double theta = rup;
+ const T s = T(sin(theta));
+ const T c = T(cos(theta));
+
+ pts = (::Eigen::Matrix<T, 3, 3>() << T(1), T(0), T(0), T(0), c, -s, T(0), s,
+ c).finished() *
+ pts;
+ }
+
+ // Compensate for rotation of the barrel of the camera, i.e. about the axis
+ // that points straight out from the camera lense, using an AngleAxis instead
+ // of manually constructing the rotation matrices because once you get into
+ // this frame you no longer need to be masochistic.
+ // TODO: Maybe barrel should be extrinsics to allow rocking?
+ // Also, in this case, barrel should go above the rotation above?
+ pts = ::Eigen::AngleAxis<T>(T(rbarrel),
+ ::Eigen::Matrix<T, 3, 1>(T(0), T(0), T(1))) *
+ pts;
+
+ // TODO: Final image projection.
+ const ::Eigen::Matrix<T, 3, 1> res = pts;
+
+ // Finally, scale to account for focal length and translate to get into
+ // pixel-space.
+ const T scale = fl / res.z();
+ return ::Eigen::Matrix<T, 2, 1>(res.x() * scale + 320.0,
+ 240.0 - res.y() * scale);
+}
+
} // namespace vision
} // namespace y2019
diff --git a/y2019/wpilib_interface.cc b/y2019/wpilib_interface.cc
index e75b825..95ed28e 100644
--- a/y2019/wpilib_interface.cc
+++ b/y2019/wpilib_interface.cc
@@ -314,10 +314,13 @@
using namespace frc971::jevois;
RoborioToTeensy to_teensy{};
to_teensy.realtime_now = aos::realtime_clock::now();
+ camera_log.FetchLatest();
if (activate_usb_ && !activate_usb_->Get()) {
to_teensy.camera_command = CameraCommand::kUsb;
} else if (activate_passthrough_ && !activate_passthrough_->Get()) {
to_teensy.camera_command = CameraCommand::kCameraPassthrough;
+ } else if (camera_log.get() && camera_log->log) {
+ to_teensy.camera_command = CameraCommand::kLog;
} else {
to_teensy.camera_command = CameraCommand::kNormal;
}