commit 9c3e2daf3de8bfaa88a559cada286f90dedbf21a
author Austin Schuh <austin.linux@gmail.com> Wed Mar 13 19:50:00 2019 -0700
committer Gerrit Code Review <gerrit@robotics.mvla.net> Wed Mar 13 19:50:00 2019 -0700
tree 550710aa32c6786cdbbf7de8e037ed1b81f803c3
parent 229fcba0d59e7cad1b2382220a4ecc4e1488c9cb
parent 463a3bc0c3cdd3f96b657c7d35f50b5a0ae530d5

Merge "Recalibrate practice bot cameras with new distortion math."

y2019/vision/constants.cc

diff --git a/y2019/vision/constants.cc b/y2019/vision/constants.cc
index a587007..310faf6 100644
--- a/y2019/vision/constants.cc
+++ b/y2019/vision/constants.cc
@@ -61,12 +61,12 @@
 
 CameraCalibration camera_6 = {
     {
-        -1.11982 / 180.0 * M_PI, 347.32, 1.04799 / 180.0 * M_PI,
+        -1.15844 / 180.0 * M_PI, 348.161, 1.16894 / 180.0 * M_PI,
     },
     {
-        {{4.84356 * kInchesToMeters, 2.12355 * kInchesToMeters,
-          33.1812 * kInchesToMeters}},
-        -11.888 / 180.0 * M_PI,
+        {{4.73183 * kInchesToMeters, 2.0984 * kInchesToMeters,
+          33.2023 * kInchesToMeters}},
+        -11.8598 / 180.0 * M_PI,
     },
     {
         6,
@@ -79,12 +79,12 @@
 
 CameraCalibration camera_7 = {
     {
-        -2.21509 / 180.0 * M_PI, 328.2, 0.922133 / 180.0 * M_PI,
+        -2.24098 / 180.0 * M_PI, 339.231, 1.15487 / 180.0 * M_PI,
     },
     {
-        {{4.7585 * kInchesToMeters, 3.99542 * kInchesToMeters,
-          33.1932 * kInchesToMeters}},
-        19.2585 / 180.0 * M_PI,
+        {{3.50224 * kInchesToMeters, 3.95441 * kInchesToMeters,
+          33.3469 * kInchesToMeters}},
+        18.6782 / 180.0 * M_PI,
     },
     {
         7,
@@ -97,12 +97,12 @@
 
 CameraCalibration camera_8 = {
     {
-        37.1314 / 180.0 * M_PI, 340.763, -0.136505 / 180.0 * M_PI,
+        37.1859 / 180.0 * M_PI, 339.517, 0.0405714 / 180.0 * M_PI,
     },
     {
-        {{3.61159 * kInchesToMeters, 5.16559 * kInchesToMeters,
-          12.6279 * kInchesToMeters}},
-        92.78 / 180.0 * M_PI,
+        {{3.57002 * kInchesToMeters, 5.26966 * kInchesToMeters,
+          12.6807 * kInchesToMeters}},
+        92.6787 / 180.0 * M_PI,
     },
     {
         8,
@@ -115,12 +115,12 @@
 
 CameraCalibration camera_9 = {
     {
-        35.4057 / 180.0 * M_PI, 337.052, 3.57896 / 180.0 * M_PI,
+        35.4154 / 180.0 * M_PI, 337.471, 3.30546 / 180.0 * M_PI,
     },
     {
-        {{4.28266 * kInchesToMeters, -3.04674 * kInchesToMeters,
-          11.4219 * kInchesToMeters}},
-        -93.3496 / 180.0 * M_PI,
+        {{4.25679 * kInchesToMeters, -2.93066 * kInchesToMeters,
+          11.3228 * kInchesToMeters}},
+        -93.219 / 180.0 * M_PI,
     },
     {
         9,
@@ -133,12 +133,12 @@
 
 CameraCalibration camera_10 = {
     {
-        -0.263033 / 180.0 * M_PI, 347.579, 0.213753 / 180.0 * M_PI,
+        -0.190556 / 180.0 * M_PI, 345.022, 0.468494 / 180.0 * M_PI,
     },
     {
-        {{-4.71537 * kInchesToMeters, 2.87784 * kInchesToMeters,
-          33.3979 * kInchesToMeters}},
-        182.17 / 180.0 * M_PI,
+        {{-4.83005 * kInchesToMeters, 2.95565 * kInchesToMeters,
+          33.3624 * kInchesToMeters}},
+        182.204 / 180.0 * M_PI,
     },
     {
         10,

y2019/vision/debug_viewer.cc

diff --git a/y2019/vision/debug_viewer.cc b/y2019/vision/debug_viewer.cc
index a919bce..3c174b4 100644
--- a/y2019/vision/debug_viewer.cc
+++ b/y2019/vision/debug_viewer.cc
@@ -332,8 +332,9 @@
 }  // namespace y2017
 
 int main(int argc, char **argv) {
-  y2019::vision::FilterHarness filter_harness;
   ::gflags::ParseCommandLineFlags(&argc, &argv, true);
+
+  y2019::vision::FilterHarness filter_harness;
   aos::vision::DebugFrameworkMain(argc, argv, &filter_harness,
                                   aos::vision::CameraParams());
 }

y2019/vision/target_geometry.cc

diff --git a/y2019/vision/target_geometry.cc b/y2019/vision/target_geometry.cc
index 8239dd6..6df1a27 100644
--- a/y2019/vision/target_geometry.cc
+++ b/y2019/vision/target_geometry.cc
@@ -266,6 +266,18 @@
         NULL, &params_8point[0]);
   }
 
+  Solver::Options options;
+  options.minimizer_progress_to_stdout = false;
+  Solver::Summary summary_8point;
+  Solve(options, &problem_8point, &summary_8point);
+
+
+  // So, let's sneak up on it.  Start by warm-starting it with where we got on the 8 point solution.
+  ExtrinsicParams::get(&params_8point[0]).set(&params_4point[0]);
+  // Then solve without the bottom constraint.
+  Solver::Summary summary_4point1;
+  Solve(options, &problem_4point, &summary_4point1);
+
   // Now, add a large cost for the bottom point being below the bottom line.
   problem_4point.AddResidualBlock(
       new NumericDiffCostFunction<BottomPointCostFunctor, CENTRAL, 1, 4>(
@@ -284,18 +296,15 @@
                                      intrinsics_)),
       NULL, &params_4point[0]);
 
-  Solver::Options options;
-  options.minimizer_progress_to_stdout = false;
-  Solver::Summary summary_8point;
-  Solve(options, &problem_8point, &summary_8point);
-  Solver::Summary summary_4point;
-  Solve(options, &problem_4point, &summary_4point);
+  // And then re-solve.
+  Solver::Summary summary_4point2;
+  Solve(options, &problem_4point, &summary_4point2);
 
   IntermediateResult IR;
   IR.extrinsics = ExtrinsicParams::get(&params_8point[0]);
   IR.solver_error = summary_8point.final_cost;
   IR.backup_extrinsics = ExtrinsicParams::get(&params_4point[0]);
-  IR.backup_solver_error = summary_4point.final_cost;
+  IR.backup_solver_error = summary_4point2.final_cost;
 
   if (verbose) {
     std::cout << "rup = " << intrinsics_.mount_angle * 180 / M_PI << ";\n";
@@ -308,8 +317,11 @@
     std::cout << "r1 = " << IR.extrinsics.r1 * 180 / M_PI << ";\n";
     std::cout << "r2 = " << IR.extrinsics.r2 * 180 / M_PI << ";\n";
     std::cout << "4 points:\n";
-    std::cout << summary_4point.BriefReport() << "\n";
-    std::cout << "error = " << summary_4point.final_cost << ";\n\n";
+    std::cout << summary_4point1.BriefReport() << "\n";
+    std::cout << "error = " << summary_4point1.final_cost << ";\n\n";
+    std::cout << "4 points:\n";
+    std::cout << summary_4point2.BriefReport() << "\n";
+    std::cout << "error = " << summary_4point2.final_cost << ";\n\n";
     std::cout << "y = " << IR.backup_extrinsics.y / kInchesToMeters << ";\n";
     std::cout << "z = " << IR.backup_extrinsics.z / kInchesToMeters << ";\n";
     std::cout << "r1 = " << IR.backup_extrinsics.r1 * 180 / M_PI << ";\n";