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