Move over to ABSL logging and flags.
Removes gperftools too since that wants gflags.
Here come the fireworks.
Change-Id: I79cb7bcf60f1047fbfa28bfffc21a0fd692e4b1c
Signed-off-by: Austin Schuh <austin.linux@gmail.com>
diff --git a/frc971/vision/BUILD b/frc971/vision/BUILD
index 14e3d41..48a8fb1 100644
--- a/frc971/vision/BUILD
+++ b/frc971/vision/BUILD
@@ -53,8 +53,9 @@
"//aos/logging:log_namer",
"//aos/util:filesystem_fbs",
"//frc971/input:joystick_state_fbs",
- "@com_github_gflags_gflags//:gflags",
- "@com_github_google_glog//:glog",
+ "@com_google_absl//absl/flags:flag",
+ "@com_google_absl//absl/log",
+ "@com_google_absl//absl/log:check",
],
)
@@ -150,8 +151,9 @@
"//aos/events:event_loop",
"//aos/scoped:scoped_fd",
"//aos/util:threaded_consumer",
- "@com_github_google_glog//:glog",
"@com_google_absl//absl/base",
+ "@com_google_absl//absl/log",
+ "@com_google_absl//absl/log:check",
],
)
@@ -175,7 +177,8 @@
"//frc971/vision:vision_fbs",
"//third_party:opencv",
"@com_github_foxglove_schemas//:schemas",
- "@com_github_google_glog//:glog",
+ "@com_google_absl//absl/log",
+ "@com_google_absl//absl/log:check",
"@com_google_absl//absl/strings:str_format",
"@com_google_absl//absl/types:span",
"@org_tuxfamily_eigen//:eigen",
@@ -270,7 +273,8 @@
deps = [
"//aos/util:math",
"//third_party:opencv",
- "@com_github_google_glog//:glog",
+ "@com_google_absl//absl/log",
+ "@com_google_absl//absl/log:check",
],
)
@@ -313,7 +317,8 @@
"//aos:init",
"//frc971/vision:visualize_robot",
"//third_party:opencv",
- "@com_github_google_glog//:glog",
+ "@com_google_absl//absl/log",
+ "@com_google_absl//absl/log:check",
"@com_google_ceres_solver//:ceres",
"@org_tuxfamily_eigen//:eigen",
],
@@ -329,7 +334,8 @@
deps = [
"//aos/scoped:scoped_fd",
"//aos/util:file",
- "@com_github_google_glog//:glog",
+ "@com_google_absl//absl/log",
+ "@com_google_absl//absl/log:check",
"@com_google_absl//absl/strings",
],
)
@@ -433,7 +439,8 @@
deps = [
"//frc971/vision:calibration_fbs",
"//third_party:opencv",
- "@com_github_google_glog//:glog",
+ "@com_google_absl//absl/log",
+ "@com_google_absl//absl/log:check",
"@com_google_absl//absl/strings:str_format",
],
)
@@ -445,7 +452,8 @@
deps = [
"//aos/testing:googletest",
"//frc971/vision:vision_util_lib",
- "@com_github_google_glog//:glog",
+ "@com_google_absl//absl/log",
+ "@com_google_absl//absl/log:check",
],
)
diff --git a/frc971/vision/calibration_accumulator.cc b/frc971/vision/calibration_accumulator.cc
index bbf51ee..9b1fa97 100644
--- a/frc971/vision/calibration_accumulator.cc
+++ b/frc971/vision/calibration_accumulator.cc
@@ -5,6 +5,7 @@
#include <limits>
#include "Eigen/Dense"
+#include "absl/flags/flag.h"
#include "external/com_github_foxglove_schemas/CompressedImage_schema.h"
#include "external/com_github_foxglove_schemas/ImageAnnotations_schema.h"
#include <opencv2/highgui/highgui.hpp>
@@ -16,11 +17,11 @@
#include "frc971/vision/charuco_lib.h"
#include "frc971/wpilib/imu_batch_generated.h"
-DEFINE_bool(display_undistorted, false,
- "If true, display the undistorted image.");
-DEFINE_string(save_path, "", "Where to store annotated images");
-DEFINE_bool(save_valid_only, false,
- "If true, only save images with valid pose estimates");
+ABSL_FLAG(bool, display_undistorted, false,
+ "If true, display the undistorted image.");
+ABSL_FLAG(std::string, save_path, "", "Where to store annotated images");
+ABSL_FLAG(bool, save_valid_only, false,
+ "If true, only save images with valid pose estimates");
namespace frc971::vision {
using aos::distributed_clock;
@@ -232,8 +233,8 @@
<< "\nT:" << tvecs_eigen[0].transpose().format(HeavyFmt);
}
- if (FLAGS_visualize) {
- if (FLAGS_display_undistorted) {
+ if (absl::GetFlag(FLAGS_visualize)) {
+ if (absl::GetFlag(FLAGS_display_undistorted)) {
const cv::Size image_size(rgb_image.cols, rgb_image.rows);
cv::Mat undistorted_rgb_image(image_size, CV_8UC3);
cv::undistort(rgb_image, undistorted_rgb_image,
@@ -247,11 +248,11 @@
cv::waitKey(1);
}
- if (FLAGS_save_path != "") {
- if (!FLAGS_save_valid_only || valid) {
+ if (absl::GetFlag(FLAGS_save_path) != "") {
+ if (!absl::GetFlag(FLAGS_save_valid_only) || valid) {
static int img_count = 0;
std::string image_name = absl::StrFormat("/img_%06d.png", img_count);
- std::string path = FLAGS_save_path + image_name;
+ std::string path = absl::GetFlag(FLAGS_save_path) + image_name;
VLOG(2) << "Saving image to " << path;
cv::imwrite(path, rgb_image);
img_count++;
diff --git a/frc971/vision/ceres/BUILD b/frc971/vision/ceres/BUILD
index f2cb96b..d2671c8 100644
--- a/frc971/vision/ceres/BUILD
+++ b/frc971/vision/ceres/BUILD
@@ -15,7 +15,7 @@
target_compatible_with = ["@platforms//os:linux"],
visibility = ["//visibility:public"],
deps = [
- "@com_github_gflags_gflags//:gflags",
+ "@com_google_absl//absl/flags:flag",
"@com_google_ceres_solver//:ceres",
"@org_tuxfamily_eigen//:eigen",
],
diff --git a/frc971/vision/ceres/read_g2o.h b/frc971/vision/ceres/read_g2o.h
index c427939..b10ac22 100644
--- a/frc971/vision/ceres/read_g2o.h
+++ b/frc971/vision/ceres/read_g2o.h
@@ -36,7 +36,8 @@
#include <fstream>
#include <string>
-#include "glog/logging.h"
+#include "absl/log/check.h"
+#include "absl/log/log.h"
namespace ceres::examples {
diff --git a/frc971/vision/charuco_lib.cc b/frc971/vision/charuco_lib.cc
index 71c2ff6..dc4cb18 100644
--- a/frc971/vision/charuco_lib.cc
+++ b/frc971/vision/charuco_lib.cc
@@ -4,7 +4,10 @@
#include <functional>
#include <string_view>
-#include "glog/logging.h"
+#include "absl/flags/declare.h"
+#include "absl/flags/flag.h"
+#include "absl/log/check.h"
+#include "absl/log/log.h"
#include <opencv2/core/eigen.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc.hpp>
@@ -15,28 +18,30 @@
#include "frc971/control_loops/quaternion_utils.h"
#include "frc971/vision/vision_generated.h"
-DEFINE_string(board_template_path, "",
- "If specified, write an image to the specified path for the "
- "charuco board pattern.");
-DEFINE_bool(coarse_pattern, true, "If true, use coarse arucos; else, use fine");
-DEFINE_uint32(gray_threshold, 0,
- "If > 0, threshold image based on this grayscale value");
-DEFINE_bool(large_board, true, "If true, use the large calibration board.");
-DEFINE_uint32(
- min_charucos, 10,
+ABSL_FLAG(std::string, board_template_path, "",
+ "If specified, write an image to the specified path for the "
+ "charuco board pattern.");
+ABSL_FLAG(bool, coarse_pattern, true,
+ "If true, use coarse arucos; else, use fine");
+ABSL_FLAG(uint32_t, gray_threshold, 0,
+ "If > 0, threshold image based on this grayscale value");
+ABSL_FLAG(bool, large_board, true, "If true, use the large calibration board.");
+ABSL_FLAG(
+ uint32_t, min_charucos, 10,
"The mininum number of aruco targets in charuco board required to match.");
-DEFINE_uint32(min_id, 0, "Minimum valid charuco id");
-DEFINE_uint32(max_diamonds, 0,
- "Maximum number of diamonds to see. Set to 0 for no limit");
-DEFINE_uint32(max_id, 15, "Maximum valid charuco id");
-DEFINE_bool(visualize, false, "Whether to visualize the resulting data.");
-DEFINE_bool(
- draw_axes, false,
+ABSL_FLAG(uint32_t, min_id, 0, "Minimum valid charuco id");
+ABSL_FLAG(uint32_t, max_diamonds, 0,
+ "Maximum number of diamonds to see. Set to 0 for no limit");
+ABSL_FLAG(uint32_t, max_id, 15, "Maximum valid charuco id");
+ABSL_FLAG(bool, visualize, false, "Whether to visualize the resulting data.");
+ABSL_FLAG(
+ bool, draw_axes, false,
"Whether to draw axes on the resulting data-- warning, may cause crashes.");
-DEFINE_uint32(disable_delay, 100, "Time after an issue to disable tracing at.");
+ABSL_FLAG(uint32_t, disable_delay, 100,
+ "Time after an issue to disable tracing at.");
-DECLARE_bool(enable_ftrace);
+ABSL_DECLARE_FLAG(bool, enable_ftrace);
namespace frc971::vision {
namespace chrono = std::chrono;
@@ -121,13 +126,14 @@
const double age_double =
std::chrono::duration_cast<std::chrono::duration<double>>(age).count();
if (age > max_age_) {
- if (FLAGS_enable_ftrace) {
+ if (absl::GetFlag(FLAGS_enable_ftrace)) {
ftrace_.FormatMessage("Too late receiving image, age: %f\n",
age_double);
- if (FLAGS_disable_delay > 0) {
+ if (absl::GetFlag(FLAGS_disable_delay) > 0) {
if (!disabling_) {
- timer_fn_->Schedule(event_loop_->monotonic_now() +
- chrono::milliseconds(FLAGS_disable_delay));
+ timer_fn_->Schedule(
+ event_loop_->monotonic_now() +
+ chrono::milliseconds(absl::GetFlag(FLAGS_disable_delay)));
disabling_ = true;
}
} else {
@@ -175,28 +181,32 @@
if (target_type_ == TargetType::kCharuco ||
target_type_ == TargetType::kAruco) {
dictionary_ = cv::aruco::getPredefinedDictionary(
- FLAGS_large_board ? cv::aruco::DICT_5X5_250 : cv::aruco::DICT_6X6_250);
+ absl::GetFlag(FLAGS_large_board) ? cv::aruco::DICT_5X5_250
+ : cv::aruco::DICT_6X6_250);
if (target_type_ == TargetType::kCharuco) {
- LOG(INFO) << "Using " << (FLAGS_large_board ? "large" : "small")
+ LOG(INFO) << "Using "
+ << (absl::GetFlag(FLAGS_large_board) ? "large" : "small")
<< " charuco board with "
- << (FLAGS_coarse_pattern ? "coarse" : "fine") << " pattern";
- board_ =
- (FLAGS_large_board
- ? (FLAGS_coarse_pattern ? cv::aruco::CharucoBoard::create(
- 12, 9, 0.06, 0.04666, dictionary_)
- : cv::aruco::CharucoBoard::create(
- 25, 18, 0.03, 0.0233, dictionary_))
- : (FLAGS_coarse_pattern ? cv::aruco::CharucoBoard::create(
- 7, 5, 0.04, 0.025, dictionary_)
- // TODO(jim): Need to figure out what
- // size is for small board, fine pattern
- : cv::aruco::CharucoBoard::create(
- 7, 5, 0.03, 0.0233, dictionary_)));
- if (!FLAGS_board_template_path.empty()) {
+ << (absl::GetFlag(FLAGS_coarse_pattern) ? "coarse" : "fine")
+ << " pattern";
+ board_ = (absl::GetFlag(FLAGS_large_board)
+ ? (absl::GetFlag(FLAGS_coarse_pattern)
+ ? cv::aruco::CharucoBoard::create(
+ 12, 9, 0.06, 0.04666, dictionary_)
+ : cv::aruco::CharucoBoard::create(
+ 25, 18, 0.03, 0.0233, dictionary_))
+ : (absl::GetFlag(FLAGS_coarse_pattern)
+ ? cv::aruco::CharucoBoard::create(7, 5, 0.04, 0.025,
+ dictionary_)
+ // TODO(jim): Need to figure out what
+ // size is for small board, fine pattern
+ : cv::aruco::CharucoBoard::create(7, 5, 0.03, 0.0233,
+ dictionary_)));
+ if (!absl::GetFlag(FLAGS_board_template_path).empty()) {
cv::Mat board_image;
board_->draw(cv::Size(600, 500), board_image, 10, 1);
- cv::imwrite(FLAGS_board_template_path, board_image);
+ cv::imwrite(absl::GetFlag(FLAGS_board_template_path), board_image);
}
}
} else if (target_type_ == TargetType::kCharucoDiamond) {
@@ -244,7 +254,7 @@
// TODO<Jim>: Either track this down or reimplement drawAxes
if (result.z() < 0.01) {
LOG(INFO) << "Skipping, due to z value too small: " << result.z();
- } else if (FLAGS_draw_axes == true) {
+ } else if (absl::GetFlag(FLAGS_draw_axes) == true) {
result /= result.z();
if (target_type_ == TargetType::kCharuco ||
target_type_ == TargetType::kAprilTag) {
@@ -353,12 +363,13 @@
.count();
// Have found this useful if there is blurry / noisy images
- if (FLAGS_gray_threshold > 0) {
+ if (absl::GetFlag(FLAGS_gray_threshold) > 0) {
cv::Mat gray;
cv::cvtColor(rgb_image, gray, cv::COLOR_BGR2GRAY);
cv::Mat thresh;
- cv::threshold(gray, thresh, FLAGS_gray_threshold, 255, cv::THRESH_BINARY);
+ cv::threshold(gray, thresh, absl::GetFlag(FLAGS_gray_threshold), 255,
+ cv::THRESH_BINARY);
cv::cvtColor(thresh, rgb_image, cv::COLOR_GRAY2RGB);
}
@@ -382,7 +393,7 @@
std::vector<cv::Point2f> charuco_corners;
// If enough aruco markers detected for the Charuco board
- if (marker_ids.size() >= FLAGS_min_charucos) {
+ if (marker_ids.size() >= absl::GetFlag(FLAGS_min_charucos)) {
// Run everything twice, once with the calibration, and once
// without. This lets us both collect data to calibrate the
// intrinsics of the camera (to determine the intrinsics from
@@ -403,7 +414,7 @@
charuco_corners_with_calibration, charuco_ids_with_calibration,
calibration_.CameraIntrinsics(), calibration_.CameraDistCoeffs());
- if (charuco_ids.size() >= FLAGS_min_charucos) {
+ if (charuco_ids.size() >= absl::GetFlag(FLAGS_min_charucos)) {
cv::aruco::drawDetectedCornersCharuco(
rgb_image, charuco_corners, charuco_ids, cv::Scalar(255, 0, 0));
@@ -432,12 +443,14 @@
}
} else {
VLOG(2) << "Age: " << age_double << ", not enough charuco IDs, got "
- << charuco_ids.size() << ", needed " << FLAGS_min_charucos;
+ << charuco_ids.size() << ", needed "
+ << absl::GetFlag(FLAGS_min_charucos);
}
} else {
VLOG(2) << "Age: " << age_double
<< ", not enough marker IDs for charuco board, got "
- << marker_ids.size() << ", needed " << FLAGS_min_charucos;
+ << marker_ids.size() << ", needed "
+ << absl::GetFlag(FLAGS_min_charucos);
}
} else if (target_type_ == TargetType::kAruco ||
target_type_ == TargetType::kAprilTag) {
@@ -466,14 +479,15 @@
// Should be at least one, and no more than FLAGS_max_diamonds.
// Different calibration routines will require different values for this
if (diamond_ids.size() > 0 &&
- (FLAGS_max_diamonds == 0 ||
- diamond_ids.size() <= FLAGS_max_diamonds)) {
+ (absl::GetFlag(FLAGS_max_diamonds) == 0 ||
+ diamond_ids.size() <= absl::GetFlag(FLAGS_max_diamonds))) {
// TODO<Jim>: Could probably make this check more general than
// requiring range of ids
bool all_valid_ids = true;
for (uint i = 0; i < 4; i++) {
uint id = diamond_ids[0][i];
- if ((id < FLAGS_min_id) || (id > FLAGS_max_id)) {
+ if ((id < absl::GetFlag(FLAGS_min_id)) ||
+ (id > absl::GetFlag(FLAGS_max_id))) {
all_valid_ids = false;
LOG(INFO) << "Got invalid charuco id: " << id;
}
@@ -505,7 +519,8 @@
} else {
VLOG(2) << "Found too many number of diamond markers, which likely "
"means false positives were detected: "
- << diamond_ids.size() << " > " << FLAGS_max_diamonds;
+ << diamond_ids.size() << " > "
+ << absl::GetFlag(FLAGS_max_diamonds);
}
}
} else {
diff --git a/frc971/vision/charuco_lib.h b/frc971/vision/charuco_lib.h
index aac25ee..e53d129 100644
--- a/frc971/vision/charuco_lib.h
+++ b/frc971/vision/charuco_lib.h
@@ -15,7 +15,7 @@
#include "aos/network/message_bridge_server_generated.h"
#include "frc971/vision/calibration_generated.h"
-DECLARE_bool(visualize);
+ABSL_DECLARE_FLAG(bool, visualize);
namespace frc971::vision {
diff --git a/frc971/vision/foxglove_image_converter.cc b/frc971/vision/foxglove_image_converter.cc
index c004ac0..04f6030 100644
--- a/frc971/vision/foxglove_image_converter.cc
+++ b/frc971/vision/foxglove_image_converter.cc
@@ -1,20 +1,23 @@
+#include "absl/flags/flag.h"
+
#include "aos/events/shm_event_loop.h"
#include "aos/init.h"
#include "frc971/vision/foxglove_image_converter_lib.h"
-DEFINE_string(config, "aos_config.json", "Path to the config file to use.");
-DEFINE_string(channel, "/camera", "Input/Output channel to use.");
+ABSL_FLAG(std::string, config, "aos_config.json",
+ "Path to the config file to use.");
+ABSL_FLAG(std::string, channel, "/camera", "Input/Output channel to use.");
int main(int argc, char *argv[]) {
aos::InitGoogle(&argc, &argv);
aos::FlatbufferDetachedBuffer<aos::Configuration> config =
- aos::configuration::ReadConfig(FLAGS_config);
+ aos::configuration::ReadConfig(absl::GetFlag(FLAGS_config));
aos::ShmEventLoop event_loop(&config.message());
frc971::vision::FoxgloveImageConverter converter(
- &event_loop, FLAGS_channel, FLAGS_channel,
+ &event_loop, absl::GetFlag(FLAGS_channel), absl::GetFlag(FLAGS_channel),
frc971::vision::ImageCompression::kJpeg);
event_loop.Run();
diff --git a/frc971/vision/foxglove_image_converter_lib.cc b/frc971/vision/foxglove_image_converter_lib.cc
index 649f1bb..e6ebc4d 100644
--- a/frc971/vision/foxglove_image_converter_lib.cc
+++ b/frc971/vision/foxglove_image_converter_lib.cc
@@ -1,13 +1,14 @@
#include "frc971/vision/foxglove_image_converter_lib.h"
+#include "absl/flags/flag.h"
#include <opencv2/imgcodecs.hpp>
#include <opencv2/imgproc.hpp>
-DEFINE_int32(jpeg_quality, 60,
- "Compression quality of JPEGs, 0-100; lower numbers mean lower "
- "quality and resulting image sizes.");
-DEFINE_uint32(max_period_ms, 100,
- "Fastest period at which to convert images, to limit CPU usage.");
+ABSL_FLAG(int32_t, jpeg_quality, 60,
+ "Compression quality of JPEGs, 0-100; lower numbers mean lower "
+ "quality and resulting image sizes.");
+ABSL_FLAG(uint32_t, max_period_ms, 100,
+ "Fastest period at which to convert images, to limit CPU usage.");
namespace frc971::vision {
std::string_view ExtensionForCompression(ImageCompression compression) {
@@ -26,8 +27,9 @@
// imencode doesn't let us pass in anything other than an std::vector, and
// performance isn't yet a big enough issue to try to avoid the copy.
std::vector<uint8_t> buffer;
- CHECK(cv::imencode(absl::StrCat(".", format), image, buffer,
- {cv::IMWRITE_JPEG_QUALITY, FLAGS_jpeg_quality}));
+ CHECK(cv::imencode(
+ absl::StrCat(".", format), image, buffer,
+ {cv::IMWRITE_JPEG_QUALITY, absl::GetFlag(FLAGS_jpeg_quality)}));
const flatbuffers::Offset<flatbuffers::Vector<uint8_t>> data_offset =
fbb->CreateVector(buffer);
const struct timespec timestamp_t = aos::time::to_timespec(eof);
@@ -52,7 +54,7 @@
[this, compression](const cv::Mat image,
const aos::monotonic_clock::time_point eof) {
if (event_loop_->monotonic_now() >
- (std::chrono::milliseconds(FLAGS_max_period_ms) +
+ (std::chrono::milliseconds(absl::GetFlag(FLAGS_max_period_ms)) +
sender_.monotonic_sent_time())) {
auto builder = sender_.MakeBuilder();
builder.CheckOk(builder.Send(
diff --git a/frc971/vision/foxglove_image_converter_test.cc b/frc971/vision/foxglove_image_converter_test.cc
index c9c7d66..3059ad2 100644
--- a/frc971/vision/foxglove_image_converter_test.cc
+++ b/frc971/vision/foxglove_image_converter_test.cc
@@ -1,3 +1,5 @@
+#include "absl/flags/declare.h"
+#include "absl/flags/flag.h"
#include "gtest/gtest.h"
#include "aos/events/simulated_event_loop.h"
@@ -6,7 +8,7 @@
#include "aos/testing/tmpdir.h"
#include "frc971/vision/foxglove_image_converter_lib.h"
-DECLARE_int32(jpeg_quality);
+ABSL_DECLARE_FLAG(int32_t, jpeg_quality);
namespace frc971::vision {
std::ostream &operator<<(std::ostream &os, ImageCompression compression) {
@@ -34,7 +36,7 @@
// Because our test image for comparison was generated with a JPEG quality
// of 95, we need to use that for the test to work. This also protects the
// tests against future changes to the default JPEG quality.
- FLAGS_jpeg_quality = 95;
+ absl::SetFlag(&FLAGS_jpeg_quality, 95);
test_event_loop_->OnRun(
[this]() { image_sender_.CheckOk(image_sender_.Send(camera_image_)); });
test_event_loop_->MakeWatcher(
diff --git a/frc971/vision/geometry.h b/frc971/vision/geometry.h
index 3285446..f1495bc 100644
--- a/frc971/vision/geometry.h
+++ b/frc971/vision/geometry.h
@@ -3,7 +3,8 @@
#include <optional>
-#include "glog/logging.h"
+#include "absl/log/check.h"
+#include "absl/log/log.h"
#include "opencv2/core/types.hpp"
#include "aos/util/math.h"
diff --git a/frc971/vision/geometry_test.cc b/frc971/vision/geometry_test.cc
index f34594c..070de12 100644
--- a/frc971/vision/geometry_test.cc
+++ b/frc971/vision/geometry_test.cc
@@ -2,7 +2,8 @@
#include <cmath>
-#include "glog/logging.h"
+#include "absl/log/check.h"
+#include "absl/log/log.h"
#include "gtest/gtest.h"
#include "aos/util/math.h"
diff --git a/frc971/vision/image_logger.cc b/frc971/vision/image_logger.cc
index 2984824..cb8fc4e 100644
--- a/frc971/vision/image_logger.cc
+++ b/frc971/vision/image_logger.cc
@@ -1,8 +1,10 @@
#include <sys/resource.h>
#include <sys/time.h>
-#include "gflags/gflags.h"
-#include "glog/logging.h"
+#include "absl/flags/flag.h"
+#include "absl/flags/usage.h"
+#include "absl/log/check.h"
+#include "absl/log/log.h"
#include "aos/configuration.h"
#include "aos/events/logging/log_writer.h"
@@ -12,16 +14,16 @@
#include "aos/util/filesystem_generated.h"
#include "frc971/input/joystick_state_generated.h"
-DEFINE_string(config, "aos_config.json", "Config file to use.");
+ABSL_FLAG(std::string, config, "aos_config.json", "Config file to use.");
-DEFINE_double(rotate_every, 0.0,
- "If set, rotate the logger after this many seconds");
-DECLARE_int32(flush_size);
-DEFINE_double(disabled_time, 5.0,
- "Continue logging if disabled for this amount of time or less");
-DEFINE_bool(direct, false,
- "If true, write using O_DIRECT and write 512 byte aligned blocks "
- "whenever possible.");
+ABSL_FLAG(double, rotate_every, 0.0,
+ "If set, rotate the logger after this many seconds");
+ABSL_DECLARE_FLAG(int32_t, flush_size);
+ABSL_FLAG(double, disabled_time, 5.0,
+ "Continue logging if disabled for this amount of time or less");
+ABSL_FLAG(bool, direct, false,
+ "If true, write using O_DIRECT and write 512 byte aligned blocks "
+ "whenever possible.");
std::unique_ptr<aos::logger::MultiNodeFilesLogNamer> MakeLogNamer(
aos::EventLoop *event_loop) {
@@ -33,19 +35,20 @@
}
return std::make_unique<aos::logger::MultiNodeFilesLogNamer>(
- event_loop, std::make_unique<aos::logger::RenamableFileBackend>(
- absl::StrCat(log_name.value(), "/"), FLAGS_direct));
+ event_loop,
+ std::make_unique<aos::logger::RenamableFileBackend>(
+ absl::StrCat(log_name.value(), "/"), absl::GetFlag(FLAGS_direct)));
}
int main(int argc, char *argv[]) {
- gflags::SetUsageMessage(
+ absl::SetProgramUsageMessage(
"This program provides a simple logger binary that logs all SHMEM data "
"directly to a file specified at the command line when the robot is "
"enabled and for a bit of time after.");
aos::InitGoogle(&argc, &argv);
aos::FlatbufferDetachedBuffer<aos::Configuration> config =
- aos::configuration::ReadConfig(FLAGS_config);
+ aos::configuration::ReadConfig(absl::GetFlag(FLAGS_config));
aos::ShmEventLoop event_loop(&config.message());
@@ -60,11 +63,12 @@
event_loop.monotonic_now();
aos::logger::Logger logger(&event_loop);
- if (FLAGS_rotate_every != 0.0) {
+ if (absl::GetFlag(FLAGS_rotate_every) != 0.0) {
logger.set_on_logged_period([&](aos::monotonic_clock::time_point) {
const auto now = event_loop.monotonic_now();
- if (logging && now > last_rotation_time + std::chrono::duration<double>(
- FLAGS_rotate_every)) {
+ if (logging &&
+ now > last_rotation_time + std::chrono::duration<double>(
+ absl::GetFlag(FLAGS_rotate_every))) {
logger.Rotate();
last_rotation_time = now;
}
@@ -105,8 +109,9 @@
const bool should_be_logging =
(enabled ||
- timestamp < last_disable_time + std::chrono::duration<double>(
- FLAGS_disabled_time)) &&
+ timestamp <
+ last_disable_time + std::chrono::duration<double>(
+ absl::GetFlag(FLAGS_disabled_time))) &&
enough_space;
if (!logging && should_be_logging) {
diff --git a/frc971/vision/image_replay.cc b/frc971/vision/image_replay.cc
index 3b75c92..f4b3842 100644
--- a/frc971/vision/image_replay.cc
+++ b/frc971/vision/image_replay.cc
@@ -1,4 +1,4 @@
-#include "gflags/gflags.h"
+#include "absl/flags/flag.h"
#include "opencv2/highgui.hpp"
#include "opencv2/imgproc.hpp"
@@ -10,8 +10,8 @@
#include "aos/logging/log_message_generated.h"
#include "frc971/vision/vision_generated.h"
-DEFINE_string(node, "orin1", "The node to view the log from");
-DEFINE_string(channel, "/camera0", "The channel to view the log from");
+ABSL_FLAG(std::string, node, "orin1", "The node to view the log from");
+ABSL_FLAG(std::string, channel, "/camera0", "The channel to view the log from");
int main(int argc, char **argv) {
aos::InitGoogle(&argc, &argv);
@@ -23,11 +23,12 @@
aos::SimulatedEventLoopFactory factory(reader.configuration());
reader.Register(&factory);
- aos::NodeEventLoopFactory *node = factory.GetNodeEventLoopFactory(FLAGS_node);
+ aos::NodeEventLoopFactory *node =
+ factory.GetNodeEventLoopFactory(absl::GetFlag(FLAGS_node));
std::unique_ptr<aos::EventLoop> image_loop = node->MakeEventLoop("image");
image_loop->MakeWatcher(
- "/" + FLAGS_node + "/" + FLAGS_channel,
+ "/" + absl::GetFlag(FLAGS_node) + "/" + absl::GetFlag(FLAGS_channel),
[](const frc971::vision::CameraImage &msg) {
cv::Mat color_image(cv::Size(msg.cols(), msg.rows()), CV_8UC2,
(void *)msg.data()->data());
diff --git a/frc971/vision/intrinsics_calibration.cc b/frc971/vision/intrinsics_calibration.cc
index e0ec38c..eb5605b 100644
--- a/frc971/vision/intrinsics_calibration.cc
+++ b/frc971/vision/intrinsics_calibration.cc
@@ -1,6 +1,7 @@
#include <cmath>
#include <regex>
+#include "absl/flags/flag.h"
#include "absl/strings/str_format.h"
#include <opencv2/calib3d.hpp>
#include <opencv2/highgui/highgui.hpp>
@@ -14,32 +15,35 @@
#include "frc971/vision/intrinsics_calibration_lib.h"
// TODO: Would be nice to remove this, but it depends on year-by-year Constants
-DEFINE_string(base_intrinsics, "",
- "Intrinsics to use for estimating board pose prior to solving "
- "for the new intrinsics.");
-DEFINE_string(calibration_folder, ".", "Folder to place calibration files.");
-DEFINE_string(camera_id, "", "Camera ID in format YY-NN-- year and number.");
-DEFINE_string(channel, "/camera", "Camera channel to use");
-DEFINE_string(config, "aos_config.json", "Path to the config file to use.");
-DEFINE_string(cpu_name, "", "Pi/Orin name to calibrate.");
-DEFINE_bool(display_undistorted, false,
- "If true, display the undistorted image.");
+ABSL_FLAG(std::string, base_intrinsics, "",
+ "Intrinsics to use for estimating board pose prior to solving "
+ "for the new intrinsics.");
+ABSL_FLAG(std::string, calibration_folder, ".",
+ "Folder to place calibration files.");
+ABSL_FLAG(std::string, camera_id, "",
+ "Camera ID in format YY-NN-- year and number.");
+ABSL_FLAG(std::string, channel, "/camera", "Camera channel to use");
+ABSL_FLAG(std::string, config, "aos_config.json",
+ "Path to the config file to use.");
+ABSL_FLAG(std::string, cpu_name, "", "Pi/Orin name to calibrate.");
+ABSL_FLAG(bool, display_undistorted, false,
+ "If true, display the undistorted image.");
namespace frc971::vision {
namespace {
void Main() {
aos::FlatbufferDetachedBuffer<aos::Configuration> config =
- aos::configuration::ReadConfig(FLAGS_config);
+ aos::configuration::ReadConfig(absl::GetFlag(FLAGS_config));
aos::ShmEventLoop event_loop(&config.message());
- std::string hostname = FLAGS_cpu_name;
+ std::string hostname = absl::GetFlag(FLAGS_cpu_name);
if (hostname == "") {
hostname = aos::network::GetHostname();
LOG(INFO) << "Using pi/orin name from hostname as " << hostname;
}
- CHECK(!FLAGS_base_intrinsics.empty())
+ CHECK(!absl::GetFlag(FLAGS_base_intrinsics).empty())
<< "Need a base intrinsics json to use to auto-capture images when the "
"camera moves.";
std::unique_ptr<aos::ExitHandle> exit_handle = event_loop.MakeExitHandle();
@@ -54,18 +58,19 @@
std::string camera_name = absl::StrCat(
"/", aos::network::ParsePiOrOrin(hostname).value(),
std::to_string(aos::network::ParsePiOrOrinNumber(hostname).value()),
- FLAGS_channel);
+ absl::GetFlag(FLAGS_channel));
// THIS IS A HACK FOR 2024, since we call Orin2 "Imu"
if (aos::network::ParsePiOrOrin(hostname).value() == "orin" &&
aos::network::ParsePiOrOrinNumber(hostname).value() == 2) {
LOG(INFO) << "\nHACK for 2024: Renaming orin2 to imu\n";
- camera_name = absl::StrCat("/imu", FLAGS_channel);
+ camera_name = absl::StrCat("/imu", absl::GetFlag(FLAGS_channel));
}
- IntrinsicsCalibration extractor(&event_loop, hostname, camera_name,
- FLAGS_camera_id, FLAGS_base_intrinsics,
- FLAGS_display_undistorted,
- FLAGS_calibration_folder, exit_handle.get());
+ IntrinsicsCalibration extractor(
+ &event_loop, hostname, camera_name, absl::GetFlag(FLAGS_camera_id),
+ absl::GetFlag(FLAGS_base_intrinsics),
+ absl::GetFlag(FLAGS_display_undistorted),
+ absl::GetFlag(FLAGS_calibration_folder), exit_handle.get());
event_loop.Run();
diff --git a/frc971/vision/intrinsics_calibration_lib.cc b/frc971/vision/intrinsics_calibration_lib.cc
index 5584ed7..47e6929 100644
--- a/frc971/vision/intrinsics_calibration_lib.cc
+++ b/frc971/vision/intrinsics_calibration_lib.cc
@@ -1,6 +1,9 @@
#include "frc971/vision/intrinsics_calibration_lib.h"
-DECLARE_bool(visualize);
+#include "absl/flags/declare.h"
+#include "absl/flags/flag.h"
+
+ABSL_DECLARE_FLAG(bool, visualize);
namespace frc971::vision {
@@ -49,13 +52,13 @@
calibration_folder_(calibration_folder),
exit_handle_(exit_handle),
exit_collection_(false) {
- if (!FLAGS_visualize) {
+ if (!absl::GetFlag(FLAGS_visualize)) {
// The only way to exit into the calibration routines is by hitting "q"
// while visualization is running. The event_loop doesn't pause enough
// to handle ctrl-c exit requests
LOG(INFO) << "Setting visualize to true, since currently the intrinsics "
"only works this way";
- FLAGS_visualize = true;
+ absl::SetFlag(&FLAGS_visualize, true);
}
LOG(INFO) << "Hostname is: " << hostname_ << " and camera channel is "
<< camera_channel_;
@@ -73,7 +76,7 @@
std::vector<std::vector<cv::Point2f>> charuco_corners, bool valid,
std::vector<Eigen::Vector3d> rvecs_eigen,
std::vector<Eigen::Vector3d> tvecs_eigen) {
- if (FLAGS_visualize) {
+ if (absl::GetFlag(FLAGS_visualize)) {
// Reduce resolution displayed on remote viewer to prevent lag
cv::resize(rgb_image, rgb_image,
cv::Size(rgb_image.cols / 2, rgb_image.rows / 2));
diff --git a/frc971/vision/media_device.cc b/frc971/vision/media_device.cc
index 46f3419..c0f0ec4 100644
--- a/frc971/vision/media_device.cc
+++ b/frc971/vision/media_device.cc
@@ -13,9 +13,10 @@
#include <string_view>
#include <vector>
+#include "absl/log/check.h"
+#include "absl/log/log.h"
#include "absl/strings/str_cat.h"
#include "absl/strings/str_split.h"
-#include "glog/logging.h"
#include "aos/scoped/scoped_fd.h"
#include "aos/util/file.h"
diff --git a/frc971/vision/media_device.h b/frc971/vision/media_device.h
index 31d0882..424a363 100644
--- a/frc971/vision/media_device.h
+++ b/frc971/vision/media_device.h
@@ -12,7 +12,8 @@
#include <string_view>
#include <vector>
-#include "glog/logging.h"
+#include "absl/log/check.h"
+#include "absl/log/log.h"
#include "aos/scoped/scoped_fd.h"
diff --git a/frc971/vision/modify_extrinsics.cc b/frc971/vision/modify_extrinsics.cc
index d021744..355f790 100644
--- a/frc971/vision/modify_extrinsics.cc
+++ b/frc971/vision/modify_extrinsics.cc
@@ -4,6 +4,7 @@
#include "Eigen/Dense"
#include "Eigen/Geometry"
+#include "absl/flags/flag.h"
#include "absl/strings/str_format.h"
#include "aos/configuration.h"
@@ -23,28 +24,30 @@
// By default, writes to /tmp, but will write to calib_files folder if
// full path is given and calibration_folder is blank
-DEFINE_string(orig_calib_file, "",
- "Intrinsics to use for estimating board pose prior to solving "
- "for the new intrinsics.");
-DEFINE_string(calibration_folder, "/tmp", "Folder to place calibration files.");
-DEFINE_string(node_name, "",
- "Node name to use, e.g. orin1, imu; unchanged if blank");
-DEFINE_int32(team_number, -1, "Team number to use; unchanged if -1");
-DEFINE_int32(camera_number, -1, "Camera number to use; unchanged if -1");
+ABSL_FLAG(std::string, orig_calib_file, "",
+ "Intrinsics to use for estimating board pose prior to solving "
+ "for the new intrinsics.");
+ABSL_FLAG(std::string, calibration_folder, "/tmp",
+ "Folder to place calibration files.");
+ABSL_FLAG(std::string, node_name, "",
+ "Node name to use, e.g. orin1, imu; unchanged if blank");
+ABSL_FLAG(int32_t, team_number, -1, "Team number to use; unchanged if -1");
+ABSL_FLAG(int32_t, camera_number, -1, "Camera number to use; unchanged if -1");
-DEFINE_bool(set_extrinsics, true, "Set to false to ignore extrinsic data");
-DEFINE_bool(use_inches, true,
- "Whether to use inches as units (meters if false)");
-DEFINE_bool(use_degrees, true,
- "Whether to use degrees as units (radians if false)");
-DEFINE_double(camera_x, 0.0, "x location of camera");
-DEFINE_double(camera_y, 0.0, "y location of camera");
-DEFINE_double(camera_z, 0.0, "z location of camera");
+ABSL_FLAG(bool, set_extrinsics, true, "Set to false to ignore extrinsic data");
+ABSL_FLAG(bool, use_inches, true,
+ "Whether to use inches as units (meters if false)");
+ABSL_FLAG(bool, use_degrees, true,
+ "Whether to use degrees as units (radians if false)");
+ABSL_FLAG(double, camera_x, 0.0, "x location of camera");
+ABSL_FLAG(double, camera_y, 0.0, "y location of camera");
+ABSL_FLAG(double, camera_z, 0.0, "z location of camera");
// Don't currently allow for roll of cameras
-DEFINE_double(camera_yaw, 0.0, "yaw of camera about robot z axis");
-DEFINE_double(camera_pitch, 0.0, "pitch of camera relative to robot y axis");
+ABSL_FLAG(double, camera_yaw, 0.0, "yaw of camera about robot z axis");
+ABSL_FLAG(double, camera_pitch, 0.0,
+ "pitch of camera relative to robot y axis");
// TODO: This could be done by setting the pixel size and using the intrinsics
-DEFINE_double(focal_length, 0.002, "Focal length in meters");
+ABSL_FLAG(double, focal_length, 0.002, "Focal length in meters");
namespace frc971::vision {
namespace {
@@ -74,30 +77,34 @@
// If we're told to set the extrinsics, clear old and add in new
flatbuffers::Offset<calibration::TransformationMatrix>
fixed_extrinsics_offset;
- if (FLAGS_set_extrinsics) {
+ if (absl::GetFlag(FLAGS_set_extrinsics)) {
cal_copy.mutable_message()->clear_fixed_extrinsics();
Eigen::Affine3d extrinsic_matrix;
// Convert to metric
- double translation_scale = (FLAGS_use_inches ? 0.0254 : 1.0);
- Eigen::Translation3d translation(FLAGS_camera_x * translation_scale,
- FLAGS_camera_y * translation_scale,
- FLAGS_camera_z * translation_scale);
+ double translation_scale = (absl::GetFlag(FLAGS_use_inches) ? 0.0254 : 1.0);
+ Eigen::Translation3d translation(
+ absl::GetFlag(FLAGS_camera_x) * translation_scale,
+ absl::GetFlag(FLAGS_camera_y) * translation_scale,
+ absl::GetFlag(FLAGS_camera_z) * translation_scale);
// convert to radians
- double angle_scale = (FLAGS_use_degrees ? M_PI / 180.0 : 1.0);
+ double angle_scale =
+ (absl::GetFlag(FLAGS_use_degrees) ? M_PI / 180.0 : 1.0);
// The rotation that takes robot coordinates (x forward, z up) to camera
// coordiantes (z forward, x right)
Eigen::Quaterniond R_robo_cam =
Eigen::AngleAxisd(-M_PI / 2.0, Eigen::Vector3d::UnitZ()) *
Eigen::AngleAxisd(-M_PI / 2.0, Eigen::Vector3d::UnitX());
- Eigen::AngleAxisd pitchAngle(FLAGS_camera_pitch * angle_scale,
- Eigen::Vector3d::UnitY());
- Eigen::AngleAxisd yawAngle(FLAGS_camera_yaw * angle_scale,
+ Eigen::AngleAxisd pitchAngle(
+ absl::GetFlag(FLAGS_camera_pitch) * angle_scale,
+ Eigen::Vector3d::UnitY());
+ Eigen::AngleAxisd yawAngle(absl::GetFlag(FLAGS_camera_yaw) * angle_scale,
Eigen::Vector3d::UnitZ());
Eigen::Quaterniond rotation = yawAngle * pitchAngle * R_robo_cam;
Eigen::Vector3d focal_length_offset =
- (rotation * Eigen::Translation3d(0.0, 0.0, FLAGS_focal_length))
+ (rotation *
+ Eigen::Translation3d(0.0, 0.0, absl::GetFlag(FLAGS_focal_length)))
.translation();
translation = translation * Eigen::Translation3d(focal_length_offset);
@@ -113,7 +120,7 @@
calibration::CameraCalibration::Builder camera_calibration_builder(fbb);
camera_calibration_builder.add_node_name(node_name_offset);
camera_calibration_builder.add_team_number(team_number);
- if (FLAGS_set_extrinsics) {
+ if (absl::GetFlag(FLAGS_set_extrinsics)) {
camera_calibration_builder.add_fixed_extrinsics(fixed_extrinsics_offset);
}
camera_calibration_builder.add_camera_number(camera_number);
@@ -136,15 +143,15 @@
orig_calib_filename);
// Populate the new variables from command-line or from base_calibration
- std::string node_name =
- (FLAGS_node_name == "" ? base_calibration.message().node_name()->str()
- : FLAGS_node_name);
- int team_number =
- (FLAGS_team_number == -1 ? base_calibration.message().team_number()
- : FLAGS_team_number);
- int camera_number =
- (FLAGS_camera_number == -1 ? base_calibration.message().camera_number()
- : FLAGS_camera_number);
+ std::string node_name = (absl::GetFlag(FLAGS_node_name) == ""
+ ? base_calibration.message().node_name()->str()
+ : absl::GetFlag(FLAGS_node_name));
+ int team_number = (absl::GetFlag(FLAGS_team_number) == -1
+ ? base_calibration.message().team_number()
+ : absl::GetFlag(FLAGS_team_number));
+ int camera_number = (absl::GetFlag(FLAGS_camera_number) == -1
+ ? base_calibration.message().camera_number()
+ : absl::GetFlag(FLAGS_camera_number));
aos::FlatbufferDetachedBuffer<calibration::CameraCalibration>
new_calibration = BuildCalibration(&base_calibration.message(), node_name,
camera_number, team_number);
@@ -158,9 +165,9 @@
std::string camera_id = base_calibration.message().camera_id()->str();
const std::string dirname =
- (FLAGS_calibration_folder == ""
+ (absl::GetFlag(FLAGS_calibration_folder) == ""
? std::filesystem::path(orig_calib_filename).parent_path().string()
- : FLAGS_calibration_folder);
+ : absl::GetFlag(FLAGS_calibration_folder));
const std::string new_calib_filename = frc971::vision::CalibrationFilename(
dirname, node_name.c_str(), team_number, camera_number, camera_id.c_str(),
time_ss.str());
diff --git a/frc971/vision/target_mapper.cc b/frc971/vision/target_mapper.cc
index f245f37..36f0a29 100644
--- a/frc971/vision/target_mapper.cc
+++ b/frc971/vision/target_mapper.cc
@@ -6,25 +6,26 @@
#include "frc971/vision/ceres/pose_graph_3d_error_term.h"
#include "frc971/vision/geometry.h"
-DEFINE_uint64(max_num_iterations, 100,
- "Maximum number of iterations for the ceres solver");
-DEFINE_double(distortion_noise_scalar, 1.0,
- "Scale the target pose distortion factor by this when computing "
- "the noise.");
-DEFINE_int32(
- frozen_target_id, 1,
+ABSL_FLAG(uint64_t, max_num_iterations, 100,
+ "Maximum number of iterations for the ceres solver");
+ABSL_FLAG(double, distortion_noise_scalar, 1.0,
+ "Scale the target pose distortion factor by this when computing "
+ "the noise.");
+ABSL_FLAG(
+ int32_t, frozen_target_id, 1,
"Freeze the pose of this target so the map can have one fixed point.");
-DEFINE_int32(min_target_id, 1, "Minimum target id to solve for");
-DEFINE_int32(max_target_id, 8, "Maximum target id to solve for");
-DEFINE_bool(visualize_solver, false, "If true, visualize the solving process.");
+ABSL_FLAG(int32_t, min_target_id, 1, "Minimum target id to solve for");
+ABSL_FLAG(int32_t, max_target_id, 8, "Maximum target id to solve for");
+ABSL_FLAG(bool, visualize_solver, false,
+ "If true, visualize the solving process.");
// This does seem like a strict threshold for a constaint to be considered an
// outlier, but most inliers were very close together and this is what worked
// best for map solving.
-DEFINE_double(outlier_std_devs, 1.0,
- "Number of standard deviations above average error needed for a "
- "constraint to be considered an outlier and get removed.");
-DEFINE_bool(do_map_fitting, false,
- "Whether to do a final fit of the solved map to the original map");
+ABSL_FLAG(double, outlier_std_devs, 1.0,
+ "Number of standard deviations above average error needed for a "
+ "constraint to be considered an outlier and get removed.");
+ABSL_FLAG(bool, do_map_fitting, false,
+ "Whether to do a final fit of the solved map to the original map");
namespace frc971::vision {
Eigen::Affine3d PoseUtils::Pose3dToAffine3d(
@@ -192,14 +193,16 @@
Q_vision(kOrientation3, kOrientation3) = std::pow(0.02, 2);
// Add uncertainty for the 2 vision measurements (1 at start and 1 at end)
- P += Q_vision * std::pow(detection_start.distance_from_camera *
- (1.0 + FLAGS_distortion_noise_scalar *
- detection_start.distortion_factor),
- 2);
- P += Q_vision * std::pow(detection_end.distance_from_camera *
- (1.0 + FLAGS_distortion_noise_scalar *
- detection_end.distortion_factor),
- 2);
+ P += Q_vision *
+ std::pow(detection_start.distance_from_camera *
+ (1.0 + absl::GetFlag(FLAGS_distortion_noise_scalar) *
+ detection_start.distortion_factor),
+ 2);
+ P += Q_vision *
+ std::pow(detection_end.distance_from_camera *
+ (1.0 + absl::GetFlag(FLAGS_distortion_noise_scalar) *
+ detection_end.distortion_factor),
+ 2);
}
return P.inverse();
@@ -381,16 +384,17 @@
// algorithm has internal damping which mitigates this issue, but it is
// better to properly constrain the gauge freedom. This can be done by
// setting one of the poses as constant so the optimizer cannot change it.
- CHECK_NE(poses->count(FLAGS_frozen_target_id), 0ul)
- << "Got no poses for frozen target id " << FLAGS_frozen_target_id;
- CHECK_GE(FLAGS_frozen_target_id, min_constraint_id)
- << "target to freeze index " << FLAGS_frozen_target_id
+ CHECK_NE(poses->count(absl::GetFlag(FLAGS_frozen_target_id)), 0ul)
+ << "Got no poses for frozen target id "
+ << absl::GetFlag(FLAGS_frozen_target_id);
+ CHECK_GE(absl::GetFlag(FLAGS_frozen_target_id), min_constraint_id)
+ << "target to freeze index " << absl::GetFlag(FLAGS_frozen_target_id)
<< " must be in range of constraints, > " << min_constraint_id;
- CHECK_LE(FLAGS_frozen_target_id, max_constraint_id)
- << "target to freeze index " << FLAGS_frozen_target_id
+ CHECK_LE(absl::GetFlag(FLAGS_frozen_target_id), max_constraint_id)
+ << "target to freeze index " << absl::GetFlag(FLAGS_frozen_target_id)
<< " must be in range of constraints, < " << max_constraint_id;
ceres::examples::MapOfPoses::iterator pose_start_iter =
- poses->find(FLAGS_frozen_target_id);
+ poses->find(absl::GetFlag(FLAGS_frozen_target_id));
CHECK(pose_start_iter != poses->end()) << "There are no poses.";
problem->SetParameterBlockConstant(pose_start_iter->second.p.data());
problem->SetParameterBlockConstant(pose_start_iter->second.q.coeffs().data());
@@ -401,7 +405,8 @@
// Set up robot visualization.
vis_robot_.ClearImage();
- const size_t num_targets = FLAGS_max_target_id - FLAGS_min_target_id;
+ const size_t num_targets =
+ absl::GetFlag(FLAGS_max_target_id) - absl::GetFlag(FLAGS_min_target_id);
// Translation and rotation error for each target
const size_t num_residuals = num_targets * 6;
// Set up the only cost function (also known as residual). This uses
@@ -470,7 +475,7 @@
CHECK(problem != nullptr);
ceres::Solver::Options options;
- options.max_num_iterations = FLAGS_max_num_iterations;
+ options.max_num_iterations = absl::GetFlag(FLAGS_max_num_iterations);
options.linear_solver_type = ceres::SPARSE_NORMAL_CHOLESKY;
options.minimizer_progress_to_stdout = false;
@@ -489,7 +494,7 @@
&target_pose_problem_1);
CHECK(SolveOptimizationProblem(&target_pose_problem_1))
<< "The target pose solve 1 was not successful, exiting.";
- if (FLAGS_visualize_solver) {
+ if (absl::GetFlag(FLAGS_visualize_solver)) {
LOG(INFO) << "Displaying constraint graph before removing outliers";
DisplayConstraintGraph();
DisplaySolvedVsInitial();
@@ -503,13 +508,13 @@
&target_pose_problem_2);
CHECK(SolveOptimizationProblem(&target_pose_problem_2))
<< "The target pose solve 2 was not successful, exiting.";
- if (FLAGS_visualize_solver) {
+ if (absl::GetFlag(FLAGS_visualize_solver)) {
LOG(INFO) << "Displaying constraint graph before removing outliers";
DisplayConstraintGraph();
DisplaySolvedVsInitial();
}
- if (FLAGS_do_map_fitting) {
+ if (absl::GetFlag(FLAGS_do_map_fitting)) {
LOG(INFO) << "Solving the overall map's best alignment to the previous map";
ceres::Problem map_fitting_problem(
{.loss_function_ownership = ceres::DO_NOT_TAKE_OWNERSHIP});
@@ -523,14 +528,15 @@
LOG(INFO) << "H_frozen_actual: "
<< PoseUtils::Affine3dToPose3d(H_frozen_actual);
- auto H_world_frozen =
- PoseUtils::Pose3dToAffine3d(target_poses_[FLAGS_frozen_target_id]);
+ auto H_world_frozen = PoseUtils::Pose3dToAffine3d(
+ target_poses_[absl::GetFlag(FLAGS_frozen_target_id)]);
auto H_world_frozenactual = H_world_frozen * H_frozen_actual;
// Offset the solved poses to become the actual ones
for (auto &[id, pose] : target_poses_) {
// Don't offset targets we didn't solve for
- if (id < FLAGS_min_target_id || id > FLAGS_max_target_id) {
+ if (id < absl::GetFlag(FLAGS_min_target_id) ||
+ id > absl::GetFlag(FLAGS_max_target_id)) {
continue;
}
@@ -553,10 +559,10 @@
aos::util::WriteStringToFileOrDie(output_path, map_json);
}
- for (TargetId id_start = FLAGS_min_target_id; id_start < FLAGS_max_target_id;
- id_start++) {
- for (TargetId id_end = id_start + 1; id_end <= FLAGS_max_target_id;
- id_end++) {
+ for (TargetId id_start = absl::GetFlag(FLAGS_min_target_id);
+ id_start < absl::GetFlag(FLAGS_max_target_id); id_start++) {
+ for (TargetId id_end = id_start + 1;
+ id_end <= absl::GetFlag(FLAGS_max_target_id); id_end++) {
auto H_start_end =
PoseUtils::Pose3dToAffine3d(target_poses_.at(id_start)).inverse() *
PoseUtils::Pose3dToAffine3d(target_poses_.at(id_end));
@@ -622,17 +628,19 @@
<< PoseUtils::Affine3dToPose3d(ScalarAffineToDouble(H_frozen_actual));
Affine3s H_world_frozen =
- PoseUtils::Pose3dToAffine3d(target_poses_.at(FLAGS_frozen_target_id))
+ PoseUtils::Pose3dToAffine3d(
+ target_poses_.at(absl::GetFlag(FLAGS_frozen_target_id)))
.cast<S>();
Affine3s H_world_frozenactual = H_world_frozen * H_frozen_actual;
size_t residual_index = 0;
- if (FLAGS_visualize_solver) {
+ if (absl::GetFlag(FLAGS_visualize_solver)) {
vis_robot_.ClearImage();
}
for (const auto &[id, solved_pose] : target_poses_) {
- if (id < FLAGS_min_target_id || id > FLAGS_max_target_id) {
+ if (id < absl::GetFlag(FLAGS_min_target_id) ||
+ id > absl::GetFlag(FLAGS_max_target_id)) {
continue;
}
@@ -669,7 +677,7 @@
residual[residual_index++] =
kRotationScalar * R_ideal_actual.angle() * R_ideal_actual.axis().z();
- if (FLAGS_visualize_solver) {
+ if (absl::GetFlag(FLAGS_visualize_solver)) {
LOG(INFO) << std::to_string(id) + std::string("-est") << " at "
<< ScalarAffineToDouble(H_world_actual).matrix();
vis_robot_.DrawFrameAxes(ScalarAffineToDouble(H_world_actual),
@@ -679,7 +687,7 @@
std::to_string(id), cv::Scalar(255, 255, 255));
}
}
- if (FLAGS_visualize_solver) {
+ if (absl::GetFlag(FLAGS_visualize_solver)) {
cv::imshow("Target maps", vis_robot_.image_);
cv::waitKey(0);
}
@@ -763,12 +771,12 @@
// Remove constraints with errors significantly above
// the average
if (err.distance > stats_with_outliers_.avg_err.distance +
- FLAGS_outlier_std_devs *
+ absl::GetFlag(FLAGS_outlier_std_devs) *
stats_with_outliers_.std_dev.distance) {
return true;
}
if (err.angle > stats_with_outliers_.avg_err.angle +
- FLAGS_outlier_std_devs *
+ absl::GetFlag(FLAGS_outlier_std_devs) *
stats_with_outliers_.std_dev.angle) {
return true;
}
@@ -812,14 +820,15 @@
LOG(INFO) << "Dumping target constraints to " << path;
std::ofstream fout(path.data());
for (const auto &constraint : target_constraints_) {
- fout << constraint << std::endl;
+ fout << absl::StrCat("", constraint) << std::endl;
}
fout.flush();
fout.close();
}
void TargetMapper::PrintDiffs() const {
- for (int id = FLAGS_min_target_id; id <= FLAGS_max_target_id; id++) {
+ for (int id = absl::GetFlag(FLAGS_min_target_id);
+ id <= absl::GetFlag(FLAGS_max_target_id); id++) {
Eigen::Affine3d H_world_ideal =
PoseUtils::Pose3dToAffine3d(ideal_target_poses_.at(id));
Eigen::Affine3d H_world_solved =
@@ -839,20 +848,3 @@
}
} // namespace frc971::vision
-
-std::ostream &operator<<(std::ostream &os, ceres::examples::Pose3d pose) {
- auto rpy = frc971::vision::PoseUtils::QuaternionToEulerAngles(pose.q);
- os << absl::StrFormat(
- "{x: %.3f, y: %.3f, z: %.3f, roll: %.3f, pitch: "
- "%.3f, yaw: %.3f}",
- pose.p(0), pose.p(1), pose.p(2), rpy(0), rpy(1), rpy(2));
- return os;
-}
-
-std::ostream &operator<<(std::ostream &os,
- ceres::examples::Constraint3d constraint) {
- os << absl::StrFormat("{id_begin: %d, id_end: %d, pose: ",
- constraint.id_begin, constraint.id_end)
- << constraint.t_be << "}";
- return os;
-}
diff --git a/frc971/vision/target_mapper.h b/frc971/vision/target_mapper.h
index d1d122e..cae1c69 100644
--- a/frc971/vision/target_mapper.h
+++ b/frc971/vision/target_mapper.h
@@ -3,6 +3,7 @@
#include <unordered_map>
+#include "absl/strings/str_format.h"
#include "ceres/ceres.h"
#include "aos/events/simulated_event_loop.h"
@@ -215,8 +216,23 @@
} // namespace frc971::vision
-std::ostream &operator<<(std::ostream &os, ceres::examples::Pose3d pose);
-std::ostream &operator<<(std::ostream &os,
- ceres::examples::Constraint3d constraint);
+namespace ceres::examples {
+template <typename Sink>
+void AbslStringify(Sink &sink, ceres::examples::Pose3d pose) {
+ auto rpy = frc971::vision::PoseUtils::QuaternionToEulerAngles(pose.q);
+ absl::Format(&sink,
+ "{x: %.3f, y: %.3f, z: %.3f, roll: %.3f, pitch: "
+ "%.3f, yaw: %.3f}",
+ pose.p(0), pose.p(1), pose.p(2), rpy(0), rpy(1), rpy(2));
+}
+
+template <typename Sink>
+void AbslStringify(Sink &sink, ceres::examples::Constraint3d constraint) {
+ absl::Format(&sink, "{id_begin: %d, id_end: %d, pose: ", constraint.id_begin,
+ constraint.id_end);
+ AbslStringify(sink, constraint.t_be);
+ sink.Append("}");
+}
+} // namespace ceres::examples
#endif // FRC971_VISION_TARGET_MAPPER_H_
diff --git a/frc971/vision/target_mapper_test.cc b/frc971/vision/target_mapper_test.cc
index 8c6d37d..4e9db6f 100644
--- a/frc971/vision/target_mapper_test.cc
+++ b/frc971/vision/target_mapper_test.cc
@@ -2,7 +2,10 @@
#include <random>
-#include "glog/logging.h"
+#include "absl/flags/declare.h"
+#include "absl/flags/flag.h"
+#include "absl/log/check.h"
+#include "absl/log/log.h"
#include "gtest/gtest.h"
#include "aos/events/simulated_event_loop.h"
@@ -10,8 +13,8 @@
#include "aos/testing/random_seed.h"
#include "aos/util/math.h"
-DECLARE_int32(min_target_id);
-DECLARE_int32(max_target_id);
+ABSL_DECLARE_FLAG(int32_t, min_target_id);
+ABSL_DECLARE_FLAG(int32_t, max_target_id);
namespace frc971::vision {
@@ -342,8 +345,8 @@
}
TEST(TargetMapperTest, TwoTargetsOneConstraint) {
- FLAGS_min_target_id = 0;
- FLAGS_max_target_id = 1;
+ absl::SetFlag(&FLAGS_min_target_id, 0);
+ absl::SetFlag(&FLAGS_max_target_id, 1);
ceres::examples::MapOfPoses target_poses;
target_poses[0] = Make2dPose(5.0, 0.0, M_PI);
@@ -368,8 +371,8 @@
}
TEST(TargetMapperTest, TwoTargetsTwoConstraints) {
- FLAGS_min_target_id = 0;
- FLAGS_max_target_id = 1;
+ absl::SetFlag(&FLAGS_min_target_id, 0);
+ absl::SetFlag(&FLAGS_max_target_id, 1);
ceres::examples::MapOfPoses target_poses;
target_poses[0] = Make2dPose(5.0, 0.0, M_PI);
@@ -400,8 +403,8 @@
}
TEST(TargetMapperTest, TwoTargetsOneNoisyConstraint) {
- FLAGS_min_target_id = 0;
- FLAGS_max_target_id = 1;
+ absl::SetFlag(&FLAGS_min_target_id, 0);
+ absl::SetFlag(&FLAGS_max_target_id, 1);
ceres::examples::MapOfPoses target_poses;
target_poses[0] = Make2dPose(5.0, 0.0, M_PI);
@@ -486,8 +489,8 @@
// when freeing memory. For some reason this segfault occurs only in this test,
// but when running the test with gdb it doesn't occur...
TEST_F(ChargedUpTargetMapperTest, DISABLED_FieldCircleMotion) {
- FLAGS_min_target_id = 1;
- FLAGS_max_target_id = 8;
+ absl::SetFlag(&FLAGS_min_target_id, 1);
+ absl::SetFlag(&FLAGS_max_target_id, 8);
// Read target map
auto target_map_fbs = aos::JsonFileToFlatbuffer<TargetMap>(
diff --git a/frc971/vision/v4l2_reader.cc b/frc971/vision/v4l2_reader.cc
index 2d67c9c..e333dc5 100644
--- a/frc971/vision/v4l2_reader.cc
+++ b/frc971/vision/v4l2_reader.cc
@@ -6,8 +6,10 @@
#include <sys/stat.h>
#include <sys/types.h>
-DEFINE_bool(ignore_timestamps, false,
- "Don't require timestamps on images. Used to allow webcams");
+#include "absl/flags/flag.h"
+
+ABSL_FLAG(bool, ignore_timestamps, false,
+ "Don't require timestamps on images. Used to allow webcams");
namespace frc971::vision {
@@ -233,7 +235,7 @@
CHECK_EQ(ImageSize(), buffer.length);
}
CHECK(buffer.flags & V4L2_BUF_FLAG_TIMESTAMP_MONOTONIC);
- if (!FLAGS_ignore_timestamps) {
+ if (!absl::GetFlag(FLAGS_ignore_timestamps)) {
// Require that we have good timestamp on images
CHECK_EQ(buffer.flags & V4L2_BUF_FLAG_TSTAMP_SRC_MASK,
static_cast<uint32_t>(V4L2_BUF_FLAG_TSTAMP_SRC_EOF));
diff --git a/frc971/vision/v4l2_reader.h b/frc971/vision/v4l2_reader.h
index 4906d8e..01634e2 100644
--- a/frc971/vision/v4l2_reader.h
+++ b/frc971/vision/v4l2_reader.h
@@ -4,8 +4,8 @@
#include <array>
#include <string>
+#include "absl/log/check.h"
#include "absl/types/span.h"
-#include "glog/logging.h"
#include "aos/containers/ring_buffer.h"
#include "aos/events/epoll.h"
diff --git a/frc971/vision/vision_util_lib.cc b/frc971/vision/vision_util_lib.cc
index 45aa199..089f3ac 100644
--- a/frc971/vision/vision_util_lib.cc
+++ b/frc971/vision/vision_util_lib.cc
@@ -1,7 +1,8 @@
#include "frc971/vision/vision_util_lib.h"
+#include "absl/log/check.h"
+#include "absl/log/log.h"
#include "absl/strings/str_format.h"
-#include "glog/logging.h"
namespace frc971::vision {
diff --git a/frc971/vision/visualize_robot.cc b/frc971/vision/visualize_robot.cc
index d13668e..52a7695 100644
--- a/frc971/vision/visualize_robot.cc
+++ b/frc971/vision/visualize_robot.cc
@@ -1,6 +1,7 @@
#include "frc971/vision/visualize_robot.h"
-#include "glog/logging.h"
+#include "absl/log/check.h"
+#include "absl/log/log.h"
#include <opencv2/calib3d.hpp>
#include <opencv2/core/eigen.hpp>
#include <opencv2/highgui/highgui.hpp>
diff --git a/frc971/vision/visualize_robot_sample.cc b/frc971/vision/visualize_robot_sample.cc
index ad1d6b6..6e15f2f 100644
--- a/frc971/vision/visualize_robot_sample.cc
+++ b/frc971/vision/visualize_robot_sample.cc
@@ -1,7 +1,8 @@
#include <math.h>
#include "Eigen/Dense"
-#include "glog/logging.h"
+#include "absl/log/check.h"
+#include "absl/log/log.h"
#include <opencv2/aruco.hpp>
#include <opencv2/aruco/charuco.hpp>
#include <opencv2/calib3d.hpp>