Merge changes I3e764082,I73bfde64,I94fd3166
* changes:
Account for delayed U in using localizer
Add interface for target selector
Add basic line-following drivetrain
diff --git a/aos/vision/blob/contour.h b/aos/vision/blob/contour.h
index 9cd1400..bbd59a9 100644
--- a/aos/vision/blob/contour.h
+++ b/aos/vision/blob/contour.h
@@ -25,6 +25,7 @@
ContourNode *pappend(int x, int y, AnalysisAllocator *alloc) {
return alloc->cons_obj<ContourNode>(x, y, this);
}
+ void set_point(Point new_pt) { pt = new_pt; }
Point pt;
ContourNode *next;
diff --git a/aos/vision/blob/range_image.cc b/aos/vision/blob/range_image.cc
index 946859d..c01a919 100644
--- a/aos/vision/blob/range_image.cc
+++ b/aos/vision/blob/range_image.cc
@@ -69,17 +69,17 @@
RangeImage rimg = MergeRangeImage(blobl);
int minx = rimg.ranges()[0][0].st;
int maxx = 0;
- for (const auto &range : rimg.ranges()) {
- for (const auto &span : range) {
+ for (const std::vector<ImageRange> &range : rimg.ranges()) {
+ for (const ImageRange &span : range) {
if (span.st < minx) minx = span.st;
if (span.ed > maxx) maxx = span.ed;
}
}
printf("maxx: %d minx: %d\n", maxx, minx);
char buf[maxx - minx];
- for (const auto &range : rimg.ranges()) {
+ for (const std::vector<ImageRange> &range : rimg.ranges()) {
int i = minx;
- for (const auto &span : range) {
+ for (const ImageRange &span : range) {
for (; i < span.st; ++i) buf[i - minx] = ' ';
for (; i < span.ed; ++i) buf[i - minx] = '#';
}
diff --git a/frc971/control_loops/drivetrain/libspline.cc b/frc971/control_loops/drivetrain/libspline.cc
index f931e59..1d7aa0b 100644
--- a/frc971/control_loops/drivetrain/libspline.cc
+++ b/frc971/control_loops/drivetrain/libspline.cc
@@ -197,9 +197,9 @@
void TrajectoryGetPlanXVA(::std::vector<::Eigen::Matrix<double, 3, 1>> *vec,
double *X, double *V, double *A) {
for (size_t i = 0; i < vec->size(); ++i) {
- X[i] = (*vec)[0][0];
- V[i] = (*vec)[0][1];
- A[i] = (*vec)[0][2];
+ X[i] = (*vec)[i][0];
+ V[i] = (*vec)[i][1];
+ A[i] = (*vec)[i][2];
}
}
diff --git a/motors/print/BUILD b/motors/print/BUILD
index 26ee976..26d8e45 100644
--- a/motors/print/BUILD
+++ b/motors/print/BUILD
@@ -8,6 +8,7 @@
restricted_to = mcu_cpus,
visibility = ["//visibility:public"],
deps = [
+ "//aos/containers:sized_array",
"//motors/core",
"//third_party/GSL",
],
diff --git a/motors/print/print.h b/motors/print/print.h
index d51a2c3..89da913 100644
--- a/motors/print/print.h
+++ b/motors/print/print.h
@@ -3,6 +3,7 @@
#include <memory>
+#include "aos/containers/sized_array.h"
#include "motors/core/kinetis.h"
#include "third_party/GSL/include/gsl/gsl"
@@ -29,6 +30,11 @@
// Writes something to a separate debug stream. Some implementations will
// always ignore this, and others will ignore it under some conditions.
virtual int WriteDebug(gsl::span<const char> buffer) { return buffer.size(); }
+
+ // Reads any characters which are available (never blocks).
+ //
+ // The default never returns any data.
+ virtual aos::SizedArray<char, 4> ReadStdin() { return {}; }
};
// A trivial printing "implementation" which simply does nothing. This is used
diff --git a/motors/print/usb.h b/motors/print/usb.h
index 7e63184..981c5d3 100644
--- a/motors/print/usb.h
+++ b/motors/print/usb.h
@@ -28,6 +28,12 @@
return debug_tty_->Write(buffer.data(), buffer.size());
}
+ aos::SizedArray<char, 4> ReadStdin() override {
+ aos::SizedArray<char, 4> result;
+ result.set_size(stdout_tty_->Read(result.data(), result.max_size()));
+ return result;
+ }
+
private:
teensy::AcmTty *const stdout_tty_;
teensy::AcmTty *const debug_tty_;
@@ -50,6 +56,12 @@
return debug_tty_.Write(buffer.data(), buffer.size());
}
+ aos::SizedArray<char, 4> ReadStdin() override {
+ aos::SizedArray<char, 4> result;
+ result.set_size(stdout_tty_.Read(result.data(), result.max_size()));
+ return result;
+ }
+
private:
teensy::UsbDevice usb_device_;
teensy::AcmTty stdout_tty_;
diff --git a/y2019/jevois/spi.cc b/y2019/jevois/spi.cc
index 2d3e2f2..b6e6632 100644
--- a/y2019/jevois/spi.cc
+++ b/y2019/jevois/spi.cc
@@ -252,6 +252,8 @@
memcpy(remaining_space.data(), &realtime_now, sizeof(realtime_now));
remaining_space = remaining_space.subspan(sizeof(realtime_now));
}
+ memcpy(remaining_space.data(), &message.camera_command, 1);
+ remaining_space = remaining_space.subspan(1);
{
uint16_t crc = jevois_crc_init();
crc = jevois_crc_update(crc, transfer.data(),
@@ -281,6 +283,8 @@
aos::realtime_clock::duration(realtime_now));
remaining_input = remaining_input.subspan(sizeof(realtime_now));
}
+ memcpy(&message.camera_command, remaining_input.data(), 1);
+ remaining_input = remaining_input.subspan(1);
{
uint16_t calculated_crc = jevois_crc_init();
calculated_crc =
diff --git a/y2019/jevois/spi_test.cc b/y2019/jevois/spi_test.cc
index 8f08fcb..32551db 100644
--- a/y2019/jevois/spi_test.cc
+++ b/y2019/jevois/spi_test.cc
@@ -107,7 +107,28 @@
// Tests packing and then unpacking an empty message.
TEST(SpiToTeensyPackTest, Empty) {
+ RoborioToTeensy input_message{};
+ const SpiTransfer transfer = SpiPackToTeensy(input_message);
+ const auto output_message = SpiUnpackToTeensy(transfer);
+ ASSERT_TRUE(output_message);
+ EXPECT_EQ(input_message, output_message.value());
+}
+
+// Tests packing and then unpacking a message with all the fields set.
+TEST(SpiToTeensyPackTest, Full) {
RoborioToTeensy input_message;
+ input_message.beacon_brightness[0] = 9;
+ input_message.beacon_brightness[1] = 7;
+ input_message.beacon_brightness[2] = 1;
+ input_message.light_rings[0] = 1;
+ input_message.light_rings[1] = 0;
+ input_message.light_rings[2] = 0;
+ input_message.light_rings[3] = 1;
+ input_message.light_rings[4] = 0;
+ input_message.realtime_now =
+ aos::realtime_clock::epoch() + std::chrono::seconds(971254);
+ input_message.camera_command = CameraCommand::kUsb;
+
const SpiTransfer transfer = SpiPackToTeensy(input_message);
const auto output_message = SpiUnpackToTeensy(transfer);
ASSERT_TRUE(output_message);
diff --git a/y2019/jevois/structures.h b/y2019/jevois/structures.h
index 28da405..1f10dc0 100644
--- a/y2019/jevois/structures.h
+++ b/y2019/jevois/structures.h
@@ -106,17 +106,17 @@
camera_duration age;
};
+enum class CameraCommand : char {
+ // Stay in normal mode.
+ kNormal,
+ // Go to camera passthrough mode.
+ kCameraPassthrough,
+ // Go to being a useful USB device.
+ kUsb,
+};
+
// This is all the information sent from the Teensy to each camera.
struct CameraCalibration {
- enum class CameraCommand : char {
- // Stay in normal mode.
- kNormal,
- // Go to camera passthrough mode.
- kCameraPassthrough,
- // Go to being a useful USB device.
- kUsb,
- };
-
bool operator==(const CameraCalibration &other) const {
if (other.calibration != calibration) {
return false;
@@ -139,6 +139,9 @@
// The calibration matrix. This defines where the camera is pointing.
//
// TODO(Parker): What are the details on how this is defined?
+ // [0][0]: mount_angle
+ // [0][1]: focal_length
+ // [0][2]: barrel_mount
Eigen::Matrix<float, 3, 4> calibration;
// A local timestamp from the Teensy. This starts at 0 when the Teensy is
@@ -194,6 +197,9 @@
// The current time.
aos::realtime_clock::time_point realtime_now;
+
+ // A command to send to all the cameras.
+ CameraCommand camera_command;
};
} // namespace jevois
diff --git a/y2019/jevois/teensy.cc b/y2019/jevois/teensy.cc
index 44b2505..a3b93d2 100644
--- a/y2019/jevois/teensy.cc
+++ b/y2019/jevois/teensy.cc
@@ -577,7 +577,8 @@
// claiming that it's impossible to queue up the first byte for the slave end of
// an SPI connection properly. Instead, we just accept there will be a garbage
// byte and the other end ignores it.
-__attribute__((unused)) void TransferData() {
+__attribute__((unused)) void TransferData(
+ frc971::motors::PrintingImplementation *printing) {
Uarts *const uarts = Uarts::global_instance;
std::array<CobsPacketizer<uart_to_teensy_size()>, 5> packetizers;
std::array<TransmitBuffer, 5> transmit_buffers{
@@ -585,13 +586,18 @@
FrameQueue frame_queue;
aos::monotonic_clock::time_point last_camera_send =
aos::monotonic_clock::min_time;
+ CameraCommand stdin_camera_command = CameraCommand::kNormal;
+ CameraCommand last_roborio_camera_command = CameraCommand::kNormal;
+
bool first = true;
while (true) {
{
const auto received_transfer = SpiQueue::global_instance->Tick();
if (received_transfer) {
const auto unpacked = SpiUnpackToTeensy(*received_transfer);
- if (!unpacked) {
+ if (unpacked) {
+ last_roborio_camera_command = unpacked->camera_command;
+ } else {
printf("UART decode error\n");
}
}
@@ -639,7 +645,11 @@
CameraCalibration calibration{};
calibration.teensy_now = aos::monotonic_clock::now();
calibration.realtime_now = aos::realtime_clock::min_time;
- calibration.camera_command = CameraCalibration::CameraCommand::kNormal;
+ if (last_roborio_camera_command != CameraCommand::kNormal) {
+ calibration.camera_command = last_roborio_camera_command;
+ } else {
+ calibration.camera_command = stdin_camera_command;
+ }
// TODO(Brian): Actually fill out the calibration field.
transmit_buffers[0].MaybeWritePacket(calibration);
transmit_buffers[1].MaybeWritePacket(calibration);
@@ -652,6 +662,29 @@
}
}
+ {
+ const auto stdin_data = printing->ReadStdin();
+ if (!stdin_data.empty()) {
+ switch (stdin_data.back()) {
+ case 'p':
+ printf("Entering passthrough mode\n");
+ stdin_camera_command = CameraCommand::kCameraPassthrough;
+ break;
+ case 'u':
+ printf("Entering USB mode\n");
+ stdin_camera_command = CameraCommand::kUsb;
+ break;
+ case 'n':
+ printf("Entering normal mode\n");
+ stdin_camera_command = CameraCommand::kNormal;
+ break;
+ default:
+ printf("Unrecognized character\n");
+ break;
+ }
+ }
+ }
+
first = false;
}
}
@@ -817,7 +850,7 @@
NVIC_ENABLE_IRQ(IRQ_SPI0);
NVIC_ENABLE_IRQ(IRQ_PORTA);
- TransferData();
+ TransferData(printing.get());
while (true) {
}
diff --git a/y2019/jevois/uart_test.cc b/y2019/jevois/uart_test.cc
index 1e90d84..d669688 100644
--- a/y2019/jevois/uart_test.cc
+++ b/y2019/jevois/uart_test.cc
@@ -66,8 +66,7 @@
input_message.teensy_now =
aos::monotonic_clock::time_point(std::chrono::seconds(1678));
input_message.realtime_now = aos::realtime_clock::min_time;
- input_message.camera_command =
- CameraCalibration::CameraCommand::kCameraPassthrough;
+ input_message.camera_command = CameraCommand::kCameraPassthrough;
const UartToCameraBuffer buffer = UartPackToCamera(input_message);
const auto output_message = UartUnpackToCamera(buffer);
ASSERT_TRUE(output_message);
diff --git a/y2019/vision/BUILD b/y2019/vision/BUILD
index bdef187..7300850 100644
--- a/y2019/vision/BUILD
+++ b/y2019/vision/BUILD
@@ -90,7 +90,7 @@
cc_binary(
name = "global_calibration",
- srcs = ["global_calibration.cc"],
+ srcs = ["global_calibration.cc", "constants_formatting.cc"],
deps = [
":target_finder",
"//aos/logging",
diff --git a/y2019/vision/constants.cc b/y2019/vision/constants.cc
index bf3953c..ea47dd7 100644
--- a/y2019/vision/constants.cc
+++ b/y2019/vision/constants.cc
@@ -3,29 +3,52 @@
namespace y2019 {
namespace vision {
-constexpr double kInchesToMeters = 0.0254;
+static constexpr double kInchesToMeters = 0.0254;
CameraCalibration camera_4 = {
{
- 3.50309 / 180.0 * M_PI, 593.557, -0.0487739 / 180.0 * M_PI,
+ 3.73623 / 180.0 * M_PI, 588.1, 0.269291 / 180.0 * M_PI,
},
{
- {{5.56082 / kInchesToMeters, 4.70235 / kInchesToMeters,
- 33.4998 / kInchesToMeters}},
- 22.2155 * M_PI / 180.0,
+ {{6.02674 * kInchesToMeters, 4.57805 * kInchesToMeters,
+ 33.3849 * kInchesToMeters}},
+ 22.4535 / 180.0 * M_PI,
},
{
4,
- {{12.5 / kInchesToMeters, 12.0 / kInchesToMeters}},
- {{kInchesToMeters, 0.0}},
- 26.0,
+ {{12.5 * kInchesToMeters, 12 * kInchesToMeters}},
+ {{1 * kInchesToMeters, 0.0}},
+ 26,
"cam4_0/debug_viewer_jpeg_",
+ 52,
+ }};
+
+CameraCalibration camera_5 = {
+ {
+ 1.00774 / 180.0 * M_PI, 658.554, 2.43864 / 180.0 * M_PI,
+ },
+ {
+ {{5.51248 * kInchesToMeters, 2.04087 * kInchesToMeters,
+ 33.2555 * kInchesToMeters}},
+ -13.1396 / 180.0 * M_PI,
+ },
+ {
+ 5,
+ {{12.5 * kInchesToMeters, 0.5 * kInchesToMeters}},
+ {{1 * kInchesToMeters, 0.0}},
+ 26,
+ "cam5_0/debug_viewer_jpeg_",
+ 59,
}};
const CameraCalibration *GetCamera(int camera_id) {
switch (camera_id) {
- case 4: return &camera_4;
- default: return nullptr;
+ case 4:
+ return &camera_4;
+ case 5:
+ return &camera_5;
+ default:
+ return nullptr;
}
}
diff --git a/y2019/vision/constants.h b/y2019/vision/constants.h
index cbad8bf..b33b045 100644
--- a/y2019/vision/constants.h
+++ b/y2019/vision/constants.h
@@ -10,6 +10,7 @@
// Position of the idealized camera in 3d space.
struct CameraGeometry {
+ static constexpr size_t kNumParams = 4;
// In Meters from floor under imu center.
std::array<double, 3> location{{0, 0, 0}};
double heading = 0.0;
@@ -28,6 +29,8 @@
out.heading = data[3];
return out;
}
+
+ void dump(std::basic_ostream<char> &o) const;
};
struct IntrinsicParams {
@@ -50,6 +53,7 @@
out.barrel_mount = data[2];
return out;
}
+ void dump(std::basic_ostream<char> &o) const;
};
// Metadata about the calibration results (Should be good enough to reproduce).
@@ -62,6 +66,9 @@
// This will multiply tape_measure_direction and thus has no units.
double beginning_tape_measure_reading;
const char *filename_prefix;
+ int num_images;
+
+ void dump(std::basic_ostream<char> &o) const;
};
struct CameraCalibration {
@@ -72,6 +79,8 @@
const CameraCalibration *GetCamera(int camera_id);
+void DumpCameraConstants(int camera_id, const CameraCalibration &value);
+
} // namespace vision
} // namespace y2019
diff --git a/y2019/vision/constants_formatting.cc b/y2019/vision/constants_formatting.cc
new file mode 100644
index 0000000..74a9ed3
--- /dev/null
+++ b/y2019/vision/constants_formatting.cc
@@ -0,0 +1,98 @@
+#include "y2019/vision/constants.h"
+
+#include <fstream>
+#include <sstream>
+
+namespace y2019 {
+namespace vision {
+
+namespace {
+// 64 should be enough for any mortal.
+constexpr int kMaxNumCameras = 64;
+constexpr double kInchesToMeters = 0.0254;
+} // namespace
+
+static std::string fmt_rad(double v) {
+ std::stringstream ss;
+ if (v == 0.0) {
+ ss << "0.0";
+ } else {
+ ss << v * 180.0 / M_PI << " / 180.0 * M_PI";
+ }
+ return ss.str();
+}
+
+static std::string fmt_meters(double v) {
+ if (v == 0.0) return "0.0";
+ if (v == 1.0) return "kInchesToMeters";
+ std::stringstream ss;
+ ss << v / kInchesToMeters << " * kInchesToMeters";
+ return ss.str();
+}
+
+void IntrinsicParams::dump(std::basic_ostream<char> &o) const {
+ o << " {\n " << fmt_rad(mount_angle) << ", " << focal_length;
+ o << ", " << fmt_rad(barrel_mount) << ",\n },\n";
+}
+
+void CameraGeometry::dump(std::basic_ostream<char> &o) const {
+ o << "{{{" << fmt_meters(location[0]) << ", " << fmt_meters(location[1])
+ << ", " << fmt_meters(location[2]) << "}}," << fmt_rad(heading) << ",},";
+}
+
+void DatasetInfo::dump(std::basic_ostream<char> &o) const {
+ o << "{\n"
+ << camera_id << ", "
+ << "{{" << fmt_meters(to_tape_measure_start[0]) << ", "
+ << fmt_meters(to_tape_measure_start[1]) << "}},\n"
+ << "{{" << fmt_meters(tape_measure_direction[0]) << ", "
+ << fmt_meters(tape_measure_direction[1]) << "}},\n"
+ << beginning_tape_measure_reading << ",\n"
+ << "\"" << filename_prefix << "\",\n"
+ << num_images << ",\n}";
+}
+
+void DumpCameraConstants(int camera_id, const CameraCalibration &value) {
+ std::ofstream o("y2019/vision/constants.cc");
+ o << R"(#include "y2019/vision/constants.h"
+
+namespace y2019 {
+namespace vision {
+
+static constexpr double kInchesToMeters = 0.0254;
+)";
+
+ // Go through all the cameras and either use the existing compiled-in
+ // calibration data or the new data which was passed in.
+ for (int i = 0; i < kMaxNumCameras; ++i) {
+ auto *params = (i == camera_id) ? &value : GetCamera(i);
+ if (params) {
+ o << "\nCameraCalibration camera_" << i << " = {\n";
+ params->intrinsics.dump(o);
+ params->geometry.dump(o);
+ params->dataset.dump(o);
+ o << "};\n";
+ }
+ }
+
+ o << R"(
+const CameraCalibration *GetCamera(int camera_id) {
+ switch (camera_id) {
+)";
+ for (int i = 0; i < kMaxNumCameras; ++i) {
+ if (i == camera_id || GetCamera(i) != nullptr) {
+ o << " case " << i << ": return &camera_" << i << ";\n";
+ }
+ }
+ o << R"( default: return nullptr;
+ }
+}
+
+} // namespace vision
+} // namespace y2019
+)";
+ o.close();
+}
+
+} // namespace vision
+} // namespace y2019
diff --git a/y2019/vision/debug_serial.cc b/y2019/vision/debug_serial.cc
index a095b14..857f20f 100644
--- a/y2019/vision/debug_serial.cc
+++ b/y2019/vision/debug_serial.cc
@@ -68,10 +68,9 @@
if (n >= 1) {
CameraCalibration calibration{};
if (data[0] == 'p') {
- calibration.camera_command =
- CameraCalibration::CameraCommand::kCameraPassthrough;
+ calibration.camera_command = CameraCommand::kCameraPassthrough;
} else {
- calibration.camera_command = CameraCalibration::CameraCommand::kUsb;
+ calibration.camera_command = CameraCommand::kUsb;
}
if (write(itsDev, "\0", 1) == 1) {
const auto out_data = frc971::jevois::UartPackToCamera(calibration);
diff --git a/y2019/vision/debug_viewer.cc b/y2019/vision/debug_viewer.cc
index 4f43c9a..533dcf6 100644
--- a/y2019/vision/debug_viewer.cc
+++ b/y2019/vision/debug_viewer.cc
@@ -92,10 +92,23 @@
// Find polygons from blobs.
std::vector<std::vector<Segment<2>>> raw_polys;
for (const RangeImage &blob : imgs) {
+ // Convert blobs to contours in the corrected space.
+ ContourNode* contour = finder_.GetContour(blob);
+ if (draw_contours_) {
+ DrawContour(contour, {255, 0, 0});
+ }
+ finder_.UnWarpContour(contour);
+ if (draw_contours_) {
+ DrawContour(contour, {0, 0, 255});
+ }
+
+ // Process to polygons.
std::vector<Segment<2>> polygon =
- finder_.FillPolygon(blob, draw_raw_poly_);
+ finder_.FillPolygon(contour, draw_raw_poly_);
if (polygon.empty()) {
- DrawBlob(blob, {255, 0, 0});
+ if (!draw_contours_) {
+ DrawBlob(blob, {255, 0, 0});
+ }
} else {
raw_polys.push_back(polygon);
if (draw_select_blob_) {
@@ -168,7 +181,21 @@
} else if (key == 'b') {
draw_raw_poly_ = !draw_raw_poly_;
} else if (key == 'n') {
+ draw_contours_ = !draw_contours_;
+ } else if (key == 'm') {
draw_select_blob_ = !draw_select_blob_;
+ } else if (key == 'h') {
+ printf("Key Mappings:\n");
+ printf(" z: Toggle drawing final target pose.\n");
+ printf(" x: Toggle drawing re-projected targets and print solver results.\n");
+ printf(" c: Toggle drawing proposed target groupings.\n");
+ printf(" v: Toggle drawing ordered target components.\n");
+ printf(" b: Toggle drawing proposed target components.\n");
+ printf(" n: Toggle drawing countours before and after warping.\n");
+ printf(" m: Toggle drawing raw blob data (may need to change image to toggle a redraw).\n");
+ printf(" h: Print this message.\n");
+ printf(" a: May log camera image to /tmp/debug_viewer_jpeg_<#>.yuyv"
+ printf(" q: Exit the application.\n");
} else if (key == 'q') {
printf("User requested shutdown.\n");
exit(0);
@@ -178,6 +205,17 @@
};
}
+ void DrawContour(ContourNode *contour, PixelRef color) {
+ if (viewer_) {
+ for (ContourNode *node = contour; node->next != contour;) {
+ Vector<2> a(node->pt.x, node->pt.y);
+ Vector<2> b(node->next->pt.x, node->next->pt.y);
+ overlay_.AddLine(a, b, color);
+ node = node->next;
+ }
+ }
+ }
+
void DrawComponent(const TargetComponent &comp, PixelRef top_color,
PixelRef bot_color, PixelRef in_color,
PixelRef out_color) {
@@ -252,6 +290,7 @@
BlobList imgs_last_;
ImageFormat fmt_last_;
bool draw_select_blob_ = false;
+ bool draw_contours_ = false;
bool draw_raw_poly_ = false;
bool draw_components_ = false;
bool draw_raw_target_ = false;
diff --git a/y2019/vision/global_calibration.cc b/y2019/vision/global_calibration.cc
index e92ae56..bf64310 100644
--- a/y2019/vision/global_calibration.cc
+++ b/y2019/vision/global_calibration.cc
@@ -42,17 +42,34 @@
return new T(std::move(t));
}
-std::array<double, 3> GetError(const double *const extrinsics,
+std::array<double, 3> GetError(const DatasetInfo &info,
+ const double *const extrinsics,
const double *const geometry, int i) {
- auto ex = ExtrinsicParams::get(extrinsics);
+ auto extrinsic_params = ExtrinsicParams::get(extrinsics);
+ auto geo = CameraGeometry::get(geometry);
- double s = sin(geometry[2] + ex.r2);
- double c = cos(geometry[2] + ex.r2);
+ const double s = sin(geo.heading + extrinsic_params.r2);
+ const double c = cos(geo.heading + extrinsic_params.r2);
- // TODO: Generalize this from being just for a single calibration.
- double dx = 12.5 + 26.0 + i - (geometry[0] + c * ex.z) / kInchesToMeters;
- double dy = 12.0 - (geometry[1] + s * ex.z) / kInchesToMeters;
- double dz = 28.5 - (geometry[3] + ex.y) / kInchesToMeters;
+ // Take the tape measure starting point (this will be at the perimeter of the
+ // robot), add the offset to the first sample, and then add the per sample
+ // offset.
+ const double dx =
+ (info.to_tape_measure_start[0] +
+ (info.beginning_tape_measure_reading + i) *
+ info.tape_measure_direction[0]) /
+ kInchesToMeters -
+ (geo.location[0] + c * extrinsic_params.z) / kInchesToMeters;
+ const double dy =
+ (info.to_tape_measure_start[1] +
+ (info.beginning_tape_measure_reading + i) *
+ info.tape_measure_direction[1]) /
+ kInchesToMeters -
+ (geo.location[1] + s * extrinsic_params.z) / kInchesToMeters;
+
+ constexpr double kCalibrationTargetHeight = 28.5;
+ const double dz = kCalibrationTargetHeight -
+ (geo.location[2] + extrinsic_params.y) / kInchesToMeters;
return {{dx, dy, dz}};
}
@@ -61,6 +78,19 @@
(void)argv;
using namespace y2019::vision;
// gflags::ParseCommandLineFlags(&argc, &argv, false);
+
+ int camera_id = 5;
+ const char *base_directory = "/home/parker/data/frc/2019_calibration/";
+
+ DatasetInfo info = {
+ camera_id,
+ {{12.5 * kInchesToMeters, 0.5 * kInchesToMeters}},
+ {{kInchesToMeters, 0.0}},
+ 26,
+ "cam5_0/debug_viewer_jpeg_",
+ 59,
+ };
+
::aos::logging::Init();
::aos::logging::AddImplementation(
new ::aos::logging::StreamLogImplementation(stderr));
@@ -77,9 +107,8 @@
double intrinsics[IntrinsicParams::kNumParams];
IntrinsicParams().set(&intrinsics[0]);
- // To know the meaning, see the printout below...
- constexpr size_t GeometrykNumParams = 4;
- double geometry[GeometrykNumParams] = {0, 0, 0, 0};
+ double geometry[CameraGeometry::kNumParams];
+ CameraGeometry().set(&geometry[0]);
Solver::Options options;
options.minimizer_progress_to_stdout = false;
@@ -91,11 +120,10 @@
std::cout << "fl = " << intrinsics_.focal_length << ";\n";
std::cout << "error = " << summary.final_cost << ";\n";
- int nimgs = 56;
- for (int i = 0; i < nimgs; ++i) {
- auto frame = aos::vision::LoadFile(
- "/home/parker/data/frc/2019_calibration/cam4_0/debug_viewer_jpeg_" +
- std::to_string(i) + ".yuyv");
+ for (int i = 0; i < info.num_images; ++i) {
+ auto frame = aos::vision::LoadFile(std::string(base_directory) +
+ info.filename_prefix +
+ std::to_string(i) + ".yuyv");
aos::vision::ImageFormat fmt{640, 480};
aos::vision::BlobList imgs = aos::vision::FindBlobs(
@@ -105,7 +133,10 @@
bool verbose = false;
std::vector<std::vector<Segment<2>>> raw_polys;
for (const RangeImage &blob : imgs) {
- std::vector<Segment<2>> polygon = finder_.FillPolygon(blob, verbose);
+ // Convert blobs to contours in the corrected space.
+ ContourNode* contour = finder_.GetContour(blob);
+ finder_.UnWarpContour(contour);
+ std::vector<Segment<2>> polygon = finder_.FillPolygon(contour, verbose);
if (polygon.empty()) {
} else {
raw_polys.push_back(polygon);
@@ -138,8 +169,8 @@
const double *const extrinsics,
double *residual) {
auto in = IntrinsicParams::get(intrinsics);
- auto ex = ExtrinsicParams::get(extrinsics);
- auto pt = targ - Project(temp, in, ex);
+ auto extrinsic_params = ExtrinsicParams::get(extrinsics);
+ auto pt = targ - Project(temp, in, extrinsic_params);
residual[0] = pt.x();
residual[1] = pt.y();
return true;
@@ -152,9 +183,9 @@
NULL, &intrinsics[0], extrinsics);
}
- auto ftor = [i](const double *const extrinsics,
- const double *const geometry, double *residual) {
- auto err = GetError(extrinsics, geometry, i);
+ auto ftor = [&info, i](const double *const extrinsics,
+ const double *const geometry, double *residual) {
+ auto err = GetError(info, extrinsics, geometry, i);
residual[0] = 32.0 * err[0];
residual[1] = 32.0 * err[1];
residual[2] = 32.0 * err[2];
@@ -164,7 +195,7 @@
problem.AddResidualBlock(
new NumericDiffCostFunction<decltype(ftor), CENTRAL, 3,
ExtrinsicParams::kNumParams,
- GeometrykNumParams>(
+ CameraGeometry::kNumParams>(
new decltype(ftor)(std::move(ftor))),
NULL, extrinsics, &geometry[0]);
}
@@ -177,14 +208,18 @@
{
std::cout << summary.BriefReport() << "\n";
auto intrinsics_ = IntrinsicParams::get(&intrinsics[0]);
+ auto geometry_ = CameraGeometry::get(&geometry[0]);
std::cout << "rup = " << intrinsics_.mount_angle * 180 / M_PI << ";\n";
std::cout << "fl = " << intrinsics_.focal_length << ";\n";
std::cout << "error = " << summary.final_cost << ";\n";
- std::cout << "camera_height = " << geometry[3] / kInchesToMeters << "\n";
- std::cout << "camera_angle = " << geometry[2] * 180 / M_PI << "\n";
- std::cout << "camera_x = " << geometry[0] / kInchesToMeters << "\n";
- std::cout << "camera_y = " << geometry[1] / kInchesToMeters << "\n";
+ std::cout << "camera_angle = " << geometry_.heading * 180 / M_PI << "\n";
+ std::cout << "camera_x = " << geometry_.location[0] / kInchesToMeters
+ << "\n";
+ std::cout << "camera_y = " << geometry_.location[1] / kInchesToMeters
+ << "\n";
+ std::cout << "camera_z = " << geometry_.location[2] / kInchesToMeters
+ << "\n";
std::cout << "camera_barrel = " << intrinsics_.barrel_mount * 180.0 / M_PI
<< "\n";
@@ -194,7 +229,7 @@
auto extn = ExtrinsicParams::get(data);
- auto err = GetError(data, &geometry[0], i);
+ auto err = GetError(info, data, &geometry[0], i);
std::cout << i << ", ";
std::cout << extn.z / kInchesToMeters << ", ";
@@ -208,6 +243,12 @@
std::cout << err[2] << "\n";
}
}
+
+ CameraCalibration results;
+ results.dataset = info;
+ results.intrinsics = IntrinsicParams::get(&intrinsics[0]);
+ results.geometry = CameraGeometry::get(&geometry[0]);
+ DumpCameraConstants(camera_id, results);
}
} // namespace y2019
diff --git a/y2019/vision/target_finder.cc b/y2019/vision/target_finder.cc
index f69e987..b46d802 100644
--- a/y2019/vision/target_finder.cc
+++ b/y2019/vision/target_finder.cc
@@ -33,39 +33,96 @@
imgs->end());
}
+ContourNode* TargetFinder::GetContour(const RangeImage &blob) {
+ alloc_.reset();
+ return RangeImgToContour(blob, &alloc_);
+}
+
+// TODO(ben): These values will be moved into the constants.h file.
+namespace {
+
+constexpr double f_x = 481.4957;
+constexpr double c_x = 341.215;
+constexpr double f_y = 484.314;
+constexpr double c_y = 251.29;
+
+constexpr double f_x_prime = 363.1424;
+constexpr double c_x_prime = 337.9895;
+constexpr double f_y_prime = 366.4837;
+constexpr double c_y_prime = 240.0702;
+
+constexpr double k_1 = -0.2739;
+constexpr double k_2 = 0.01583;
+constexpr double k_3 = 0.04201;
+
+constexpr int iterations = 7;
+
+}
+
+Point UnWarpPoint(const Point &point, int iterations) {
+ const double x0 = ((double)point.x - c_x) / f_x;
+ const double y0 = ((double)point.y - c_y) / f_y;
+ double x = x0;
+ double y = y0;
+ for (int i = 0; i < iterations; i++) {
+ const double r_sqr = x * x + y * y;
+ const double coeff =
+ 1.0 + r_sqr * (k_1 + k_2 * r_sqr * (1.0 + k_3 * r_sqr));
+ x = x0 / coeff;
+ y = y0 / coeff;
+ }
+ double nx = x * f_x_prime + c_x_prime;
+ double ny = y * f_y_prime + c_y_prime;
+ Point p = {static_cast<int>(nx), static_cast<int>(ny)};
+ return p;
+}
+
+void TargetFinder::UnWarpContour(ContourNode *start) const {
+ ContourNode *node = start;
+ while (node->next != start) {
+ node->set_point(UnWarpPoint(node->pt, iterations));
+ node = node->next;
+ }
+ node->set_point(UnWarpPoint(node->pt, iterations));
+}
+
// TODO: Try hierarchical merge for this.
// Convert blobs into polygons.
std::vector<aos::vision::Segment<2>> TargetFinder::FillPolygon(
- const RangeImage &blob, bool verbose) {
+ ContourNode* start, bool verbose) {
if (verbose) printf("Process Polygon.\n");
- alloc_.reset();
- auto *st = RangeImgToContour(blob, &alloc_);
struct Pt {
float x;
float y;
};
- std::vector<Pt> pts;
+ std::vector<Pt> points;
// Collect all slopes from the contour.
- auto opt = st->pt;
- for (auto *node = st; node->next != st;) {
+ Point previous_point = start->pt;
+ for (ContourNode *node = start; node->next != start;) {
node = node->next;
- auto npt = node->pt;
+ Point current_point = node->pt;
- pts.push_back(
- {static_cast<float>(npt.x - opt.x), static_cast<float>(npt.y - opt.y)});
+ points.push_back({static_cast<float>(current_point.x - previous_point.x),
+ static_cast<float>(current_point.y - previous_point.y)});
- opt = npt;
+ previous_point = current_point;
}
- const int n = pts.size();
- auto get_pt = [&](int i) { return pts[(i + n * 2) % n]; };
+ const int num_points = points.size();
+ auto get_pt = [&points, num_points](int i) {
+ return points[(i + num_points * 2) % num_points];
+ };
- std::vector<Pt> pts_new = pts;
- auto run_box_filter = [&](int window_size) {
- for (size_t i = 0; i < pts.size(); ++i) {
+ std::vector<Pt> filtered_points = points;
+ // Three box filter makith a guassian?
+ // Run gaussian filter over the slopes 3 times. That'll get us pretty close
+ // to running a gausian over it.
+ for (int k = 0; k < 3; ++k) {
+ const int window_size = 2;
+ for (size_t i = 0; i < points.size(); ++i) {
Pt a{0.0, 0.0};
for (int j = -window_size; j <= window_size; ++j) {
Pt p = get_pt(j + i);
@@ -75,25 +132,20 @@
a.x /= (window_size * 2 + 1);
a.y /= (window_size * 2 + 1);
- float scale = 1.0 + (i / float(pts.size() * 10));
+ const float scale = 1.0 + (i / float(points.size() * 10));
a.x *= scale;
a.y *= scale;
- pts_new[i] = a;
+ filtered_points[i] = a;
}
- pts = pts_new;
- };
- // Three box filter makith a guassian?
- // Run gaussian filter over the slopes.
- run_box_filter(2);
- run_box_filter(2);
- run_box_filter(2);
+ points = filtered_points;
+ }
// Heuristic which says if a particular slope is part of a corner.
auto is_corner = [&](size_t i) {
- Pt a = get_pt(i - 3);
- Pt b = get_pt(i + 3);
- double dx = (a.x - b.x);
- double dy = (a.y - b.y);
+ const Pt a = get_pt(i - 3);
+ const Pt b = get_pt(i + 3);
+ const double dx = (a.x - b.x);
+ const double dy = (a.y - b.y);
return dx * dx + dy * dy > 0.25;
};
@@ -102,11 +154,11 @@
// Find all centers of corners.
// Because they round, multiple points may be a corner.
std::vector<size_t> edges;
- size_t kBad = pts.size() + 10;
+ size_t kBad = points.size() + 10;
size_t prev_up = kBad;
size_t wrapped_n = prev_up;
- for (size_t i = 0; i < pts.size(); ++i) {
+ for (size_t i = 0; i < points.size(); ++i) {
bool v = is_corner(i);
if (prev_v && !v) {
if (prev_up == kBad) {
@@ -122,7 +174,7 @@
}
if (wrapped_n != kBad) {
- edges.push_back(((prev_up + pts.size() + wrapped_n - 1) / 2) % pts.size());
+ edges.push_back(((prev_up + points.size() + wrapped_n - 1) / 2) % points.size());
}
if (verbose) printf("Edge Count (%zu).\n", edges.size());
@@ -133,7 +185,7 @@
{
std::vector<ContourNode *> segments_all;
- for (ContourNode *node = st; node->next != st;) {
+ for (ContourNode *node = start; node->next != start;) {
node = node->next;
segments_all.push_back(node);
}
@@ -147,12 +199,13 @@
std::vector<Segment<2>> seg_list;
if (segments.size() == 4) {
for (size_t i = 0; i < segments.size(); ++i) {
- auto *ed = segments[(i + 1) % segments.size()];
- auto *st = segments[i];
+ ContourNode *segment_end = segments[(i + 1) % segments.size()];
+ ContourNode *segment_start = segments[i];
float mx = 0.0;
float my = 0.0;
int n = 0;
- for (auto *node = st; node != ed; node = node->next) {
+ for (ContourNode *node = segment_start; node != segment_end;
+ node = node->next) {
mx += node->pt.x;
my += node->pt.y;
++n;
@@ -164,26 +217,27 @@
float xx = 0.0;
float xy = 0.0;
float yy = 0.0;
- for (auto *node = st; node != ed; node = node->next) {
- float x = node->pt.x - mx;
- float y = node->pt.y - my;
+ for (ContourNode *node = segment_start; node != segment_end;
+ node = node->next) {
+ const float x = node->pt.x - mx;
+ const float y = node->pt.y - my;
xx += x * x;
xy += x * y;
yy += y * y;
}
// TODO: Extract common to hierarchical merge.
- float neg_b_over_2 = (xx + yy) / 2.0;
- float c = (xx * yy - xy * xy);
+ const float neg_b_over_2 = (xx + yy) / 2.0;
+ const float c = (xx * yy - xy * xy);
- float sqr = sqrt(neg_b_over_2 * neg_b_over_2 - c);
+ const float sqr = sqrt(neg_b_over_2 * neg_b_over_2 - c);
{
- float lam = neg_b_over_2 + sqr;
+ const float lam = neg_b_over_2 + sqr;
float x = xy;
float y = lam - xx;
- float norm = sqrt(x * x + y * y);
+ const float norm = hypot(x, y);
x /= norm;
y /= norm;
@@ -212,7 +266,7 @@
const std::vector<std::vector<Segment<2>>> &seg_list) {
std::vector<TargetComponent> list;
TargetComponent new_target;
- for (const auto &poly : seg_list) {
+ for (const std::vector<Segment<2>> &poly : seg_list) {
// Reject missized pollygons for now. Maybe rectify them here in the future;
if (poly.size() != 4) continue;
std::vector<Vector<2>> corners;
diff --git a/y2019/vision/target_finder.h b/y2019/vision/target_finder.h
index 633733a..bdf67f2 100644
--- a/y2019/vision/target_finder.h
+++ b/y2019/vision/target_finder.h
@@ -4,6 +4,7 @@
#include "aos/vision/blob/region_alloc.h"
#include "aos/vision/blob/threshold.h"
#include "aos/vision/blob/transpose.h"
+#include "aos/vision/blob/contour.h"
#include "aos/vision/debug/overlay.h"
#include "aos/vision/math/vector.h"
#include "y2019/vision/target_types.h"
@@ -15,6 +16,7 @@
using aos::vision::RangeImage;
using aos::vision::BlobList;
using aos::vision::Vector;
+using aos::vision::ContourNode;
class TargetFinder {
public:
@@ -28,8 +30,11 @@
// filter out obvious or durranged blobs.
void PreFilter(BlobList *imgs);
+ ContourNode* GetContour(const RangeImage &blob);
+ void UnWarpContour(ContourNode* start) const;
+
// Turn a blob into a polgygon.
- std::vector<aos::vision::Segment<2>> FillPolygon(const RangeImage &blob,
+ std::vector<aos::vision::Segment<2>> FillPolygon(ContourNode *start,
bool verbose);
// Turn a bloblist into components of a target.
@@ -57,6 +62,7 @@
const Target &GetTemplateTarget() { return target_template_; }
const IntrinsicParams &intrinsics() const { return intrinsics_; }
+ IntrinsicParams *mutable_intrinsics() { return &intrinsics_; }
private:
// Find a loosly connected target.
diff --git a/y2019/vision/target_sender.cc b/y2019/vision/target_sender.cc
index be56e91..2c7d8e2 100644
--- a/y2019/vision/target_sender.cc
+++ b/y2019/vision/target_sender.cc
@@ -275,7 +275,7 @@
(void)argc;
(void)argv;
using namespace y2019::vision;
- using frc971::jevois::CameraCalibration;
+ using frc971::jevois::CameraCommand;
// gflags::ParseCommandLineFlags(&argc, &argv, false);
::aos::logging::Init();
::aos::logging::AddImplementation(
@@ -309,9 +309,11 @@
bool verbose = false;
std::vector<std::vector<Segment<2>>> raw_polys;
for (const RangeImage &blob : imgs) {
- std::vector<Segment<2>> polygon = finder_.FillPolygon(blob, verbose);
- if (polygon.empty()) {
- } else {
+ // Convert blobs to contours in the corrected space.
+ ContourNode* contour = finder_.GetContour(blob);
+ finder_.UnWarpContour(contour);
+ std::vector<Segment<2>> polygon = finder_.FillPolygon(contour, verbose);
+ if (!polygon.empty()) {
raw_polys.push_back(polygon);
}
}
@@ -381,12 +383,17 @@
frc971::jevois::UartUnpackToCamera(packet);
if (calibration_question) {
const auto &calibration = *calibration_question;
+ 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);
+
switch (calibration.camera_command) {
- case CameraCalibration::CameraCommand::kNormal:
+ case CameraCommand::kNormal:
break;
- case CameraCalibration::CameraCommand::kUsb:
+ case CameraCommand::kUsb:
return 0;
- case CameraCalibration::CameraCommand::kCameraPassthrough:
+ case CameraCommand::kCameraPassthrough:
return system("touch /tmp/do_not_export_sd_card");
}
} else {
diff --git a/y2019/wpilib_interface.cc b/y2019/wpilib_interface.cc
index 29377c8..02269eb 100644
--- a/y2019/wpilib_interface.cc
+++ b/y2019/wpilib_interface.cc
@@ -279,10 +279,26 @@
spi_->SetMSBFirst();
}
+ void set_activate_usb(std::unique_ptr<frc::DigitalInput> activate_usb) {
+ activate_usb_ = std::move(activate_usb);
+ }
+
+ void set_activate_passthrough(
+ std::unique_ptr<frc::DigitalInput> activate_passthrough) {
+ activate_passthrough_ = std::move(activate_passthrough);
+ }
+
void DoSpiTransaction() {
using namespace frc971::jevois;
RoborioToTeensy to_teensy{};
to_teensy.realtime_now = aos::realtime_clock::now();
+ 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 {
+ to_teensy.camera_command = CameraCommand::kNormal;
+ }
std::array<char, spi_transfer_size() + 1> to_send{};
{
@@ -343,6 +359,9 @@
frc::SPI *spi_ = nullptr;
::std::unique_ptr<frc::SPI> dummy_spi_;
+ std::unique_ptr<frc::DigitalInput> activate_usb_;
+ std::unique_ptr<frc::DigitalInput> activate_passthrough_;
+
frc971::wpilib::SpiRxClearer rx_clearer_;
};
@@ -553,6 +572,9 @@
frc::SPI camera_spi(frc::SPI::Port::kOnboardCS3);
camera_reader.set_spi(&camera_spi);
camera_reader.SetDummySPI(frc::SPI::Port::kOnboardCS2);
+ // Austin says 8, 9, 24, and 25 are good options to choose from for these.
+ camera_reader.set_activate_usb(make_unique<frc::DigitalInput>(24));
+ camera_reader.set_activate_passthrough(make_unique<frc::DigitalInput>(25));
auto imu_trigger = make_unique<frc::DigitalInput>(0);
::frc971::wpilib::ADIS16448 imu(frc::SPI::Port::kOnboardCS1,