Adding global_calibration for finding the relative position of the camera inside the robot.
Change-Id: I98a9fb4544f17969c47b519114fb8ecf8e412329
diff --git a/aos/vision/image/image_dataset.cc b/aos/vision/image/image_dataset.cc
index cd3ec46..d4dde95 100644
--- a/aos/vision/image/image_dataset.cc
+++ b/aos/vision/image/image_dataset.cc
@@ -45,6 +45,15 @@
}
} // namespace
+DatasetFrame LoadFile(const std::string &jpeg_filename) {
+ bool is_jpeg = true;
+ size_t l = jpeg_filename.size();
+ if (l > 4 && jpeg_filename[l - 1] == 'v') {
+ is_jpeg = false;
+ }
+ return DatasetFrame{is_jpeg, GetFileContents(jpeg_filename)};
+}
+
std::vector<DatasetFrame> LoadDataset(const std::string &jpeg_list_filename) {
std::vector<DatasetFrame> images;
auto contents = GetFileContents(jpeg_list_filename);
@@ -62,17 +71,10 @@
if (jpeg_filename[i] == '#') return;
if (jpeg_filename[i] != ' ') break;
}
- bool is_jpeg = true;
- size_t l = jpeg_filename.size();
- if (l > 4 && jpeg_filename[l - 1] == 'v') {
- is_jpeg = false;
- }
if (jpeg_filename[0] == '/') {
- images.emplace_back(
- DatasetFrame{is_jpeg, GetFileContents(jpeg_filename)});
+ images.emplace_back(LoadFile(jpeg_filename));
} else {
- images.emplace_back(
- DatasetFrame{is_jpeg, GetFileContents(basename + jpeg_filename)});
+ images.emplace_back(LoadFile(basename + jpeg_filename));
}
}();
}
diff --git a/aos/vision/image/image_dataset.h b/aos/vision/image/image_dataset.h
index ce8c841..7801cec 100644
--- a/aos/vision/image/image_dataset.h
+++ b/aos/vision/image/image_dataset.h
@@ -15,6 +15,8 @@
std::vector<DatasetFrame> LoadDataset(const std::string &jpeg_list_filename);
+DatasetFrame LoadFile(const std::string &jpeg_filename);
+
} // namespace vision
} // namespace aos
diff --git a/y2019/vision/BUILD b/y2019/vision/BUILD
index e620ffa..c2f9da5 100644
--- a/y2019/vision/BUILD
+++ b/y2019/vision/BUILD
@@ -81,10 +81,9 @@
],
)
-"""
cc_binary(
- name = "calibration",
- srcs = ["calibration.cc"],
+ name = "global_calibration",
+ srcs = ["global_calibration.cc"],
deps = [
":target_finder",
"//aos/logging",
@@ -95,9 +94,9 @@
"//aos/vision/events:socket_types",
"//aos/vision/events:udp",
"//aos/vision/image:image_stream",
+ "//aos/vision/image:image_dataset",
"//aos/vision/image:reader",
"@com_google_ceres_solver//:ceres",
],
restricted_to = VISION_TARGETS,
)
-"""
diff --git a/y2019/vision/global_calibration.cc b/y2019/vision/global_calibration.cc
new file mode 100644
index 0000000..e92ae56
--- /dev/null
+++ b/y2019/vision/global_calibration.cc
@@ -0,0 +1,216 @@
+#include <fstream>
+
+#include "aos/logging/implementations.h"
+#include "aos/logging/logging.h"
+#include "aos/vision/blob/codec.h"
+#include "aos/vision/blob/find_blob.h"
+#include "aos/vision/events/socket_types.h"
+#include "aos/vision/events/udp.h"
+#include "aos/vision/image/image_dataset.h"
+#include "aos/vision/image/image_stream.h"
+#include "aos/vision/image/reader.h"
+#include "y2019/vision/target_finder.h"
+
+#undef CHECK_NOTNULL
+#undef CHECK_OP
+#undef PCHECK
+// CERES Clashes with logging symbols...
+#include "ceres/ceres.h"
+
+using ::aos::events::DataSocket;
+using ::aos::events::RXUdpSocket;
+using ::aos::events::TCPServer;
+using ::aos::vision::DataRef;
+using ::aos::vision::Int32Codec;
+using ::aos::monotonic_clock;
+using aos::vision::Segment;
+
+using ceres::NumericDiffCostFunction;
+using ceres::CENTRAL;
+using ceres::CostFunction;
+using ceres::Problem;
+using ceres::Solver;
+using ceres::Solve;
+
+namespace y2019 {
+namespace vision {
+
+constexpr double kInchesToMeters = 0.0254;
+
+template <size_t k, size_t n, size_t n2, typename T>
+T *MakeFunctor(T &&t) {
+ return new T(std::move(t));
+}
+
+std::array<double, 3> GetError(const double *const extrinsics,
+ const double *const geometry, int i) {
+ auto ex = ExtrinsicParams::get(extrinsics);
+
+ double s = sin(geometry[2] + ex.r2);
+ double c = cos(geometry[2] + ex.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;
+ return {{dx, dy, dz}};
+}
+
+void main(int argc, char **argv) {
+ (void)argc;
+ (void)argv;
+ using namespace y2019::vision;
+ // gflags::ParseCommandLineFlags(&argc, &argv, false);
+ ::aos::logging::Init();
+ ::aos::logging::AddImplementation(
+ new ::aos::logging::StreamLogImplementation(stderr));
+
+ TargetFinder finder_;
+
+ ceres::Problem problem;
+
+ struct Sample {
+ std::unique_ptr<double[]> extrinsics;
+ int i;
+ };
+ std::vector<Sample> all_extrinsics;
+ 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};
+
+ Solver::Options options;
+ options.minimizer_progress_to_stdout = false;
+ Solver::Summary summary;
+
+ std::cout << summary.BriefReport() << "\n";
+ auto intrinsics_ = IntrinsicParams::get(&intrinsics[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";
+
+ 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");
+
+ aos::vision::ImageFormat fmt{640, 480};
+ aos::vision::BlobList imgs = aos::vision::FindBlobs(
+ aos::vision::DoThresholdYUYV(fmt, frame.data.data(), 120));
+ finder_.PreFilter(&imgs);
+
+ 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 {
+ raw_polys.push_back(polygon);
+ }
+ }
+
+ // Calculate each component side of a possible target.
+ std::vector<TargetComponent> target_component_list =
+ finder_.FillTargetComponentList(raw_polys);
+
+ // Put the compenents together into targets.
+ std::vector<Target> target_list =
+ finder_.FindTargetsFromComponents(target_component_list, verbose);
+
+ // Use the solver to generate an intermediate version of our results.
+ std::vector<IntermediateResult> results;
+ for (const Target &target : target_list) {
+ auto target_value = target.toPointList();
+ auto template_value = finder_.GetTemplateTarget().toPointList();
+
+ auto *extrinsics = new double[ExtrinsicParams::kNumParams];
+ ExtrinsicParams().set(extrinsics);
+ all_extrinsics.push_back({std::unique_ptr<double[]>(extrinsics), i});
+
+ for (size_t j = 0; j < 8; ++j) {
+ auto temp = template_value[j];
+ auto targ = target_value[j];
+
+ auto ftor = [temp, targ, i](const double *const intrinsics,
+ const double *const extrinsics,
+ double *residual) {
+ auto in = IntrinsicParams::get(intrinsics);
+ auto ex = ExtrinsicParams::get(extrinsics);
+ auto pt = targ - Project(temp, in, ex);
+ residual[0] = pt.x();
+ residual[1] = pt.y();
+ return true;
+ };
+ problem.AddResidualBlock(
+ new NumericDiffCostFunction<decltype(ftor), CENTRAL, 2,
+ IntrinsicParams::kNumParams,
+ ExtrinsicParams::kNumParams>(
+ new decltype(ftor)(std::move(ftor))),
+ NULL, &intrinsics[0], extrinsics);
+ }
+
+ auto ftor = [i](const double *const extrinsics,
+ const double *const geometry, double *residual) {
+ auto err = GetError(extrinsics, geometry, i);
+ residual[0] = 32.0 * err[0];
+ residual[1] = 32.0 * err[1];
+ residual[2] = 32.0 * err[2];
+ return true;
+ };
+
+ problem.AddResidualBlock(
+ new NumericDiffCostFunction<decltype(ftor), CENTRAL, 3,
+ ExtrinsicParams::kNumParams,
+ GeometrykNumParams>(
+ new decltype(ftor)(std::move(ftor))),
+ NULL, extrinsics, &geometry[0]);
+ }
+ }
+ // TODO: Debug solver convergence?
+ Solve(options, &problem, &summary);
+ Solve(options, &problem, &summary);
+ Solve(options, &problem, &summary);
+
+ {
+ std::cout << summary.BriefReport() << "\n";
+ auto intrinsics_ = IntrinsicParams::get(&intrinsics[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_barrel = " << intrinsics_.barrel_mount * 180.0 / M_PI
+ << "\n";
+
+ for (auto &sample : all_extrinsics) {
+ int i = sample.i;
+ double *data = sample.extrinsics.get();
+
+ auto extn = ExtrinsicParams::get(data);
+
+ auto err = GetError(data, &geometry[0], i);
+
+ std::cout << i << ", ";
+ std::cout << extn.z / kInchesToMeters << ", ";
+ std::cout << extn.y / kInchesToMeters << ", ";
+ std::cout << extn.r1 * 180 / M_PI << ", ";
+ std::cout << extn.r2 * 180 / M_PI << ", ";
+ // TODO: Methodology problem: This should really have a separate solve for
+ // extrinsics...
+ std::cout << err[0] << ", ";
+ std::cout << err[1] << ", ";
+ std::cout << err[2] << "\n";
+ }
+ }
+}
+
+} // namespace y2019
+} // namespace vision
+
+int main(int argc, char **argv) { y2019::vision::main(argc, argv); }
diff --git a/y2019/vision/target_geometry.cc b/y2019/vision/target_geometry.cc
index 77adc83..1c1fb99 100644
--- a/y2019/vision/target_geometry.cc
+++ b/y2019/vision/target_geometry.cc
@@ -58,6 +58,7 @@
double r1 = extrinsics.r1;
double r2 = extrinsics.r2;
double rup = intrinsics.mount_angle;
+ double rbarrel = intrinsics.barrel_mount;
double fl = intrinsics.focal_length;
::Eigen::Matrix<double, 1, 3> pts{pt.x(), pt.y() + y, 0.0};
@@ -93,6 +94,11 @@
pts.transpose();
}
+ // 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.
::Eigen::Matrix<double, 1, 3> res = pts;
diff --git a/y2019/vision/target_types.h b/y2019/vision/target_types.h
index 557b04a..089b897 100644
--- a/y2019/vision/target_types.h
+++ b/y2019/vision/target_types.h
@@ -43,19 +43,23 @@
};
struct IntrinsicParams {
- static constexpr size_t kNumParams = 2;
+ static constexpr size_t kNumParams = 3;
- double mount_angle = 10.0481 / 180.0 * M_PI;
- double focal_length = 729.445;
+ double mount_angle = 0.819433 / 180.0 * M_PI; // 9.32615 / 180.0 * M_PI;
+ double focal_length = 666.763; // 734.328;
+ // This is a final rotation where the camera isn't straight.
+ double barrel_mount = 2.72086 / 180.0 * M_PI;
void set(double *data) {
data[0] = mount_angle;
data[1] = focal_length;
+ data[2] = barrel_mount;
}
static IntrinsicParams get(const double *data) {
IntrinsicParams out;
out.mount_angle = data[0];
out.focal_length = data[1];
+ out.barrel_mount = data[2];
return out;
}
};