Solver with turret fixes and visualization
Converges generally well, but still has sensitivity to some parameters
In addition to turret and visualization, significant changes include:
-- Added quaternion parameterization for pivot_to_imu
-- Filtering out of bad gyro readings
-- Scaling of rotation error (residual)
Visualization tool is really just a set of helper routines to plot axes
in 3D space in a projected 2D virtual image
Change-Id: I03a6939b6b12df4b8116c30c617a1595781fe635
Signed-off-by: Jim Ostrowski <yimmy13@gmail.com>
diff --git a/frc971/vision/BUILD b/frc971/vision/BUILD
index 7872926..8c35711 100644
--- a/frc971/vision/BUILD
+++ b/frc971/vision/BUILD
@@ -79,6 +79,7 @@
"//aos/events/logging:log_reader",
"//frc971/analysis:in_process_plotter",
"//frc971/control_loops/drivetrain:improved_down_estimator",
+ "//frc971/vision:visualize_robot",
"//frc971/wpilib:imu_batch_fbs",
"//frc971/wpilib:imu_fbs",
"//third_party:opencv",
@@ -143,3 +144,36 @@
"//aos/testing:googletest",
],
)
+
+cc_library(
+ name = "visualize_robot",
+ srcs = [
+ "visualize_robot.cc",
+ ],
+ hdrs = [
+ "visualize_robot.h",
+ ],
+ deps = [
+ "//aos:init",
+ "//third_party:opencv",
+ "@com_google_absl//absl/strings:str_format",
+ "@org_tuxfamily_eigen//:eigen",
+ ],
+)
+
+cc_binary(
+ name = "visualize_robot_sample",
+ srcs = [
+ "visualize_robot_sample.cc",
+ ],
+ target_compatible_with = ["@platforms//os:linux"],
+ visibility = ["//visibility:public"],
+ deps = [
+ "//aos:init",
+ "//frc971/vision:visualize_robot",
+ "//third_party:opencv",
+ "@com_github_google_glog//:glog",
+ "@com_google_ceres_solver//:ceres",
+ "@org_tuxfamily_eigen//:eigen",
+ ],
+)