Running global_calibration updates constants.cc.
Change-Id: I1663987d6906f7899a90a95d694bd1db825afeef
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