constify and deautoify more things in vision
This was part of my effort to understand how things work.
Change-Id: I49dae7400fba8ec1f02d006f3d14c6dc254a8187
diff --git a/y2019/vision/constants.h b/y2019/vision/constants.h
index 1e0380b..a56e9b3 100644
--- a/y2019/vision/constants.h
+++ b/y2019/vision/constants.h
@@ -60,9 +60,9 @@
struct DatasetInfo {
int camera_id;
// In meters from IMU start.
- std::array<double, 2> to_tape_measure_start;
+ ::std::array<double, 2> to_tape_measure_start;
// In meters,
- std::array<double, 2> tape_measure_direction;
+ ::std::array<double, 2> tape_measure_direction;
// This will multiply tape_measure_direction and thus has no units.
double beginning_tape_measure_reading;
const char *filename_prefix;
@@ -85,7 +85,7 @@
// Get the IDs of the cameras in each port for a particular teensy board.
// inlined so that we don't have to deal with including it in the autogenerated
// constants.cc.
-inline std::array<int, 5> CameraSerialNumbers(uint32_t processor_id) {
+inline ::std::array<int, 5> CameraSerialNumbers(uint32_t processor_id) {
switch (processor_id) {
case CodeBotTeensyId():
return {{0, 0, 0, 16, 19}};
diff --git a/y2019/vision/global_calibration.cc b/y2019/vision/global_calibration.cc
index c962fc9..b72752a 100644
--- a/y2019/vision/global_calibration.cc
+++ b/y2019/vision/global_calibration.cc
@@ -36,19 +36,17 @@
namespace y2019 {
namespace vision {
+namespace {
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));
-}
+} // namespace
-std::array<double, 3> GetError(const DatasetInfo &info,
- const double *const extrinsics,
- const double *const geometry, int i) {
- auto extrinsic_params = ExtrinsicParams::get(extrinsics);
- auto geo = CameraGeometry::get(geometry);
+::std::array<double, 3> GetError(const DatasetInfo &info,
+ const double *const extrinsics,
+ const double *const geometry, int i) {
+ const ExtrinsicParams extrinsic_params = ExtrinsicParams::get(extrinsics);
+ const CameraGeometry geo = CameraGeometry::get(geometry);
const double s = sin(geo.heading + extrinsic_params.r2);
const double c = cos(geo.heading + extrinsic_params.r2);
@@ -94,15 +92,15 @@
::aos::logging::AddImplementation(
new ::aos::logging::StreamLogImplementation(stderr));
- TargetFinder finder_;
+ TargetFinder target_finder;
ceres::Problem problem;
struct Sample {
- std::unique_ptr<double[]> extrinsics;
+ ::std::unique_ptr<double[]> extrinsics;
int i;
};
- std::vector<Sample> all_extrinsics;
+ ::std::vector<Sample> all_extrinsics;
double intrinsics[IntrinsicParams::kNumParams];
IntrinsicParams().set(&intrinsics[0]);
@@ -113,51 +111,51 @@
options.minimizer_progress_to_stdout = false;
Solver::Summary summary;
- std::cout << summary.BriefReport() << "\n";
+ ::std::cout << summary.BriefReport() << "\n";
IntrinsicParams 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 << "rup = " << intrinsics_.mount_angle * 180 / M_PI << ";\n";
+ ::std::cout << "fl = " << intrinsics_.focal_length << ";\n";
+ ::std::cout << "error = " << summary.final_cost << ";\n";
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(aos::vision::DoThresholdYUYV(
+ const ::aos::vision::ImageFormat fmt{640, 480};
+ ::aos::vision::BlobList imgs =
+ ::aos::vision::FindBlobs(aos::vision::DoThresholdYUYV(
fmt, frame.data.data(), TargetFinder::GetThresholdValue()));
- finder_.PreFilter(&imgs);
+ target_finder.PreFilter(&imgs);
- bool verbose = false;
- std::vector<std::vector<Segment<2>>> raw_polys;
+ constexpr bool verbose = false;
+ ::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);
- finder_.UnWarpContour(contour);
- std::vector<Segment<2>> polygon = finder_.FillPolygon(contour, verbose);
- if (polygon.empty()) {
- } else {
+ ContourNode *contour = target_finder.GetContour(blob);
+ target_finder.UnWarpContour(contour);
+ const ::std::vector<Segment<2>> polygon =
+ target_finder.FillPolygon(contour, verbose);
+ if (!polygon.empty()) {
raw_polys.push_back(polygon);
}
}
// Calculate each component side of a possible target.
- std::vector<TargetComponent> target_component_list =
- finder_.FillTargetComponentList(raw_polys);
+ const ::std::vector<TargetComponent> target_component_list =
+ target_finder.FillTargetComponentList(raw_polys);
// Put the compenents together into targets.
- std::vector<Target> target_list =
- finder_.FindTargetsFromComponents(target_component_list, verbose);
+ const ::std::vector<Target> target_list =
+ target_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) {
- ::std::array<aos::vision::Vector<2>, 8> target_value =
+ const ::std::array<::aos::vision::Vector<2>, 8> target_value =
target.ToPointList();
- ::std::array<aos::vision::Vector<2>, 8> template_value =
- finder_.GetTemplateTarget().ToPointList();
+ const ::std::array<::aos::vision::Vector<2>, 8> template_value =
+ target_finder.GetTemplateTarget().ToPointList();
// TODO(austin): Memory leak below, fix.
double *extrinsics = new double[ExtrinsicParams::kNumParams];
@@ -165,24 +163,33 @@
all_extrinsics.push_back({std::unique_ptr<double[]>(extrinsics), i});
for (size_t j = 0; j < 8; ++j) {
- aos::vision::Vector<2> temp = template_value[j];
- aos::vision::Vector<2> targ = target_value[j];
+ // Template target in target space as documented by GetTemplateTarget()
+ const ::aos::vision::Vector<2> template_point = template_value[j];
+ // Target in pixel space.
+ const ::aos::vision::Vector<2> target_point = target_value[j];
- auto ftor = [temp, targ, i](const double *const intrinsics,
- const double *const extrinsics,
- double *residual) {
- auto in = IntrinsicParams::get(intrinsics);
- auto extrinsic_params = ExtrinsicParams::get(extrinsics);
- auto pt = targ - Project(temp, in, extrinsic_params);
- residual[0] = pt.x();
- residual[1] = pt.y();
+ // Now build up the residual block.
+ auto ftor = [template_point, target_point, i](
+ const double *const intrinsics, const double *const extrinsics,
+ double *residual) {
+ const IntrinsicParams intrinsic_params =
+ IntrinsicParams::get(intrinsics);
+ const ExtrinsicParams extrinsic_params =
+ ExtrinsicParams::get(extrinsics);
+ // Project the target location into pixel coordinates.
+ const ::aos::vision::Vector<2> projected_point =
+ Project(template_point, intrinsic_params, extrinsic_params);
+ const ::aos::vision::Vector<2> residual_point =
+ target_point - projected_point;
+ residual[0] = residual_point.x();
+ residual[1] = residual_point.y();
return true;
};
problem.AddResidualBlock(
new NumericDiffCostFunction<decltype(ftor), CENTRAL, 2,
IntrinsicParams::kNumParams,
ExtrinsicParams::kNumParams>(
- new decltype(ftor)(std::move(ftor))),
+ new decltype(ftor)(::std::move(ftor))),
NULL, &intrinsics[0], extrinsics);
}
@@ -199,7 +206,7 @@
new NumericDiffCostFunction<decltype(ftor), CENTRAL, 3,
ExtrinsicParams::kNumParams,
CameraGeometry::kNumParams>(
- new decltype(ftor)(std::move(ftor))),
+ new decltype(ftor)(::std::move(ftor))),
NULL, extrinsics, &geometry[0]);
}
}
@@ -209,36 +216,36 @@
Solve(options, &problem, &summary);
{
- std::cout << summary.BriefReport() << "\n";
+ ::std::cout << summary.BriefReport() << ::std::endl;
IntrinsicParams intrinsics_ = IntrinsicParams::get(&intrinsics[0]);
CameraGeometry 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 << "rup = " << intrinsics_.mount_angle * 180 / M_PI << ";"
+ << ::std::endl;
+ ::std::cout << "fl = " << intrinsics_.focal_length << ";" << ::std::endl;
+ ::std::cout << "error = " << summary.final_cost << ";" << ::std::endl;
- 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";
+ ::std::cout << "camera_angle = " << geometry_.heading * 180 / M_PI
+ << ::std::endl;
+ ::std::cout << "camera_x = " << geometry_.location[0] << ::std::endl;
+ ::std::cout << "camera_y = " << geometry_.location[1] << ::std::endl;
+ ::std::cout << "camera_z = " << geometry_.location[2] << ::std::endl;
+ ::std::cout << "camera_barrel = " << intrinsics_.barrel_mount * 180.0 / M_PI
+ << ::std::endl;
for (const Sample &sample : all_extrinsics) {
- int i = sample.i;
+ const int i = sample.i;
double *data = sample.extrinsics.get();
- ExtrinsicParams extn = ExtrinsicParams::get(data);
+ const ExtrinsicParams extrinsic_params = ExtrinsicParams::get(data);
- auto err = GetError(info, data, &geometry[0], i);
+ const ::std::array<double, 3> error =
+ GetError(info, 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 << ", ";
+ ::std::cout << i << ", ";
+ ::std::cout << extrinsic_params.z << "m, ";
+ ::std::cout << extrinsic_params.y << "m, ";
+ ::std::cout << extrinsic_params.r1 * 180 / M_PI << ", ";
+ ::std::cout << extrinsic_params.r2 * 180 / M_PI << ", ";
// TODO: Methodology problem: This should really have a separate solve for
// extrinsics...
std::cout << err[0] << ", ";
diff --git a/y2019/vision/target_finder.cc b/y2019/vision/target_finder.cc
index d39764a..45678eb 100644
--- a/y2019/vision/target_finder.cc
+++ b/y2019/vision/target_finder.cc
@@ -7,7 +7,7 @@
namespace y2019 {
namespace vision {
-TargetFinder::TargetFinder() { target_template_ = Target::MakeTemplate(); }
+TargetFinder::TargetFinder() : target_template_(Target::MakeTemplate()) {}
aos::vision::RangeImage TargetFinder::Threshold(aos::vision::ImagePtr image) {
const uint8_t threshold_value = GetThresholdValue();
diff --git a/y2019/vision/target_finder.h b/y2019/vision/target_finder.h
index 666ae2f..1ba79ba 100644
--- a/y2019/vision/target_finder.h
+++ b/y2019/vision/target_finder.h
@@ -73,7 +73,7 @@
aos::vision::AnalysisAllocator alloc_;
// The template for the default target in the standard space.
- Target target_template_;
+ const Target target_template_;
IntrinsicParams intrinsics_;
diff --git a/y2019/vision/target_geometry.cc b/y2019/vision/target_geometry.cc
index 1dd6a1a..d4b8a54 100644
--- a/y2019/vision/target_geometry.cc
+++ b/y2019/vision/target_geometry.cc
@@ -124,20 +124,18 @@
Target Project(const Target &target, const IntrinsicParams &intrinsics,
const ExtrinsicParams &extrinsics) {
- auto project = [&](Vector<2> pt) {
- return Project(pt, intrinsics, extrinsics);
- };
Target new_targ;
new_targ.right.is_right = true;
- new_targ.right.top = project(target.right.top);
- new_targ.right.inside = project(target.right.inside);
- new_targ.right.bottom = project(target.right.bottom);
- new_targ.right.outside = project(target.right.outside);
+ new_targ.right.top = Project(target.right.top, intrinsics, extrinsics);
+ new_targ.right.inside = Project(target.right.inside, intrinsics, extrinsics);
+ new_targ.right.bottom = Project(target.right.bottom, intrinsics, extrinsics);
+ new_targ.right.outside =
+ Project(target.right.outside, intrinsics, extrinsics);
- new_targ.left.top = project(target.left.top);
- new_targ.left.inside = project(target.left.inside);
- new_targ.left.bottom = project(target.left.bottom);
- new_targ.left.outside = project(target.left.outside);
+ new_targ.left.top = Project(target.left.top, intrinsics, extrinsics);
+ new_targ.left.inside = Project(target.left.inside, intrinsics, extrinsics);
+ new_targ.left.bottom = Project(target.left.bottom, intrinsics, extrinsics);
+ new_targ.left.outside = Project(target.left.outside, intrinsics, extrinsics);
return new_targ;
}
diff --git a/y2019/vision/target_types.h b/y2019/vision/target_types.h
index d662316..8f763aa 100644
--- a/y2019/vision/target_types.h
+++ b/y2019/vision/target_types.h
@@ -72,6 +72,7 @@
return out;
}
};
+
// Projects a point from idealized template space to camera space.
aos::vision::Vector<2> Project(aos::vision::Vector<2> pt,
const IntrinsicParams &intrinsics,