Merge changes Ie4081c1e,I254408f3,I99d5113e
* changes:
Make the Teensy actually send useful camera commands
Add support for active-low buttons to switch camera modes
Send a CameraCommand from the roboRIO too
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/y2019/jevois/structures.h b/y2019/jevois/structures.h
index aa4fc07..1f10dc0 100644
--- a/y2019/jevois/structures.h
+++ b/y2019/jevois/structures.h
@@ -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
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/global_calibration.cc b/y2019/vision/global_calibration.cc
index e92ae56..d5f0d4e 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(
@@ -138,8 +166,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 +180,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 +192,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 +205,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 +226,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 +240,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..6b90080 100644
--- a/y2019/vision/target_finder.cc
+++ b/y2019/vision/target_finder.cc
@@ -39,33 +39,39 @@
const RangeImage &blob, bool verbose) {
if (verbose) printf("Process Polygon.\n");
alloc_.reset();
- auto *st = RangeImgToContour(blob, &alloc_);
+ ContourNode *start = 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 +81,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 +103,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 +123,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 +134,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 +148,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 +166,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 +215,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..5957fa6 100644
--- a/y2019/vision/target_finder.h
+++ b/y2019/vision/target_finder.h
@@ -57,6 +57,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 d6c5dc2..ad42ce4 100644
--- a/y2019/vision/target_sender.cc
+++ b/y2019/vision/target_sender.cc
@@ -381,6 +381,11 @@
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 CameraCommand::kNormal:
break;