Merge "Add a neutral variant to the PivotGoal"
diff --git a/WORKSPACE b/WORKSPACE
index c603581..0c98e3f 100644
--- a/WORKSPACE
+++ b/WORKSPACE
@@ -251,11 +251,11 @@
load("@com_grail_bazel_toolchain//toolchain:rules.bzl", "llvm", "llvm_toolchain")
-llvm_version = "13.0.0"
+llvm_version = "16.0.3"
llvm(
name = "llvm_k8",
- distribution = "clang+llvm-%s-x86_64-linux-gnu-ubuntu-16.04.tar.xz" % llvm_version,
+ distribution = "clang+llvm-%s-x86_64-linux-gnu-ubuntu-22.04.tar.xz" % llvm_version,
llvm_version = llvm_version,
)
diff --git a/aos/aos_dump.cc b/aos/aos_dump.cc
index 1dbcd44..18d2ce1 100644
--- a/aos/aos_dump.cc
+++ b/aos/aos_dump.cc
@@ -51,8 +51,6 @@
return 0;
}
- uint64_t message_count = 0;
-
aos::monotonic_clock::time_point next_send_time =
aos::monotonic_clock::min_time;
@@ -74,7 +72,6 @@
cli_info.event_loop->MakeRawFetcher(channel);
if (fetcher->Fetch()) {
printer.PrintMessage(channel, fetcher->context());
- ++message_count;
}
}
diff --git a/aos/events/event_loop.h b/aos/events/event_loop.h
index 1ed4337..827dd46 100644
--- a/aos/events/event_loop.h
+++ b/aos/events/event_loop.h
@@ -113,15 +113,15 @@
public:
using SharedSpan = std::shared_ptr<const absl::Span<const uint8_t>>;
- enum class [[nodiscard]] Error{
- // Represents success and no error
- kOk,
+ enum class [[nodiscard]] Error {
+ // Represents success and no error
+ kOk,
- // Error for messages on channels being sent faster than their
- // frequency and channel storage duration allow
- kMessagesSentTooFast,
- // Access to Redzone was attempted in Sender Queue
- kInvalidRedzone,
+ // Error for messages on channels being sent faster than their
+ // frequency and channel storage duration allow
+ kMessagesSentTooFast,
+ // Access to Redzone was attempted in Sender Queue
+ kInvalidRedzone,
};
RawSender(EventLoop *event_loop, const Channel *channel);
diff --git a/aos/events/logging/multinode_logger_test_lib.cc b/aos/events/logging/multinode_logger_test_lib.cc
index 64dd1cd..9b3e00c 100644
--- a/aos/events/logging/multinode_logger_test_lib.cc
+++ b/aos/events/logging/multinode_logger_test_lib.cc
@@ -544,14 +544,12 @@
}
std::vector<std::tuple<std::string, std::string, int>> result;
- int channel = 0;
for (size_t i = 0; i < counts.size(); ++i) {
if (counts[i] != 0) {
const Channel *channel = config->channels()->Get(i);
result.push_back(std::make_tuple(channel->name()->str(),
channel->type()->str(), counts[i]));
}
- ++channel;
}
return result;
diff --git a/aos/logging/logging.h b/aos/logging/logging.h
index c71e5c4..8d83037 100644
--- a/aos/logging/logging.h
+++ b/aos/logging/logging.h
@@ -51,7 +51,7 @@
// It's currently using __PRETTY_FUNCTION__ because both GCC and Clang support
// that and it gives nicer results in C++ than the standard __func__ (which
// would also work).
-//#define LOG_CURRENT_FUNCTION __PRETTY_FUNCTION__
+// #define LOG_CURRENT_FUNCTION __PRETTY_FUNCTION__
#define LOG_CURRENT_FUNCTION __func__
#define LOG_SOURCENAME __FILE__
diff --git a/aos/network/message_bridge_client_lib.cc b/aos/network/message_bridge_client_lib.cc
index 19f3420..34f578e 100644
--- a/aos/network/message_bridge_client_lib.cc
+++ b/aos/network/message_bridge_client_lib.cc
@@ -62,7 +62,6 @@
const Node *my_node,
const Node *other_node) {
std::vector<bool> stream_reply_with_timestamp;
- int channel_index = 0;
for (const Channel *channel : *config->channels()) {
if (configuration::ChannelIsSendableOnNode(channel, other_node)) {
const Connection *connection =
@@ -78,7 +77,6 @@
configuration::ChannelMessageIsLoggedOnNode(channel, my_node));
}
}
- ++channel_index;
}
return stream_reply_with_timestamp;
diff --git a/aos/network/multinode_timestamp_filter.cc b/aos/network/multinode_timestamp_filter.cc
index 55b6b02..b4ebef4 100644
--- a/aos/network/multinode_timestamp_filter.cc
+++ b/aos/network/multinode_timestamp_filter.cc
@@ -1820,7 +1820,6 @@
}
if (filter_fps_.size() != 0 && !destructor) {
- size_t node_a_index = 0;
for (const auto &filters : filters_per_node_) {
for (const auto &filter : filters) {
while (true) {
@@ -1832,7 +1831,6 @@
WriteFilter(filter.filter, *sample);
}
}
- ++node_a_index;
}
}
diff --git a/aos/network/rawrtc.h b/aos/network/rawrtc.h
index f43cc5e..7908124 100644
--- a/aos/network/rawrtc.h
+++ b/aos/network/rawrtc.h
@@ -170,9 +170,7 @@
void Open();
// Returns the connection if Open has been called.
- struct rawrtc_peer_connection *connection() {
- return connection_;
- }
+ struct rawrtc_peer_connection *connection() { return connection_; }
private:
// Trampolines from C -> C++.
diff --git a/aos/network/sctp_client.h b/aos/network/sctp_client.h
index 06f6b15..9b9265e 100644
--- a/aos/network/sctp_client.h
+++ b/aos/network/sctp_client.h
@@ -44,9 +44,7 @@
void SetPriorityScheduler(sctp_assoc_t assoc_id);
// Remote to send to.
- struct sockaddr_storage sockaddr_remote() const {
- return sockaddr_remote_;
- }
+ struct sockaddr_storage sockaddr_remote() const { return sockaddr_remote_; }
void LogSctpStatus(sctp_assoc_t assoc_id);
diff --git a/compilers/yocto_orin_rootfs.BUILD b/compilers/yocto_orin_rootfs.BUILD
new file mode 100644
index 0000000..425b63a
--- /dev/null
+++ b/compilers/yocto_orin_rootfs.BUILD
@@ -0,0 +1,41 @@
+filegroup(
+ name = "sysroot_files",
+ srcs = glob(
+ include = [
+ "include/**",
+ "lib/**",
+ "lib64/**",
+ "usr/include/**",
+ "usr/lib/**",
+ "usr/lib64/**",
+ ],
+ exclude = [
+ "usr/share/**",
+ ],
+ ),
+ visibility = ["//visibility:public"],
+)
+
+cc_library(
+ name = "argus",
+ srcs = [
+ "usr/lib/libnvargus_socketclient.so",
+ ],
+ hdrs = glob(
+ include = ["usr/include/Argus/**"],
+ ),
+ includes = ["usr/include/Argus/utils/"],
+ visibility = ["//visibility:public"],
+)
+
+cc_library(
+ name = "eglstream",
+ srcs = [
+ #"usr/lib/libnvargus_socketclient.so",
+ ],
+ hdrs = glob(
+ include = ["usr/include/EGLStream/**"],
+ ),
+ includes = ["usr/include/EGLStream/"],
+ visibility = ["//visibility:public"],
+)
diff --git a/debian/BUILD.zlib.bazel b/debian/BUILD.zlib.bazel
index c0f81fa..c859eda 100644
--- a/debian/BUILD.zlib.bazel
+++ b/debian/BUILD.zlib.bazel
@@ -8,6 +8,8 @@
copts = [
"-w",
"-Dverbose=-1",
+ "-Wno-unused-but-set-variable",
+ "-Wno-implicit-function-declaration",
],
includes = [
".",
diff --git a/debian/boringssl.patch b/debian/boringssl.patch
index d12a7db..b7ac2ae 100644
--- a/debian/boringssl.patch
+++ b/debian/boringssl.patch
@@ -49,7 +49,7 @@
# Modern build environments should be able to set this to use atomic
# operations for reference counting rather than locks. However, it's
-@@ -86,17 +103,26 @@ posix_copts = [
+@@ -86,17 +103,29 @@ posix_copts = [
boringssl_copts = select({
":linux_x86_64": posix_copts,
":linux_ppc64le": posix_copts,
@@ -62,7 +62,10 @@
],
"//conditions:default": ["-DOPENSSL_NO_ASM"],
+}) + compiler_select({
-+ "clang": [],
++ "clang": [
++ "-Wno-unused-but-set-variable",
++ "-Wno-array-parameter",
++ ],
+ "gcc": [
+ "-Wno-array-parameter",
+ ],
diff --git a/debian/clapack.BUILD b/debian/clapack.BUILD
index 9ed7f82..c85e2df 100644
--- a/debian/clapack.BUILD
+++ b/debian/clapack.BUILD
@@ -295,7 +295,9 @@
"-Wno-unused-but-set-variable",
] + compiler_select({
"clang": [
+ "-Wno-deprecated-non-prototype",
"-Wno-self-assign",
+ "-Wno-unused-but-set-parameter",
],
"gcc": [
"-Wno-implicit-fallthrough",
diff --git a/debian/curl.BUILD b/debian/curl.BUILD
index c6c2d2c..a4f6526 100644
--- a/debian/curl.BUILD
+++ b/debian/curl.BUILD
@@ -426,6 +426,7 @@
"-Wno-cast-qual",
"-Wno-format-nonliteral",
"-Wno-tautological-type-limit-compare",
+ "-Wno-unused-but-set-variable",
],
}) + select({
":macos": [
diff --git a/debian/slycot.BUILD b/debian/slycot.BUILD
index 21c0df7..fb3f376 100644
--- a/debian/slycot.BUILD
+++ b/debian/slycot.BUILD
@@ -15,6 +15,7 @@
] + compiler_select({
"clang": [
"-Wno-unused-but-set-parameter",
+ "-Wno-deprecated-non-prototype",
],
"gcc": [
"-Wno-discarded-qualifiers",
diff --git a/frc971/control_loops/drivetrain/trajectory.h b/frc971/control_loops/drivetrain/trajectory.h
index e30ca98..b2cecc0 100644
--- a/frc971/control_loops/drivetrain/trajectory.h
+++ b/frc971/control_loops/drivetrain/trajectory.h
@@ -92,8 +92,8 @@
}
const StateFeedbackLoop<2, 2, 2, double, StateFeedbackHybridPlant<2, 2, 2>,
- HybridKalman<2, 2, 2>>
- &velocity_drivetrain() const {
+ HybridKalman<2, 2, 2>> &
+ velocity_drivetrain() const {
return *velocity_drivetrain_;
}
diff --git a/frc971/control_loops/hybrid_state_feedback_loop.h b/frc971/control_loops/hybrid_state_feedback_loop.h
index d8dbe72..79e7da7 100644
--- a/frc971/control_loops/hybrid_state_feedback_loop.h
+++ b/frc971/control_loops/hybrid_state_feedback_loop.h
@@ -120,8 +120,8 @@
return coefficients().U_max;
}
Scalar U_max(int i, int j) const { return U_max()(i, j); }
- const Eigen::Matrix<Scalar, number_of_inputs, number_of_states>
- &U_limit_coefficient() const {
+ const Eigen::Matrix<Scalar, number_of_inputs, number_of_states> &
+ U_limit_coefficient() const {
return coefficients().U_limit_coefficient;
}
Scalar U_limit_coefficient(int i, int j) const {
@@ -145,14 +145,14 @@
Scalar &mutable_Y(int i) { return mutable_Y()(i); }
const StateFeedbackHybridPlantCoefficients<number_of_states, number_of_inputs,
- number_of_outputs, Scalar>
- &coefficients(int index) const {
+ number_of_outputs, Scalar> &
+ coefficients(int index) const {
return *coefficients_[index];
}
const StateFeedbackHybridPlantCoefficients<number_of_states, number_of_inputs,
- number_of_outputs, Scalar>
- &coefficients() const {
+ number_of_outputs, Scalar> &
+ coefficients() const {
return *coefficients_[index_];
}
@@ -398,14 +398,14 @@
int index() const { return index_; }
const HybridKalmanCoefficients<number_of_states, number_of_inputs,
- number_of_outputs>
- &coefficients(int index) const {
+ number_of_outputs> &
+ coefficients(int index) const {
return *coefficients_[index];
}
const HybridKalmanCoefficients<number_of_states, number_of_inputs,
- number_of_outputs>
- &coefficients() const {
+ number_of_outputs> &
+ coefficients() const {
return *coefficients_[index_];
}
diff --git a/frc971/control_loops/profiled_subsystem.h b/frc971/control_loops/profiled_subsystem.h
index 1fa8ac2..2a15201 100644
--- a/frc971/control_loops/profiled_subsystem.h
+++ b/frc971/control_loops/profiled_subsystem.h
@@ -61,8 +61,9 @@
}
// Returns the controller.
- const StateFeedbackLoop<number_of_states, number_of_inputs, number_of_outputs>
- &controller() const {
+ const StateFeedbackLoop<number_of_states, number_of_inputs,
+ number_of_outputs> &
+ controller() const {
return *loop_;
}
diff --git a/frc971/control_loops/state_feedback_loop.h b/frc971/control_loops/state_feedback_loop.h
index 295beca..c5d05c1 100644
--- a/frc971/control_loops/state_feedback_loop.h
+++ b/frc971/control_loops/state_feedback_loop.h
@@ -144,8 +144,8 @@
return coefficients().U_max;
}
Scalar U_max(int i, int j = 0) const { return U_max()(i, j); }
- const Eigen::Matrix<Scalar, number_of_inputs, number_of_states>
- &U_limit_coefficient() const {
+ const Eigen::Matrix<Scalar, number_of_inputs, number_of_states> &
+ U_limit_coefficient() const {
return coefficients().U_limit_coefficient;
}
Scalar U_limit_coefficient(int i, int j) const {
@@ -173,14 +173,14 @@
size_t coefficients_size() const { return coefficients_.size(); }
const StateFeedbackPlantCoefficients<number_of_states, number_of_inputs,
- number_of_outputs, Scalar>
- &coefficients(int index) const {
+ number_of_outputs, Scalar> &
+ coefficients(int index) const {
return *coefficients_[index];
}
const StateFeedbackPlantCoefficients<number_of_states, number_of_inputs,
- number_of_outputs, Scalar>
- &coefficients() const {
+ number_of_outputs, Scalar> &
+ coefficients() const {
return *coefficients_[index_];
}
@@ -326,14 +326,14 @@
int index() const { return index_; }
const StateFeedbackControllerCoefficients<number_of_states, number_of_inputs,
- number_of_outputs, Scalar>
- &coefficients(int index) const {
+ number_of_outputs, Scalar> &
+ coefficients(int index) const {
return *coefficients_[index];
}
const StateFeedbackControllerCoefficients<number_of_states, number_of_inputs,
- number_of_outputs, Scalar>
- &coefficients() const {
+ number_of_outputs, Scalar> &
+ coefficients() const {
return *coefficients_[index_];
}
@@ -450,14 +450,14 @@
int index() const { return index_; }
const StateFeedbackObserverCoefficients<number_of_states, number_of_inputs,
- number_of_outputs, Scalar>
- &coefficients(int index) const {
+ number_of_outputs, Scalar> &
+ coefficients(int index) const {
return *coefficients_[index];
}
const StateFeedbackObserverCoefficients<number_of_states, number_of_inputs,
- number_of_outputs, Scalar>
- &coefficients() const {
+ number_of_outputs, Scalar> &
+ coefficients() const {
return *coefficients_[index_];
}
@@ -550,8 +550,8 @@
PlantType *mutable_plant() { return &plant_; }
const StateFeedbackController<number_of_states, number_of_inputs,
- number_of_outputs, Scalar>
- &controller() const {
+ number_of_outputs, Scalar> &
+ controller() const {
return controller_;
}
diff --git a/frc971/vision/charuco_lib.cc b/frc971/vision/charuco_lib.cc
index 82cc6b3..b8edcae 100644
--- a/frc971/vision/charuco_lib.cc
+++ b/frc971/vision/charuco_lib.cc
@@ -25,8 +25,10 @@
DEFINE_uint32(
min_charucos, 10,
"The mininum number of aruco targets in charuco board required to match.");
-DEFINE_uint32(min_id, 12, "Minimum valid charuco id");
-DEFINE_uint32(max_id, 15, "Minimum valid charuco id");
+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,
@@ -280,9 +282,21 @@
}
CharucoExtractor::CharucoExtractor(
+ const calibration::CameraCalibration *calibration,
+ const TargetType target_type)
+ : event_loop_(nullptr),
+ target_type_(target_type),
+ calibration_(CHECK_NOTNULL(calibration)) {
+ VLOG(2) << "Configuring CharucoExtractor without event_loop";
+ SetupTargetData();
+ VLOG(2) << "Camera matrix:\n" << calibration_.CameraIntrinsics();
+ VLOG(2) << "Distortion Coefficients:\n" << calibration_.CameraDistCoeffs();
+}
+
+CharucoExtractor::CharucoExtractor(
aos::EventLoop *event_loop,
- const calibration::CameraCalibration *calibration, TargetType target_type,
- std::string_view image_channel,
+ const calibration::CameraCalibration *calibration,
+ const TargetType target_type, std::string_view image_channel,
std::function<void(cv::Mat, monotonic_clock::time_point,
std::vector<cv::Vec4i>,
std::vector<std::vector<cv::Point2f>>, bool,
@@ -303,9 +317,34 @@
void CharucoExtractor::HandleImage(cv::Mat rgb_image,
const monotonic_clock::time_point eof) {
+ // Set up the variables we'll use in the callback function
+ bool valid = false;
+ // ids and corners for final, refined board / marker detections
+ // Using Vec4i type since it supports Charuco Diamonds
+ // And overloading it using 1st int in Vec4i for others target types
+ std::vector<cv::Vec4i> result_ids;
+ std::vector<std::vector<cv::Point2f>> result_corners;
+
+ // Return a list of poses; for Charuco Board there will be just one
+ std::vector<Eigen::Vector3d> rvecs_eigen;
+ std::vector<Eigen::Vector3d> tvecs_eigen;
+ ProcessImage(rgb_image, eof, event_loop_->monotonic_now(), result_ids,
+ result_corners, valid, rvecs_eigen, tvecs_eigen);
+
+ handle_charuco_(rgb_image, eof, result_ids, result_corners, valid,
+ rvecs_eigen, tvecs_eigen);
+}
+
+void CharucoExtractor::ProcessImage(
+ cv::Mat rgb_image, const monotonic_clock::time_point eof,
+ const monotonic_clock::time_point current_time,
+ std::vector<cv::Vec4i> &result_ids,
+ std::vector<std::vector<cv::Point2f>> &result_corners, bool &valid,
+ std::vector<Eigen::Vector3d> &rvecs_eigen,
+ std::vector<Eigen::Vector3d> &tvecs_eigen) {
const double age_double =
- std::chrono::duration_cast<std::chrono::duration<double>>(
- event_loop_->monotonic_now() - eof)
+ std::chrono::duration_cast<std::chrono::duration<double>>(current_time -
+ eof)
.count();
// Have found this useful if there is blurry / noisy images
@@ -318,22 +357,10 @@
cv::cvtColor(thresh, rgb_image, cv::COLOR_GRAY2RGB);
}
- // Set up the variables we'll use in the callback function
- bool valid = false;
- // Return a list of poses; for Charuco Board there will be just one
- std::vector<Eigen::Vector3d> rvecs_eigen;
- std::vector<Eigen::Vector3d> tvecs_eigen;
-
// ids and corners for initial aruco marker detections
std::vector<int> marker_ids;
std::vector<std::vector<cv::Point2f>> marker_corners;
- // ids and corners for final, refined board / marker detections
- // Using Vec4i type since it supports Charuco Diamonds
- // And overloading it using 1st int in Vec4i for others target types
- std::vector<cv::Vec4i> result_ids;
- std::vector<std::vector<cv::Point2f>> result_corners;
-
// Do initial marker detection; this is the same for all target types
cv::aruco::detectMarkers(rgb_image, dictionary_, marker_corners, marker_ids);
cv::aruco::drawDetectedMarkers(rgb_image, marker_corners, marker_ids);
@@ -429,11 +456,14 @@
square_length_ / marker_length_,
diamond_corners, diamond_ids);
- // Check that we have exactly one charuco diamond. For calibration, we
- // can constrain things so that this is the case
- if (diamond_ids.size() == 1) {
- // TODO<Jim>: Could probably make this check more general than requiring
- // range of ids
+ // Check that we have an acceptable number of diamonds detected.
+ // 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)) {
+ // 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];
@@ -446,14 +476,15 @@
cv::aruco::drawDetectedDiamonds(rgb_image, diamond_corners,
diamond_ids);
- // estimate pose for diamonds doesn't return valid, so marking true
+ // estimate pose for diamonds doesn't return valid, so marking
+ // true
valid = true;
std::vector<cv::Vec3d> rvecs, tvecs;
cv::aruco::estimatePoseSingleMarkers(
diamond_corners, square_length_, calibration_.CameraIntrinsics(),
calibration_.CameraDistCoeffs(), rvecs, tvecs);
- DrawTargetPoses(rgb_image, rvecs, tvecs);
+ DrawTargetPoses(rgb_image, rvecs, tvecs);
PackPoseResults(rvecs, tvecs, &rvecs_eigen, &tvecs_eigen);
result_ids = diamond_ids;
result_corners = diamond_corners;
@@ -463,12 +494,12 @@
} else {
if (diamond_ids.size() == 0) {
// OK to not see any markers sometimes
- VLOG(2)
- << "Found aruco markers, but no valid charuco diamond targets";
+ VLOG(2) << "Found aruco markers, but no valid charuco diamond "
+ "targets";
} else {
- // But should never detect multiple
- LOG(FATAL) << "Found multiple charuco diamond markers. Should only "
- "be one";
+ VLOG(2) << "Found too many number of diamond markers, which likely "
+ "means false positives were detected: "
+ << diamond_ids.size() << " > " << FLAGS_max_diamonds;
}
}
} else {
@@ -476,9 +507,6 @@
<< static_cast<uint8_t>(target_type_);
}
}
-
- handle_charuco_(rgb_image, eof, result_ids, result_corners, valid,
- rvecs_eigen, tvecs_eigen);
}
flatbuffers::Offset<foxglove::ImageAnnotations> BuildAnnotations(
diff --git a/frc971/vision/charuco_lib.h b/frc971/vision/charuco_lib.h
index 8af7b8c..2f274bb 100644
--- a/frc971/vision/charuco_lib.h
+++ b/frc971/vision/charuco_lib.h
@@ -93,6 +93,11 @@
// extracted from it.
class CharucoExtractor {
public:
+ // Setting up a constructor that doesn't require an event_loop, so we can call
+ // and get results back from ProcessImage directly
+ CharucoExtractor(const calibration::CameraCalibration *calibration,
+ const TargetType target_type);
+
// The callback takes the following arguments:
// cv::Mat -> image with overlays drawn on it.
// monotonic_clock::time_point -> Time on this node when this image was
@@ -117,7 +122,16 @@
std::vector<Eigen::Vector3d>)> &&handle_charuco_fn);
// Handles the image by detecting the charuco board in it.
- void HandleImage(cv::Mat rgb_image, aos::monotonic_clock::time_point eof);
+ void HandleImage(cv::Mat rgb_image,
+ const aos::monotonic_clock::time_point eof);
+
+ void ProcessImage(cv::Mat rgb_image,
+ const aos::monotonic_clock::time_point eof,
+ const aos::monotonic_clock::time_point current_time,
+ std::vector<cv::Vec4i> &result_ids,
+ std::vector<std::vector<cv::Point2f>> &result_corners,
+ bool &valid, std::vector<Eigen::Vector3d> &rvecs_eigen,
+ std::vector<Eigen::Vector3d> &tvecs_eigen);
// Returns the aruco dictionary in use.
cv::Ptr<cv::aruco::Dictionary> dictionary() const { return dictionary_; }
@@ -140,8 +154,8 @@
void DrawTargetPoses(cv::Mat rgb_image, std::vector<cv::Vec3d> rvecs,
std::vector<cv::Vec3d> tvecs);
- // Helper function to convert rotation (rvecs) and translation (tvecs) vectors
- // into Eigen vectors and store in corresponding vectors
+ // Helper function to convert rotation (rvecs) and translation (tvecs)
+ // vectors into Eigen vectors and store in corresponding vectors
void PackPoseResults(std::vector<cv::Vec3d> &rvecs,
std::vector<cv::Vec3d> &tvecs,
std::vector<Eigen::Vector3d> *rvecs_eigen,
@@ -184,7 +198,8 @@
const foxglove::PointsAnnotationType line_type =
foxglove::PointsAnnotationType::POINTS);
-// Creates a PointsAnnotation to build up ImageAnnotations with different types
+// Creates a PointsAnnotation to build up ImageAnnotations with different
+// types
flatbuffers::Offset<foxglove::PointsAnnotation> BuildPointsAnnotation(
flatbuffers::FlatBufferBuilder *fbb,
const aos::monotonic_clock::time_point monotonic_now,
diff --git a/frc971/vision/target_mapper.cc b/frc971/vision/target_mapper.cc
index c4af163..8077f16 100644
--- a/frc971/vision/target_mapper.cc
+++ b/frc971/vision/target_mapper.cc
@@ -303,6 +303,9 @@
ceres::LocalParameterization *quaternion_local_parameterization =
new ceres::EigenQuaternionParameterization;
+ int min_constraint_id = std::numeric_limits<int>::max();
+ int max_constraint_id = std::numeric_limits<int>::min();
+
for (ceres::examples::VectorOfConstraints::const_iterator constraints_iter =
constraints.begin();
constraints_iter != constraints.end(); ++constraints_iter) {
@@ -325,6 +328,15 @@
<< "Should have counted constraints for " << id_pair.first << "->"
<< id_pair.second;
+ VLOG(1) << "Adding constraint pair: " << id_pair.first << " and "
+ << id_pair.second;
+ // Store min & max id's; assumes first id < second id
+ if (id_pair.first < min_constraint_id) {
+ min_constraint_id = id_pair.first;
+ }
+ if (id_pair.second > max_constraint_id) {
+ max_constraint_id = id_pair.second;
+ }
// Normalize constraint cost by occurances
size_t constraint_count = constraint_counts_[id_pair];
// Scale all costs so the total cost comes out to more reasonable numbers
@@ -358,6 +370,12 @@
// 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
+ << " 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
+ << " must be in range of constraints, < " << max_constraint_id;
ceres::examples::MapOfPoses::iterator pose_start_iter =
poses->find(FLAGS_frozen_target_id);
CHECK(pose_start_iter != poses->end()) << "There are no poses.";
@@ -479,8 +497,8 @@
PoseUtils::Pose3dToAffine3d(target_poses_.at(id_start)).inverse() *
PoseUtils::Pose3dToAffine3d(target_poses_.at(id_end));
auto constraint = PoseUtils::Affine3dToPose3d(H_start_end);
- LOG(INFO) << id_start << "->" << id_end << ": " << constraint.p.norm()
- << " meters";
+ VLOG(1) << id_start << "->" << id_end << ": " << constraint.p.norm()
+ << " meters";
}
}
}
diff --git a/third_party/apriltag/common/workerpool.c b/third_party/apriltag/common/workerpool.c
index 29eccfc..a23832e 100644
--- a/third_party/apriltag/common/workerpool.c
+++ b/third_party/apriltag/common/workerpool.c
@@ -52,7 +52,7 @@
{
workerpool_t *wp = (workerpool_t*) p;
- int cnt = 0;
+ //int cnt = 0;
while (1) {
struct task *task;
@@ -63,13 +63,13 @@
// printf("%"PRId64" thread %d did %d\n", utime_now(), pthread_self(), cnt);
pthread_cond_broadcast(&wp->endcond);
pthread_cond_wait(&wp->startcond, &wp->mutex);
- cnt = 0;
+ //cnt = 0;
// printf("%"PRId64" thread %d awake\n", utime_now(), pthread_self());
}
zarray_get_volatile(wp->tasks, wp->taskspos, &task);
wp->taskspos++;
- cnt++;
+ //cnt++;
pthread_mutex_unlock(&wp->mutex);
// pthread_yield();
sched_yield();
diff --git a/third_party/apriltag/common/zarray.c b/third_party/apriltag/common/zarray.c
index 43e6a7e..fa1f8a2 100644
--- a/third_party/apriltag/common/zarray.c
+++ b/third_party/apriltag/common/zarray.c
@@ -41,7 +41,7 @@
return strcmp(a,b);
}
-void zarray_vmap(zarray_t *za, void (*f)())
+void zarray_vmap(zarray_t *za, void (*f)(void*))
{
assert(za != NULL);
assert(f != NULL);
diff --git a/third_party/apriltag/common/zhash.c b/third_party/apriltag/common/zhash.c
index 914f530..9414513 100644
--- a/third_party/apriltag/common/zhash.c
+++ b/third_party/apriltag/common/zhash.c
@@ -353,7 +353,7 @@
zit->last_entry--;
}
-void zhash_map_keys(zhash_t *zh, void (*f)())
+void zhash_map_keys(zhash_t *zh, void (*f)(void*))
{
assert(zh != NULL);
if (f == NULL)
@@ -369,7 +369,7 @@
}
}
-void zhash_vmap_keys(zhash_t * zh, void (*f)())
+void zhash_vmap_keys(zhash_t * zh, void (*f)(void*))
{
assert(zh != NULL);
if (f == NULL)
@@ -386,7 +386,7 @@
}
}
-void zhash_map_values(zhash_t * zh, void (*f)())
+void zhash_map_values(zhash_t * zh, void (*f)(void*))
{
assert(zh != NULL);
if (f == NULL)
@@ -401,7 +401,7 @@
}
}
-void zhash_vmap_values(zhash_t * zh, void (*f)())
+void zhash_vmap_values(zhash_t * zh, void (*f)(void*))
{
assert(zh != NULL);
if (f == NULL)
diff --git a/third_party/apriltag/common/zmaxheap.c b/third_party/apriltag/common/zmaxheap.c
index e04d03e..5cf5292 100644
--- a/third_party/apriltag/common/zmaxheap.c
+++ b/third_party/apriltag/common/zmaxheap.c
@@ -167,7 +167,7 @@
}
}
-void zmaxheap_vmap(zmaxheap_t *heap, void (*f)())
+void zmaxheap_vmap(zmaxheap_t *heap, void (*f)(void*))
{
assert(heap != NULL);
assert(f != NULL);
diff --git a/third_party/bazel-toolchain/toolchain/cc_toolchain_config.bzl b/third_party/bazel-toolchain/toolchain/cc_toolchain_config.bzl
index 59b542c..b167f6e 100644
--- a/third_party/bazel-toolchain/toolchain/cc_toolchain_config.bzl
+++ b/third_party/bazel-toolchain/toolchain/cc_toolchain_config.bzl
@@ -53,6 +53,12 @@
target_os_arch_key = _os_arch_pair(target_os, target_arch)
_check_os_arch_keys([host_os_arch_key, target_os_arch_key])
+ ## doesn't seem to have a feature for this.
+ llvm_version_split = llvm_version.split(".")
+ llvm_major_ver = int(llvm_version_split[0]) if len(llvm_version_split) else 0
+
+ llvm_subfolder = llvm_version_split[0] if llvm_major_ver > 14 else llvm_version
+
# A bunch of variables that get passed straight through to
# `create_cc_toolchain_config_info`.
# TODO: What do these values mean, and are they actually all correct?
@@ -129,7 +135,7 @@
resource_dir = [
"-resource-dir",
- "{}lib/clang/{}".format(target_toolchain_path_prefix, llvm_version),
+ "{}lib/clang/{}".format(target_toolchain_path_prefix, llvm_subfolder),
]
# Default compiler flags:
@@ -278,7 +284,7 @@
common_include_flags = [
"-nostdinc",
"-isystem",
- target_toolchain_path_prefix + "lib/clang/{}/include".format(llvm_version),
+ target_toolchain_path_prefix + "lib/clang/{}/include".format(llvm_subfolder),
"-isystem",
sysroot_path + "/usr/local/include",
"-isystem",
@@ -293,6 +299,7 @@
sysroot_path + "/usr/include",
]
compile_not_cxx_flags.extend(common_include_flags)
+
cxx_flags.extend([
"-nostdinc++",
"-isystem",
@@ -321,13 +328,12 @@
coverage_link_flags = ["-fprofile-instr-generate"]
## NOTE: framework paths is missing here; unix_cc_toolchain_config
- ## doesn't seem to have a feature for this.
# C++ built-in include directories:
cxx_builtin_include_directories = [
target_toolchain_path_prefix + "include/c++/v1",
- target_toolchain_path_prefix + "lib/clang/{}/include".format(llvm_version),
- target_toolchain_path_prefix + "lib64/clang/{}/include".format(llvm_version),
+ target_toolchain_path_prefix + "lib/clang/{}/include".format(llvm_subfolder),
+ target_toolchain_path_prefix + "lib64/clang/{}/include".format(llvm_subfolder),
]
sysroot_prefix = ""
@@ -354,8 +360,6 @@
# Tool paths:
# `llvm-strip` was introduced in V7 (https://reviews.llvm.org/D46407):
- llvm_version = llvm_version.split(".")
- llvm_major_ver = int(llvm_version[0]) if len(llvm_version) else 0
strip_binary = (tools_path_prefix + "bin/llvm-strip") if llvm_major_ver >= 7 else _host_tools.get_and_assert(host_tools_info, "strip")
# TODO: The command line formed on darwin does not work with llvm-ar.
diff --git a/third_party/bazel-toolchain/toolchain/internal/llvm_distributions.bzl b/third_party/bazel-toolchain/toolchain/internal/llvm_distributions.bzl
index 4cdbf75..031f4cf 100644
--- a/third_party/bazel-toolchain/toolchain/internal/llvm_distributions.bzl
+++ b/third_party/bazel-toolchain/toolchain/internal/llvm_distributions.bzl
@@ -181,6 +181,102 @@
"clang+llvm-13.0.0-x86_64-linux-gnu-ubuntu-16.04.tar.xz": "76d0bf002ede7a893f69d9ad2c4e101d15a8f4186fbfe24e74856c8449acd7c1",
"clang+llvm-13.0.0-x86_64-linux-gnu-ubuntu-20.04.tar.xz": "2c2fb857af97f41a5032e9ecadf7f78d3eff389a5cd3c9ec620d24f134ceb3c8",
"clang+llvm-13.0.0-aarch64-linux-gnu.tar.xz": "968d65d2593850ee9b37fcda074fb7641529bd45d2f976af6c8197de3c22612f",
+
+ # 13.0.1
+ "clang+llvm-13.0.1-aarch64-linux-gnu.tar.xz": "15ff2db12683e69e552b6668f7ca49edaa01ce32cb1cbc8f8ed2e887ab291069",
+ "clang+llvm-13.0.1-amd64-unknown-freebsd12.tar.xz": "8101c8d3a920bf930b33987ada5373f43537c5de8c194be0ea10530fd0ad5617",
+ "clang+llvm-13.0.1-amd64-unknown-freebsd13.tar.xz": "f1ba8ec77b5e82399af738ad9897a8aafc11c5692ceb331c8373eae77018d428",
+ "clang+llvm-13.0.1-armv7a-linux-gnueabihf.tar.xz": "1215720114538f57acbe2f3b0614c23f4fc551ba2976afa3779a3c01aaaf1221",
+ "clang+llvm-13.0.1-i386-unknown-freebsd12.tar.xz": "e3c921e0f130afa6a6ebac23c31b66b32563a5ec53a2f4ed4676f31a81379f70",
+ "clang+llvm-13.0.1-i386-unknown-freebsd13.tar.xz": "e85c46bd64a0217f3df1f42421a502648d6741ef29fd5d44674b87af119ce25d",
+ "clang+llvm-13.0.1-powerpc64le-linux-rhel-7.9.tar.xz": "ab659c290536182a99c064d4537d2fb1273bb2b1bf8c6a43866f033bf1ece4a8",
+ "clang+llvm-13.0.1-powerpc64le-linux-ubuntu-18.04.5.tar.xz": "7a4be2508aa0b4ee3f72c312af4b62ea14581a5db61aa703ea0822f46e5598cb",
+ "clang+llvm-13.0.1-x86_64-apple-darwin.tar.xz": "dec02d17698514d0fc7ace8869c38937851c542b02adf102c4e898f027145a4d",
+ "clang+llvm-13.0.1-x86_64-linux-gnu-ubuntu-18.04.tar.xz": "84a54c69781ad90615d1b0276a83ff87daaeded99fbc64457c350679df7b4ff0",
+
+ # 14.0.0
+ "clang+llvm-14.0.0-aarch64-linux-gnu.tar.xz": "1792badcd44066c79148ffeb1746058422cc9d838462be07e3cb19a4b724a1ee",
+ "clang+llvm-14.0.0-amd64-pc-solaris2.11.tar.xz": "a708470fdbaadf530d6cfd56f92fde1328cb47ef8439ecf1a2126523e7c94a50",
+ "clang+llvm-14.0.0-amd64-unknown-freebsd12.tar.xz": "7eaff7ee2a32babd795599f41f4a5ffe7f161721ebf5630f48418e626650105e",
+ "clang+llvm-14.0.0-amd64-unknown-freebsd13.tar.xz": "b68d73fd57be385e7f06046a87381f7520c8861f492c294e6301d2843d9a1f57",
+ "clang+llvm-14.0.0-armv7a-linux-gnueabihf.tar.xz": "17d5f60c3d5f9494be7f67b2dc9e6017cd5e8457e53465968a54ec7069923bfe",
+ "clang+llvm-14.0.0-i386-unknown-freebsd12.tar.xz": "5ed9d93a8425132e8117d7061d09c2989ce6b2326f25c46633e2b2dee955bb00",
+ "clang+llvm-14.0.0-i386-unknown-freebsd13.tar.xz": "81f49eb466ce9149335ac8918a5f02fa724d562a94464ed13745db0165b4a220",
+ "clang+llvm-14.0.0-powerpc64-ibm-aix-7.2.tar.xz": "4ad5866de6c69d989cbbc989201b46dfdcd7d2b23a712fcad7baa09c204f10de",
+ "clang+llvm-14.0.0-powerpc64le-linux-rhel-7.9.tar.xz": "7a31de37959fdf3be897b01f284a91c28cd38a2e2fa038ff58121d1b6f6eb087",
+ "clang+llvm-14.0.0-powerpc64le-linux-ubuntu-18.04.tar.xz": "2d504c4920885c86b306358846178bc2232dfac83b47c3b1d05861a8162980e6",
+ "clang+llvm-14.0.0-sparcv9-sun-solaris2.11.tar.xz": "b342cdaaea3b44de5b0f45052e2df49bcdf69dcc8ad0c23ec5afc04668929681",
+ "clang+llvm-14.0.0-x86_64-apple-darwin.tar.xz": "cf5af0f32d78dcf4413ef6966abbfd5b1445fe80bba57f2ff8a08f77e672b9b3",
+ "clang+llvm-14.0.0-x86_64-linux-gnu-ubuntu-18.04.tar.xz": "61582215dafafb7b576ea30cc136be92c877ba1f1c31ddbbd372d6d65622fef5",
+ "clang+llvm-14.0.0-x86_64-linux-sles12.4.tar.xz": "78f70cc94c3b6f562455b15cebb63e75571d50c3d488d53d9aa4cd9dded30627",
+
+ # 15.0.0
+ "clang+llvm-15.0.0-aarch64-linux-gnu.tar.xz": "527ed550784681f95ec7a1be8fbf5a24bd03d7da9bf31afb6523996f45670be3",
+ "clang+llvm-15.0.0-amd64-pc-solaris2.11.tar.xz": "5b9fd6a30ce6941adf74667d2076a49aa047fa040e3690f7af26c264d4ce58e7",
+ "clang+llvm-15.0.0-arm64-apple-darwin21.0.tar.xz": "cfd5c3fa07d7fccea0687f5b4498329a6172b7a15bbc45b547d0ac86bd3452a5",
+ "clang+llvm-15.0.0-armv7a-linux-gnueabihf.tar.xz": "58ce8877642fc1399736ffc81bc8ef6244440fc78d72e097a07475b8b25e2bf1",
+ "clang+llvm-15.0.0-powerpc64-ibm-aix-7.2.tar.xz": "c5f63401fa88ea96ca7110bd81ead1bf1a2575962e9cc84a6713ec29c02b1c10",
+ "clang+llvm-15.0.0-powerpc64le-linux-rhel-8.4.tar.xz": "c94448766b6b92cfc8f35e611308c9680a9ad2177f88d358c2b06e9b108d61bd",
+ "clang+llvm-15.0.0-powerpc64le-linux-ubuntu-18.04.6.tar.xz": "6bcedc3d18552732f219c1d0f8c4b0c917ff5f800400a31dabfe8d040cbf1f02",
+ "clang+llvm-15.0.0-sparc64-unknown-linux-gnu.tar.xz": "b5a8108040d5d5d69d6106fa89a6cffc71a16a3583b74c1f15c42f392a47a3d9",
+ "clang+llvm-15.0.0-sparcv9-sun-solaris2.11.tar.xz": "4354854976355ca6f4ac90231a97121844c4fc9f998c9850527390120c62f01f",
+ "clang+llvm-15.0.0-x86_64-apple-darwin.tar.xz": "8fb11e6ada98b901398b2e7b0378a3a59e88c88c754e95d8f6b54613254d7d65",
+
+ # 15.0.6
+ "clang+llvm-15.0.6-aarch64-linux-gnu.tar.xz": "8ca4d68cf103da8331ca3f35fe23d940c1b78fb7f0d4763c1c059e352f5d1bec",
+ "clang+llvm-15.0.6-arm64-apple-darwin21.0.tar.xz": "32bc7b8eee3d98f72dd4e5651e6da990274ee2d28c5c19a7d8237eb817ce8d91",
+ "clang+llvm-15.0.6-armv7a-linux-gnueabihf.tar.xz": "c12e9298f9a9ed3a96342e9ffb2c02146a0cd7535231fef57c7217bd3a36f53b",
+ "clang+llvm-15.0.6-powerpc64-ibm-aix-7.2.tar.xz": "6bc1c2fcc8069e28773f6a0d16624160cd6de01b8f15aab27652eedad665d462",
+ "clang+llvm-15.0.6-powerpc64le-linux-rhel-8.4.tar.xz": "c26e5563e6ff46a03bc45fe27547c69283b64cba2359ccd3a42f735c995c0511",
+ "clang+llvm-15.0.6-powerpc64le-linux-ubuntu-18.04.tar.xz": "7fc9f07ff0fcf191df93fe4adc1da555e43f62fe1d3ddafb15c943f72b1bda17",
+ "clang+llvm-15.0.6-x86_64-linux-gnu-ubuntu-18.04.tar.xz": "38bc7f5563642e73e69ac5626724e206d6d539fbef653541b34cae0ba9c3f036",
+
+ # 15.0.7
+ "clang+llvm-15.0.7-arm64-apple-darwin22.0.tar.xz": "867c6afd41158c132ef05a8f1ddaecf476a26b91c85def8e124414f9a9ba188d",
+ "clang+llvm-15.0.7-powerpc64-ibm-aix-7.2.tar.xz": "6cbc7c7f4395abb9c1a5bdcab3811bd6b1a6c4d08756ba674bfbbd732e2b23ac",
+ "clang+llvm-15.0.7-powerpc64le-linux-rhel-8.4.tar.xz": "2163cc934437146dc30810a21a46327ba3983f123c3bea19be316a64135b6414",
+ "clang+llvm-15.0.7-powerpc64le-linux-ubuntu-18.04.tar.xz": "19a16d768e15966923b0cbf8fc7dc148c89e316857acd89ad3aff72dcfcd61f4",
+ "clang+llvm-15.0.7-x86_64-apple-darwin21.0.tar.xz": "d16b6d536364c5bec6583d12dd7e6cf841b9f508c4430d9ee886726bd9983f1c",
+
+ # 16.0.0
+ "clang+llvm-16.0.0-aarch64-linux-gnu.tar.xz": "b750ba3120e6153fc5b316092f19b52cf3eb64e19e5f44bd1b962cb54a20cf0a",
+ "clang+llvm-16.0.0-amd64-pc-solaris2.11.tar.xz": "b637b7da383d3417ac4862342911cb467fba2ec00f48f163eb8308f2bbb9b7ad",
+ "clang+llvm-16.0.0-amd64-unknown-freebsd13.tar.xz": "c4fe6293349b3ab7d802793103d1d44f58831884e63ff1b40ce29c3e7408257b",
+ "clang+llvm-16.0.0-arm64-apple-darwin22.0.tar.xz": "2041587b90626a4a87f0de14a5842c14c6c3374f42c8ed12726ef017416409d9",
+ "clang+llvm-16.0.0-powerpc64-ibm-aix-7.2.tar.xz": "e51209eeea3c3db41084d8625ab3357991980831e0b641d633ec23e9d858333f",
+ "clang+llvm-16.0.0-powerpc64le-linux-rhel-8.4.tar.xz": "eb56949af9a83a12754f7cf254886d30c4be8a1da4dd0f27db790a7fcd35a3bf",
+ "clang+llvm-16.0.0-powerpc64le-linux-ubuntu-18.04.tar.xz": "ae34b037cde14f19c3c431de5fc04e06fa43d2cce3f8d44a63659b48afdf1f7a",
+ "clang+llvm-16.0.0-sparc64-unknown-linux-gnu.tar.xz": "a2627fcb6d97405b38c9e4c17ccfdc5d61fdd1bee742dcce0726ed39e2dcd92c",
+ "clang+llvm-16.0.0-sparcv9-sun-solaris2.11.tar.xz": "45c2ac0c10c3876332407a1ea893dccbde77a490f4a9b54a00e4881681a3c5ea",
+ "clang+llvm-16.0.0-x86_64-linux-gnu-ubuntu-18.04.tar.xz": "2b8a69798e8dddeb57a186ecac217a35ea45607cb2b3cf30014431cff4340ad1",
+
+ # 16.0.1
+ "clang+llvm-16.0.1-aarch64-linux-gnu.tar.xz": "83e38451772120b016432687c0a3aab391808442b86f54966ef44c73a26280ac",
+ "clang+llvm-16.0.1-amd64-unknown-freebsd13.tar.xz": "970359de2a1a09a93a9e1cf3405e5758dfe463567b20a168f9156bd72b7f8ac6",
+ "clang+llvm-16.0.1-arm64-apple-darwin22.0.tar.xz": "cb487fa991f047dc79ae36430cbb9ef14621c1262075373955b1d97215c75879",
+ "clang+llvm-16.0.1-powerpc64-ibm-aix-7.2.tar.xz": "c56d9cf643b7f39e40436e55b59b3bd88057ec0fa084bd8e06ac17fb20ea2a21",
+ "clang+llvm-16.0.1-powerpc64le-linux-rhel-8.4.tar.xz": "c89a9af64a35ee58ef4eac7b52c173707140dc7eac6839ff254b656de8eb6c3c",
+ "clang+llvm-16.0.1-powerpc64le-linux-ubuntu-20.04.tar.xz": "08b39f9e6c19086aaf029d155c42a4db96ce662f84d6e89d8c9037d3baeee036",
+
+ # 16.0.2
+ "clang+llvm-16.0.2-aarch64-linux-gnu.tar.xz": "de89d138cfb17e2d81fdaca2f9c5e0c042014beea6bcacde7f27db40b69c0bdc",
+ "clang+llvm-16.0.2-amd64-unknown-freebsd13.tar.xz": "0cd92b6a84e7477aa8070465f01eec8198e0b1e38d1b6da8c61859a633ec9a71",
+ "clang+llvm-16.0.2-arm64-apple-darwin22.0.tar.xz": "539861297b8aa6be8e89bf68268b07d79d7a1fde87f4b98f123709f13933f326",
+ "clang+llvm-16.0.2-powerpc64-ibm-aix-7.2.tar.xz": "8c9cbf29b261f1af905f41032b446fd78bd560b549ab31d05a16d0cc972df23d",
+ "clang+llvm-16.0.2-powerpc64le-linux-rhel-8.4.tar.xz": "fe21023b64d2298d65fea0f4832a27a9948121662b54a8c8ce8a9331c4039c36",
+ "clang+llvm-16.0.2-x86_64-linux-gnu-ubuntu-22.04.tar.xz": "9530eccdffedb9761f23cbd915cf95d861b1d95f340ea36ded68bd6312af912e",
+
+ # 16.0.3
+ "clang+llvm-16.0.3-aarch64-linux-gnu.tar.xz": "315fd821ddb3e4b10c4dfabe7f200d1d17902b6a5ccd5dd665a0cd454bca379f",
+ "clang+llvm-16.0.3-arm64-apple-darwin22.0.tar.xz": "b9068eee1cf1e17848241ea581a2abe6cb4a15d470ec515c100f8b52e4c6a7cb",
+ "clang+llvm-16.0.3-powerpc64-ibm-aix-7.2.tar.xz": "f0372ea5b665ca1b8524b933b84ccbe59e9441537388815b24323aa4aab7db2f",
+ "clang+llvm-16.0.3-powerpc64le-linux-rhel-8.4.tar.xz": "9804721c746d74a85ce935d938509277af728fad1548835f539660ff1380e04d",
+ "clang+llvm-16.0.3-x86_64-linux-gnu-ubuntu-22.04.tar.xz": "638d32fd0032f99bafaab3bae63a406adb771825a02b6b7da119ee7e71af26c6",
+
+ # 16.0.4
+ "clang+llvm-16.0.4-amd64-unknown-freebsd13.tar.xz": "cf9d73bcf05b8749c7f3efbe86654b8fe0209f28993eafe26c27eb85885593f7",
+ "clang+llvm-16.0.4-arm64-apple-darwin22.0.tar.xz": "429b8061d620108fee636313df55a0602ea0d14458c6d3873989e6b130a074bd",
+ "clang+llvm-16.0.4-x86_64-linux-gnu-ubuntu-22.04.tar.xz": "fd464333bd55b482eb7385f2f4e18248eb43129a3cda4c0920ad9ac3c12bdacf",
}
# Note: Unlike the user-specified llvm_mirror attribute, the URL prefixes in
@@ -201,6 +297,16 @@
"12.0.0": "https://github.com/llvm/llvm-project/releases/download/llvmorg-",
"12.0.1": "https://github.com/llvm/llvm-project/releases/download/llvmorg-",
"13.0.0": "https://github.com/llvm/llvm-project/releases/download/llvmorg-",
+ "13.0.1": "https://github.com/llvm/llvm-project/releases/download/llvmorg-",
+ "14.0.0": "https://github.com/llvm/llvm-project/releases/download/llvmorg-",
+ "15.0.0": "https://github.com/llvm/llvm-project/releases/download/llvmorg-",
+ "15.0.6": "https://github.com/llvm/llvm-project/releases/download/llvmorg-",
+ "15.0.7": "https://github.com/llvm/llvm-project/releases/download/llvmorg-",
+ "16.0.0": "https://github.com/llvm/llvm-project/releases/download/llvmorg-",
+ "16.0.1": "https://github.com/llvm/llvm-project/releases/download/llvmorg-",
+ "16.0.2": "https://github.com/llvm/llvm-project/releases/download/llvmorg-",
+ "16.0.3": "https://github.com/llvm/llvm-project/releases/download/llvmorg-",
+ "16.0.4": "https://github.com/llvm/llvm-project/releases/download/llvmorg-",
}
def _get_auth(ctx, urls):
diff --git a/third_party/eigen/Eigen/src/SparseCore/TriangularSolver.h b/third_party/eigen/Eigen/src/SparseCore/TriangularSolver.h
index f9c56ba..07c0d88 100644
--- a/third_party/eigen/Eigen/src/SparseCore/TriangularSolver.h
+++ b/third_party/eigen/Eigen/src/SparseCore/TriangularSolver.h
@@ -270,17 +270,12 @@
}
- Index count = 0;
// FIXME compute a reference value to filter zeros
for (typename AmbiVector<Scalar,StorageIndex>::Iterator it(tempVector/*,1e-12*/); it; ++it)
{
- ++ count;
-// std::cerr << "fill " << it.index() << ", " << col << "\n";
-// std::cout << it.value() << " ";
// FIXME use insertBack
res.insert(it.index(), col) = it.value();
}
-// std::cout << "tempVector.nonZeros() == " << int(count) << " / " << (other.rows()) << "\n";
}
res.finalize();
other = res.markAsRValue();
diff --git a/third_party/eigen/Eigen/src/SparseLU/SparseLU_heap_relax_snode.h b/third_party/eigen/Eigen/src/SparseLU/SparseLU_heap_relax_snode.h
index 6f75d50..7aecbca 100644
--- a/third_party/eigen/Eigen/src/SparseLU/SparseLU_heap_relax_snode.h
+++ b/third_party/eigen/Eigen/src/SparseLU/SparseLU_heap_relax_snode.h
@@ -75,8 +75,6 @@
// Identify the relaxed supernodes by postorder traversal of the etree
Index snode_start; // beginning of a snode
StorageIndex k;
- Index nsuper_et_post = 0; // Number of relaxed snodes in postordered etree
- Index nsuper_et = 0; // Number of relaxed snodes in the original etree
StorageIndex l;
for (j = 0; j < n; )
{
@@ -88,7 +86,6 @@
parent = et(j);
}
// Found a supernode in postordered etree, j is the last column
- ++nsuper_et_post;
k = StorageIndex(n);
for (Index i = snode_start; i <= j; ++i)
k = (std::min)(k, inv_post(i));
@@ -97,7 +94,6 @@
{
// This is also a supernode in the original etree
relax_end(k) = l; // Record last column
- ++nsuper_et;
}
else
{
@@ -107,7 +103,6 @@
if (descendants(i) == 0)
{
relax_end(l) = l;
- ++nsuper_et;
}
}
}
diff --git a/third_party/google-glog/bazel/glog.bzl b/third_party/google-glog/bazel/glog.bzl
index b4825d3..63f40d3 100644
--- a/third_party/google-glog/bazel/glog.bzl
+++ b/third_party/google-glog/bazel/glog.bzl
@@ -90,6 +90,7 @@
]
linux_or_darwin_copts = wasm_copts + [
+ "-Wno-unused-but-set-variable",
"-DGLOG_EXPORT=__attribute__((visibility(\\\"default\\\")))",
# For src/utilities.cc.
"-DHAVE_SYS_SYSCALL_H",
diff --git a/third_party/googletest/googletest.patch b/third_party/googletest/googletest.patch
index ad57dc5..25d8cf4 100644
--- a/third_party/googletest/googletest.patch
+++ b/third_party/googletest/googletest.patch
@@ -30,3 +30,17 @@
linkopts = select({
"//:qnx": [],
"//:windows": [],
+diff --git a/googletest/test/BUILD.bazel b/googletest/test/BUILD.bazel
+index 1890b6ff..9bd00bd2 100644
+--- a/googletest/test/BUILD.bazel
++++ b/googletest/test/BUILD.bazel
+@@ -151,6 +151,9 @@ cc_test(
+ name = "gtest_unittest",
+ size = "small",
+ srcs = ["gtest_unittest.cc"],
++ copts = [
++ "-Wno-unused-but-set-variable",
++ ],
+ shard_count = 2,
+ deps = ["//:gtest_main"],
+ )
diff --git a/third_party/pico-sdk/tools/pioasm/BUILD b/third_party/pico-sdk/tools/pioasm/BUILD
index 355834f..1a276c3 100644
--- a/third_party/pico-sdk/tools/pioasm/BUILD
+++ b/third_party/pico-sdk/tools/pioasm/BUILD
@@ -21,6 +21,7 @@
"-Wno-unused-parameter",
"-Wno-sign-compare",
"-fexceptions",
+ "-Wno-unused-but-set-variable",
],
includes = [
".",
diff --git a/third_party/rawrtc/usrsctp/BUILD b/third_party/rawrtc/usrsctp/BUILD
index 6d1fa69..2a4118f 100644
--- a/third_party/rawrtc/usrsctp/BUILD
+++ b/third_party/rawrtc/usrsctp/BUILD
@@ -50,6 +50,7 @@
] + compiler_select({
"clang": [
"-Wno-unused-but-set-variable",
+ "-Wno-deprecated-non-prototype",
],
"gcc": [
"-Wno-discarded-qualifiers",
diff --git a/tools/build_rules/autocxx.bzl b/tools/build_rules/autocxx.bzl
index d4b6be6..01a9197 100644
--- a/tools/build_rules/autocxx.bzl
+++ b/tools/build_rules/autocxx.bzl
@@ -138,6 +138,7 @@
gen_rs.add_all(["--outdir", out_rs_json.dirname])
gen_rs.add("--gen-rs-archive")
gen_rs.add("--gen-cpp")
+ #gen_rs.add("--auto-allowlist")
gen_rs.add_all(["--generate-exact", ctx.attr.sections_to_generate])
diff --git a/y2014/control_loops/drivetrain/drivetrain_base.h b/y2014/control_loops/drivetrain/drivetrain_base.h
index a47ff5c..48143c3 100644
--- a/y2014/control_loops/drivetrain/drivetrain_base.h
+++ b/y2014/control_loops/drivetrain/drivetrain_base.h
@@ -6,8 +6,8 @@
namespace y2014 {
namespace control_loops {
-const ::frc971::control_loops::drivetrain::DrivetrainConfig<double>
- &GetDrivetrainConfig();
+const ::frc971::control_loops::drivetrain::DrivetrainConfig<double> &
+GetDrivetrainConfig();
} // namespace control_loops
} // namespace y2014
diff --git a/y2014_bot3/control_loops/drivetrain/drivetrain_base.h b/y2014_bot3/control_loops/drivetrain/drivetrain_base.h
index d41ff99..98cb31d 100644
--- a/y2014_bot3/control_loops/drivetrain/drivetrain_base.h
+++ b/y2014_bot3/control_loops/drivetrain/drivetrain_base.h
@@ -10,8 +10,8 @@
const double kDrivetrainEncoderRatio =
(17.0 / 50.0) /*output reduction*/ * (64.0 / 24.0) /*encoder gears*/;
-const ::frc971::control_loops::drivetrain::DrivetrainConfig<double>
- &GetDrivetrainConfig();
+const ::frc971::control_loops::drivetrain::DrivetrainConfig<double> &
+GetDrivetrainConfig();
} // namespace drivetrain
} // namespace control_loops
diff --git a/y2016/control_loops/drivetrain/drivetrain_base.h b/y2016/control_loops/drivetrain/drivetrain_base.h
index 4d5525a..0186a6c 100644
--- a/y2016/control_loops/drivetrain/drivetrain_base.h
+++ b/y2016/control_loops/drivetrain/drivetrain_base.h
@@ -7,8 +7,8 @@
namespace control_loops {
namespace drivetrain {
-const ::frc971::control_loops::drivetrain::DrivetrainConfig<double>
- &GetDrivetrainConfig();
+const ::frc971::control_loops::drivetrain::DrivetrainConfig<double> &
+GetDrivetrainConfig();
} // namespace drivetrain
} // namespace control_loops
diff --git a/y2016/dashboard/dashboard.cc b/y2016/dashboard/dashboard.cc
index 56b10d7..0b12b14 100644
--- a/y2016/dashboard/dashboard.cc
+++ b/y2016/dashboard/dashboard.cc
@@ -42,11 +42,11 @@
// Define the following if we want to use a local www directory and feed in
// dummy data.
-//#define DASHBOARD_TESTING
+// #define DASHBOARD_TESTING
// Define the following if we want to read from the vision queue, which has
// caused problems in the past when auto aiming that still need to be addressed.
-//#define DASHBOARD_READ_VISION_QUEUE
+// #define DASHBOARD_READ_VISION_QUEUE
DataCollector::DataCollector(::aos::EventLoop *event_loop)
: event_loop_(event_loop),
diff --git a/y2017/control_loops/drivetrain/drivetrain_base.h b/y2017/control_loops/drivetrain/drivetrain_base.h
index 59ca4a9..c84934f 100644
--- a/y2017/control_loops/drivetrain/drivetrain_base.h
+++ b/y2017/control_loops/drivetrain/drivetrain_base.h
@@ -7,8 +7,8 @@
namespace control_loops {
namespace drivetrain {
-const ::frc971::control_loops::drivetrain::DrivetrainConfig<double>
- &GetDrivetrainConfig();
+const ::frc971::control_loops::drivetrain::DrivetrainConfig<double> &
+GetDrivetrainConfig();
} // namespace drivetrain
} // namespace control_loops
diff --git a/y2017/vision/target_finder.cc b/y2017/vision/target_finder.cc
index f7a958c..e917b2e 100644
--- a/y2017/vision/target_finder.cc
+++ b/y2017/vision/target_finder.cc
@@ -68,14 +68,12 @@
RangeImage t_img = Transpose(img);
int total = 0;
int split = 0;
- int count = t_img.mini();
for (const auto &row : t_img) {
if (row.size() == 1) {
total++;
} else if (row.size() == 2) {
split++;
}
- count++;
}
return (double)split / total;
}
diff --git a/y2018/control_loops/drivetrain/drivetrain_base.h b/y2018/control_loops/drivetrain/drivetrain_base.h
index a7ce43f..0592a25 100644
--- a/y2018/control_loops/drivetrain/drivetrain_base.h
+++ b/y2018/control_loops/drivetrain/drivetrain_base.h
@@ -7,8 +7,8 @@
namespace control_loops {
namespace drivetrain {
-const ::frc971::control_loops::drivetrain::DrivetrainConfig<double>
- &GetDrivetrainConfig();
+const ::frc971::control_loops::drivetrain::DrivetrainConfig<double> &
+GetDrivetrainConfig();
} // namespace drivetrain
} // namespace control_loops
diff --git a/y2019/control_loops/drivetrain/drivetrain_base.h b/y2019/control_loops/drivetrain/drivetrain_base.h
index 5cee1b5..3a8bd6b 100644
--- a/y2019/control_loops/drivetrain/drivetrain_base.h
+++ b/y2019/control_loops/drivetrain/drivetrain_base.h
@@ -7,8 +7,8 @@
namespace control_loops {
namespace drivetrain {
-const ::frc971::control_loops::drivetrain::DrivetrainConfig<double>
- &GetDrivetrainConfig();
+const ::frc971::control_loops::drivetrain::DrivetrainConfig<double> &
+GetDrivetrainConfig();
} // namespace drivetrain
} // namespace control_loops
diff --git a/y2020/control_loops/drivetrain/drivetrain_base.h b/y2020/control_loops/drivetrain/drivetrain_base.h
index c220088..e35c2af 100644
--- a/y2020/control_loops/drivetrain/drivetrain_base.h
+++ b/y2020/control_loops/drivetrain/drivetrain_base.h
@@ -7,8 +7,8 @@
namespace control_loops {
namespace drivetrain {
-const ::frc971::control_loops::drivetrain::DrivetrainConfig<double>
- &GetDrivetrainConfig();
+const ::frc971::control_loops::drivetrain::DrivetrainConfig<double> &
+GetDrivetrainConfig();
} // namespace drivetrain
} // namespace control_loops
diff --git a/y2020/vision/sift/fast_gaussian_halide_generator.sh b/y2020/vision/sift/fast_gaussian_halide_generator.sh
index 793e56c..cf094b8 100755
--- a/y2020/vision/sift/fast_gaussian_halide_generator.sh
+++ b/y2020/vision/sift/fast_gaussian_halide_generator.sh
@@ -43,12 +43,12 @@
-isystem"${SYSROOT}/usr/include/c++/10" \
-isystem"${SYSROOT}/usr/include/${MULTIARCH}/c++/10" \
-isystem"${SYSROOT}/usr/include/c++/7/backward" \
- -isystem"${LLVM_TOOLCHAIN}/lib/clang/13.0.0/include" \
+ -isystem"${LLVM_TOOLCHAIN}/lib/clang/16/include" \
-isystem"${SYSROOT}/usr/include/${MULTIARCH}" \
-isystem"${SYSROOT}/usr/include" \
-isystem"${SYSROOT}/include" \
"--sysroot=${SYSROOT}" \
- -resource-dir "${LLVM_TOOLCHAIN}/lib/clang/13.0.0" \
+ -resource-dir "${LLVM_TOOLCHAIN}/lib/clang/16" \
-target "${TARGET}" \
-fuse-ld=lld \
-L"${LLVM_TOOLCHAIN}/lib" \
diff --git a/y2021_bot3/control_loops/drivetrain/drivetrain_base.h b/y2021_bot3/control_loops/drivetrain/drivetrain_base.h
index 7250f93..f796d4e 100644
--- a/y2021_bot3/control_loops/drivetrain/drivetrain_base.h
+++ b/y2021_bot3/control_loops/drivetrain/drivetrain_base.h
@@ -7,8 +7,8 @@
namespace control_loops {
namespace drivetrain {
-const ::frc971::control_loops::drivetrain::DrivetrainConfig<double>
- &GetDrivetrainConfig();
+const ::frc971::control_loops::drivetrain::DrivetrainConfig<double> &
+GetDrivetrainConfig();
} // namespace drivetrain
} // namespace control_loops
diff --git a/y2022/control_loops/drivetrain/drivetrain_base.h b/y2022/control_loops/drivetrain/drivetrain_base.h
index 2983016..1f4cfe4 100644
--- a/y2022/control_loops/drivetrain/drivetrain_base.h
+++ b/y2022/control_loops/drivetrain/drivetrain_base.h
@@ -7,8 +7,8 @@
namespace control_loops {
namespace drivetrain {
-const ::frc971::control_loops::drivetrain::DrivetrainConfig<double>
- &GetDrivetrainConfig();
+const ::frc971::control_loops::drivetrain::DrivetrainConfig<double> &
+GetDrivetrainConfig();
} // namespace drivetrain
} // namespace control_loops
diff --git a/y2022_bot3/control_loops/drivetrain/drivetrain_base.h b/y2022_bot3/control_loops/drivetrain/drivetrain_base.h
index cce197e..04c6c86 100644
--- a/y2022_bot3/control_loops/drivetrain/drivetrain_base.h
+++ b/y2022_bot3/control_loops/drivetrain/drivetrain_base.h
@@ -7,8 +7,8 @@
namespace control_loops {
namespace drivetrain {
-const ::frc971::control_loops::drivetrain::DrivetrainConfig<double>
- &GetDrivetrainConfig();
+const ::frc971::control_loops::drivetrain::DrivetrainConfig<double> &
+GetDrivetrainConfig();
} // namespace drivetrain
} // namespace control_loops
diff --git a/y2023/constants/7971.json b/y2023/constants/7971.json
index 74fd3aa..10cc438 100644
--- a/y2023/constants/7971.json
+++ b/y2023/constants/7971.json
@@ -4,7 +4,7 @@
"calibration": {% include 'y2023/vision/calib_files/calibration_pi-7971-1_cam-23-01_ext_2023-02-19.json' %}
},
{
- "calibration": {% include 'y2023/vision/calib_files/calibration_pi-7971-2_cam-23-02_ext_2023-04-08.json' %}
+ "calibration": {% include 'y2023/vision/calib_files/calibration_pi-7971-2_cam-23-15_ext_2023-09-09.json' %}
},
{
"calibration": {% include 'y2023/vision/calib_files/calibration_pi-7971-3_cam-23-03_ext_2023-04-08.json' %}
diff --git a/y2023/control_loops/drivetrain/drivetrain_base.h b/y2023/control_loops/drivetrain/drivetrain_base.h
index 6404081..98f984e 100644
--- a/y2023/control_loops/drivetrain/drivetrain_base.h
+++ b/y2023/control_loops/drivetrain/drivetrain_base.h
@@ -7,8 +7,8 @@
namespace control_loops {
namespace drivetrain {
-const ::frc971::control_loops::drivetrain::DrivetrainConfig<double>
- &GetDrivetrainConfig();
+const ::frc971::control_loops::drivetrain::DrivetrainConfig<double> &
+GetDrivetrainConfig();
} // namespace drivetrain
} // namespace control_loops
diff --git a/y2023/vision/BUILD b/y2023/vision/BUILD
index d5168c1..ef300e3 100644
--- a/y2023/vision/BUILD
+++ b/y2023/vision/BUILD
@@ -83,10 +83,10 @@
"//frc971/vision:calibration_fbs",
"//frc971/vision:charuco_lib",
"//frc971/vision:target_mapper",
+ "//frc971/vision:visualize_robot",
"//third_party:opencv",
"//y2023/constants:constants_fbs",
"//y2023/constants:simulated_constants_sender",
- "@org_tuxfamily_eigen//:eigen",
],
)
@@ -203,6 +203,36 @@
)
cc_binary(
+ name = "calibrate_multi_cameras",
+ srcs = [
+ "calibrate_multi_cameras.cc",
+ ],
+ data = [
+ "//y2023:aos_config",
+ "//y2023/constants:constants.json",
+ "//y2023/vision:maps",
+ ],
+ target_compatible_with = ["@platforms//os:linux"],
+ visibility = ["//y2023:__subpackages__"],
+ deps = [
+ ":aprilrobotics_lib",
+ "//aos:init",
+ "//aos/events:simulated_event_loop",
+ "//aos/events/logging:log_reader",
+ "//aos/util:mcap_logger",
+ "//frc971/constants:constants_sender_lib",
+ "//frc971/control_loops:pose",
+ "//frc971/vision:calibration_fbs",
+ "//frc971/vision:charuco_lib",
+ "//frc971/vision:target_mapper",
+ "//third_party:opencv",
+ "//y2023/constants:constants_fbs",
+ "//y2023/constants:simulated_constants_sender",
+ "@org_tuxfamily_eigen//:eigen",
+ ],
+)
+
+cc_binary(
name = "game_pieces_detector",
srcs = [
"game_pieces_main.cc",
diff --git a/y2023/vision/README.md b/y2023/vision/README.md
new file mode 100644
index 0000000..55d217f
--- /dev/null
+++ b/y2023/vision/README.md
@@ -0,0 +1,25 @@
+How to use the extrinsic calibration for camera-to-camera calibration
+
+This all assumes you have cameras that have been intrinsically
+calibrated, and that pi1 has a valid extrinsic calibration (from robot
+origin / IMU to the pi1 camera).
+
+It by default will compute the extrinsics for each of the other cameras (pi2,
+pi3, pi4) relative to the robot origin (IMU origin).
+
+Steps:
+* Place two Aruco Diamond markers about 1 meter apart
+ (center-to-center) at a height so that they will be in view of the
+ cameras when the robot is about 1-2m away
+* Start with the robot in a position that both markers are fully in
+ view by one camera
+* Enable logging for all cameras
+* Rotate the robot in place through a full circle, so that each camera sees both tags, and at times just one tag.
+* Stop the logging and copy the files to your laptop
+* Run the calibration code on the resulting files, e.g.,
+ * `bazel run -c opt //y2023/vision:calibrate_multi_cameras -- /data/PATH_TO_LOGS --team_number 971 --target_type charuco_diamond
+ * Can add "--visualize" flag to see the camera views and marker detections
+ * I've sometimes found it necessary to add the "--skip_missing_forwarding_entries" flag-- as guided by the logging messages
+* From the output, copy the calculated ("Updated") extrinsic into the
+ corresponding calibration file for the right robot and camera in the
+ calib_files directory
\ No newline at end of file
diff --git a/y2023/vision/calib_files/calibration_pi-7971-2_cam-23-02_ext_2023-04-05.json b/y2023/vision/calib_files/calibration_pi-7971-2_cam-23-02_ext_2023-04-05.json
deleted file mode 100644
index b175abb..0000000
--- a/y2023/vision/calib_files/calibration_pi-7971-2_cam-23-02_ext_2023-04-05.json
+++ /dev/null
@@ -1 +0,0 @@
-{ "node_name": "pi2", "team_number": 7971, "intrinsics": [ 895.543945, 0.0, 645.250122, 0.0, 895.308838, 354.297241, 0.0, 0.0, 1.0 ], "fixed_extrinsics": { "data": [ -0.482031, 0.00240341, 0.87615, 0.149719, -0.87612, -0.0100773, -0.481987, -0.000995038, 0.00767093, -0.999947, 0.00696306, 0.0062866, 0.0, 0.0, 0.0, 1.0 ] }, "dist_coeffs": [ -0.455658, 0.272167, 0.000796, -0.000206, -0.0975 ], "calibration_timestamp": 1358499769498335208, "camera_id": "23-02" }
diff --git a/y2023/vision/calib_files/calibration_pi-7971-2_cam-23-15_ext_2023-09-09.json b/y2023/vision/calib_files/calibration_pi-7971-2_cam-23-15_ext_2023-09-09.json
new file mode 100644
index 0000000..7f4e531
--- /dev/null
+++ b/y2023/vision/calib_files/calibration_pi-7971-2_cam-23-15_ext_2023-09-09.json
@@ -0,0 +1,25 @@
+{
+ "node_name": "pi2",
+ "team_number": 7971,
+ "intrinsics": [
+ 890.219788,
+ 0.0,
+ 647.802429,
+ 0.0,
+ 890.289368,
+ 365.760376,
+ 0.0,
+ 0.0,
+ 1.0
+ ],
+ "dist_coeffs": [
+ -0.450615,
+ 0.25791,
+ 0.000498,
+ 0.000087,
+ -0.085233
+ ],
+"fixed_extrinsics": { "data": [ -0.482031, 0.00240341, 0.87615, 0.149719, -0.87612, -0.0100773, -0.481987, -0.000995038, 0.00767093, -0.999947, 0.00696306, 0.0062866, 0.0, 0.0, 0.0, 1.0 ] },
+ "calibration_timestamp": 1358499855721459233,
+ "camera_id": "23-15"
+}
diff --git a/y2023/vision/calib_files/calibration_pi-7971-3_cam-23-03_ext_2023-04-05.json b/y2023/vision/calib_files/calibration_pi-7971-3_cam-23-03_ext_2023-04-05.json
deleted file mode 100644
index f7b554c..0000000
--- a/y2023/vision/calib_files/calibration_pi-7971-3_cam-23-03_ext_2023-04-05.json
+++ /dev/null
@@ -1 +0,0 @@
-{ "node_name": "pi3", "team_number": 7971, "intrinsics": [ 892.089172, 0.0, 648.780701, 0.0, 892.362854, 342.340668, 0.0, 0.0, 1.0 ], "fixed_extrinsics": { "data": [ 0.517483, 0.00906969, 0.855645, 0.156941, -0.855687, 0.00935422, 0.517409, 0.0833421, -0.00331132, -0.999915, 0.0126013, 0.0124048, 0.0, 0.0, 0.0, 1.0 ] }, "dist_coeffs": [ -0.451178, 0.258187, 0.001071, 0.000017, -0.085526 ], "calibration_timestamp": 1358501967378322133, "camera_id": "23-03" }
diff --git a/y2023/vision/calib_files/calibration_pi-7971-2_cam-23-02_ext_2023-04-08.json b/y2023/vision/calib_files/calibration_pi-cam_23-02.json
similarity index 100%
rename from y2023/vision/calib_files/calibration_pi-7971-2_cam-23-02_ext_2023-04-08.json
rename to y2023/vision/calib_files/calibration_pi-cam_23-02.json
diff --git a/y2023/vision/calibrate_multi_cameras.cc b/y2023/vision/calibrate_multi_cameras.cc
new file mode 100644
index 0000000..aaff033
--- /dev/null
+++ b/y2023/vision/calibrate_multi_cameras.cc
@@ -0,0 +1,456 @@
+#include <numeric>
+
+#include "aos/configuration.h"
+#include "aos/events/logging/log_reader.h"
+#include "aos/events/simulated_event_loop.h"
+#include "aos/init.h"
+#include "aos/util/mcap_logger.h"
+#include "frc971/control_loops/pose.h"
+#include "frc971/control_loops/quaternion_utils.h"
+#include "frc971/vision/calibration_generated.h"
+#include "frc971/vision/charuco_lib.h"
+#include "frc971/vision/target_mapper.h"
+#include "frc971/vision/visualize_robot.h"
+// clang-format off
+// OpenCV eigen files must be included after Eigen includes
+#include "opencv2/aruco.hpp"
+#include "opencv2/calib3d.hpp"
+#include "opencv2/core/eigen.hpp"
+#include "opencv2/features2d.hpp"
+#include "opencv2/highgui.hpp"
+#include "opencv2/highgui/highgui.hpp"
+#include "opencv2/imgproc.hpp"
+// clang-format on
+#include "y2023/constants/simulated_constants_sender.h"
+#include "y2023/vision/aprilrobotics.h"
+#include "y2023/vision/vision_util.h"
+
+DEFINE_string(config, "",
+ "If set, override the log's config file with this one.");
+DEFINE_string(constants_path, "y2023/constants/constants.json",
+ "Path to the constant file");
+DEFINE_string(target_type, "charuco_diamond",
+ "Type of target being used [aruco, charuco, charuco_diamond]");
+DEFINE_int32(team_number, 0,
+ "Required: Use the calibration for a node with this team number");
+DEFINE_uint64(
+ wait_key, 1,
+ "Time in ms to wait between images (0 to wait indefinitely until click)");
+
+// Calibrate extrinsic relationship between cameras using two targets
+// seen jointly between cameras. Uses two types of information: 1)
+// when a single camera sees two targets, we estimate the pose between
+// targets, and 2) when two separate cameras each see a target, we can
+// use the pose between targets to estimate the pose between cameras.
+
+// We then create the extrinsics for the robot by starting with the
+// given extrinsic for camera 1 (between imu/robot and camera frames)
+// and then map each subsequent camera based on the data collected and
+// the extrinsic poses computed here.
+
+// TODO<Jim>: Should export a new extrinsic file for each camera
+// TODO<Jim>: Not currently using estimate from pi1->pi4-- should do full
+// estimation, and probably also include camera->imu extrinsics from all
+// cameras, not just pi1
+// TODO<Jim>: Should add ability to do this with apriltags, since they're on the
+// field
+
+namespace y2023 {
+namespace vision {
+using frc971::vision::DataAdapter;
+using frc971::vision::ImageCallback;
+using frc971::vision::PoseUtils;
+using frc971::vision::TargetMap;
+using frc971::vision::TargetMapper;
+namespace calibration = frc971::vision::calibration;
+
+static constexpr double kImagePeriodMs =
+ 1.0 / 30.0 * 1000.0; // Image capture period in ms
+
+// Change reference frame from camera to robot
+Eigen::Affine3d CameraToRobotDetection(Eigen::Affine3d H_camera_target,
+ Eigen::Affine3d extrinsics) {
+ const Eigen::Affine3d H_robot_camera = extrinsics;
+ const Eigen::Affine3d H_robot_target = H_robot_camera * H_camera_target;
+ return H_robot_target;
+}
+
+struct TimestampedPiDetection {
+ aos::distributed_clock::time_point time;
+ // Pose of target relative to robot
+ Eigen::Affine3d H_camera_target;
+ // name of pi
+ std::string pi_name;
+ int board_id;
+};
+
+TimestampedPiDetection last_observation;
+std::vector<std::pair<TimestampedPiDetection, TimestampedPiDetection>>
+ detection_list;
+std::vector<TimestampedPiDetection> two_board_extrinsics_list;
+
+// TODO<jim>: Implement variance calcs
+Eigen::Affine3d ComputeAveragePose(
+ std::vector<Eigen::Vector3d> &translation_list,
+ std::vector<Eigen::Vector4d> &rotation_list,
+ Eigen::Vector3d *translation_variance = nullptr,
+ Eigen::Vector3d *rotation_variance = nullptr) {
+ Eigen::Vector3d avg_translation =
+ std::accumulate(translation_list.begin(), translation_list.end(),
+ Eigen::Vector3d{0, 0, 0}) /
+ translation_list.size();
+
+ // TODO<Jim>: Use QuaternionMean from quaternion_utils.cc (but this
+ // requires a fixed number of quaternions to be averaged
+ Eigen::Vector4d avg_rotation =
+ std::accumulate(rotation_list.begin(), rotation_list.end(),
+ Eigen::Vector4d{0, 0, 0, 0}) /
+ rotation_list.size();
+ // Normalize, so it's a valid quaternion
+ avg_rotation = avg_rotation / avg_rotation.norm();
+ Eigen::Quaterniond avg_rotation_q{avg_rotation[0], avg_rotation[1],
+ avg_rotation[2], avg_rotation[3]};
+ Eigen::Translation3d translation(avg_translation);
+ Eigen::Affine3d return_pose = translation * avg_rotation_q;
+ if (translation_variance != nullptr) {
+ *translation_variance = Eigen::Vector3d();
+ }
+ if (rotation_variance != nullptr) {
+ LOG(INFO) << *rotation_variance;
+ }
+
+ return return_pose;
+}
+
+Eigen::Affine3d ComputeAveragePose(
+ std::vector<Eigen::Affine3d> &pose_list,
+ Eigen::Vector3d *translation_variance = nullptr,
+ Eigen::Vector3d *rotation_variance = nullptr) {
+ std::vector<Eigen::Vector3d> translation_list;
+ std::vector<Eigen::Vector4d> rotation_list;
+
+ for (Eigen::Affine3d pose : pose_list) {
+ translation_list.push_back(pose.translation());
+ Eigen::Quaterniond quat(pose.rotation().matrix());
+ rotation_list.push_back(
+ Eigen::Vector4d(quat.w(), quat.x(), quat.y(), quat.z()));
+ }
+
+ return ComputeAveragePose(translation_list, rotation_list,
+ translation_variance, rotation_variance);
+}
+
+Eigen::Affine3d ComputeAveragePose(
+ std::vector<TimestampedPiDetection> &pose_list,
+ Eigen::Vector3d *translation_variance = nullptr,
+ Eigen::Vector3d *rotation_variance = nullptr) {
+ std::vector<Eigen::Vector3d> translation_list;
+ std::vector<Eigen::Vector4d> rotation_list;
+
+ for (TimestampedPiDetection pose : pose_list) {
+ translation_list.push_back(pose.H_camera_target.translation());
+ Eigen::Quaterniond quat(pose.H_camera_target.rotation().matrix());
+ rotation_list.push_back(
+ Eigen::Vector4d(quat.w(), quat.x(), quat.y(), quat.z()));
+ }
+ return ComputeAveragePose(translation_list, rotation_list,
+ translation_variance, rotation_variance);
+}
+
+void ProcessImage(aos::EventLoop *event_loop, cv::Mat rgb_image,
+ const aos::monotonic_clock::time_point eof,
+ aos::distributed_clock::time_point distributed_eof,
+ frc971::vision::CharucoExtractor &charuco_extractor,
+ std::string pi_name) {
+ std::vector<cv::Vec4i> charuco_ids;
+ std::vector<std::vector<cv::Point2f>> charuco_corners;
+ bool valid = false;
+ std::vector<Eigen::Vector3d> rvecs_eigen;
+ std::vector<Eigen::Vector3d> tvecs_eigen;
+ charuco_extractor.ProcessImage(rgb_image, eof, event_loop->monotonic_now(),
+ charuco_ids, charuco_corners, valid,
+ rvecs_eigen, tvecs_eigen);
+
+ if (valid) {
+ if (tvecs_eigen.size() == 2) {
+ VLOG(2) << "Saw two boards in same view from " << pi_name;
+ // Handle when we see two boards at once
+ std::vector<TimestampedPiDetection> detections;
+ for (uint i = 0; i < tvecs_eigen.size(); i++) {
+ Eigen::Quaternion<double> rotation(
+ frc971::controls::ToQuaternionFromRotationVector(rvecs_eigen[i]));
+ Eigen::Translation3d translation(tvecs_eigen[i]);
+ Eigen::Affine3d H_camera_board = translation * rotation;
+ TimestampedPiDetection new_observation{
+ .time = distributed_eof,
+ .H_camera_target = H_camera_board,
+ .pi_name = pi_name,
+ .board_id = charuco_ids[i][0]};
+ detections.emplace_back(new_observation);
+ }
+ Eigen::Affine3d H_boardA_boardB =
+ detections[0].H_camera_target.inverse() *
+ detections[1].H_camera_target;
+
+ TimestampedPiDetection board_extrinsic{.time = distributed_eof,
+ .H_camera_target = H_boardA_boardB,
+ .pi_name = pi_name,
+ .board_id = charuco_ids[0][0]};
+
+ // Make sure we've got them in the right order (sorted by id)
+ if (detections[0].board_id < detections[1].board_id) {
+ two_board_extrinsics_list.push_back(board_extrinsic);
+ } else {
+ // Flip them around
+ board_extrinsic.H_camera_target =
+ board_extrinsic.H_camera_target.inverse();
+ board_extrinsic.board_id = charuco_ids[1][0];
+ two_board_extrinsics_list.push_back(board_extrinsic);
+ }
+ } else {
+ VLOG(1) << "Saw single board in camera " << pi_name;
+ Eigen::Quaternion<double> rotation(
+ frc971::controls::ToQuaternionFromRotationVector(rvecs_eigen[0]));
+ Eigen::Translation3d translation(tvecs_eigen[0]);
+ Eigen::Affine3d H_camera_board = translation * rotation;
+ TimestampedPiDetection new_observation{.time = distributed_eof,
+ .H_camera_target = H_camera_board,
+ .pi_name = pi_name,
+ .board_id = charuco_ids[0][0]};
+
+ VLOG(2) << "Checking versus last result from " << last_observation.pi_name
+ << " at time " << last_observation.time << " with delta time = "
+ << std::abs((distributed_eof - last_observation.time).count());
+ // Only take two observations if they're near enough to each other
+ // in time. This should be within +/- kImagePeriodMs of each other (e.g.,
+ // +/-16.666ms for 30 Hz capture). This should make sure
+ // we're always getting the closest images, and not miss too many possible
+ // pairs, between cameras
+ // TODO<Jim>: Should also check that (rotational) velocity of the robot is
+ // small
+ if (std::abs((distributed_eof - last_observation.time).count()) <
+ static_cast<int>(kImagePeriodMs / 2.0 * 1000000)) {
+ Eigen::Affine3d H_camera1_board = last_observation.H_camera_target;
+ Eigen::Affine3d H_camera1_camera2 =
+ H_camera1_board * H_camera_board.inverse();
+ VLOG(1) << "Storing observation between " << last_observation.pi_name
+ << ", target " << last_observation.board_id << " and "
+ << new_observation.pi_name << ", target "
+ << new_observation.board_id << " is "
+ << H_camera1_camera2.matrix();
+ // Sort by pi numbering
+ if (last_observation.pi_name < new_observation.pi_name) {
+ detection_list.push_back(
+ std::pair(last_observation, new_observation));
+ } else if (last_observation.pi_name > new_observation.pi_name) {
+ detection_list.push_back(
+ std::pair(new_observation, last_observation));
+ } else {
+ LOG(WARNING) << "Got 2 observations in a row from same pi. Probably "
+ "not too much of an issue???";
+ }
+ } else {
+ VLOG(2) << "Storing observation for " << pi_name << " at time "
+ << distributed_eof;
+ last_observation = new_observation;
+ }
+ }
+ }
+ if (FLAGS_visualize) {
+ std::string image_name = pi_name + " Image";
+ cv::Mat rgb_small;
+ cv::resize(rgb_image, rgb_small, cv::Size(), 0.5, 0.5);
+ cv::imshow(image_name, rgb_small);
+ cv::waitKey(FLAGS_wait_key);
+ }
+}
+
+void ExtrinsicsMain(int argc, char *argv[]) {
+ std::vector<DataAdapter::TimestampedDetection> timestamped_target_detections;
+
+ std::optional<aos::FlatbufferDetachedBuffer<aos::Configuration>> config =
+ (FLAGS_config.empty()
+ ? std::nullopt
+ : std::make_optional(aos::configuration::ReadConfig(FLAGS_config)));
+
+ // open logfiles
+ aos::logger::LogReader reader(
+ aos::logger::SortParts(aos::logger::FindLogs(argc, argv)),
+ config.has_value() ? &config->message() : nullptr);
+
+ constexpr size_t kNumPis = 4;
+ for (size_t i = 1; i <= kNumPis; i++) {
+ reader.RemapLoggedChannel(absl::StrFormat("/pi%u/camera", i),
+ "frc971.vision.TargetMap");
+ reader.RemapLoggedChannel(absl::StrFormat("/pi%u/camera", i),
+ "foxglove.ImageAnnotations");
+ reader.RemapLoggedChannel(absl::StrFormat("/pi%u/constants", i),
+ "y2023.Constants");
+ }
+ reader.RemapLoggedChannel("/imu/constants", "y2023.Constants");
+ reader.Register();
+
+ SendSimulationConstants(reader.event_loop_factory(), FLAGS_team_number,
+ FLAGS_constants_path);
+
+ LOG(INFO) << "Using target type " << FLAGS_target_type;
+ std::vector<std::string> node_list;
+ node_list.push_back("pi1");
+ node_list.push_back("pi2");
+ node_list.push_back("pi3");
+ node_list.push_back("pi4");
+
+ std::vector<std::unique_ptr<aos::EventLoop>> detection_event_loops;
+ std::vector<frc971::vision::CharucoExtractor *> charuco_extractors;
+ std::vector<frc971::vision::ImageCallback *> image_callbacks;
+ std::vector<Eigen::Affine3d> default_extrinsics;
+
+ for (uint i = 0; i < node_list.size(); i++) {
+ std::string node = node_list[i];
+ const aos::Node *pi =
+ aos::configuration::GetNode(reader.configuration(), node.c_str());
+
+ detection_event_loops.emplace_back(
+ reader.event_loop_factory()->MakeEventLoop(
+ (node + "_detection").c_str(), pi));
+
+ frc971::constants::ConstantsFetcher<y2023::Constants> constants_fetcher(
+ detection_event_loops.back().get());
+ aos::FlatbufferDetachedBuffer<calibration::CameraCalibration> intrinsics =
+ aos::RecursiveCopyFlatBuffer(y2023::vision::FindCameraCalibration(
+ constants_fetcher.constants(), node));
+
+ const calibration::CameraCalibration *calibration =
+ FindCameraCalibration(constants_fetcher.constants(), node);
+
+ frc971::vision::TargetType target_type =
+ frc971::vision::TargetTypeFromString(FLAGS_target_type);
+ frc971::vision::CharucoExtractor *charuco_ext =
+ new frc971::vision::CharucoExtractor(calibration, target_type);
+ charuco_extractors.emplace_back(charuco_ext);
+
+ cv::Mat extrinsics_cv = CameraExtrinsics(calibration).value();
+ Eigen::Matrix4d extrinsics_matrix;
+ cv::cv2eigen(extrinsics_cv, extrinsics_matrix);
+ const auto ext_H_robot_pi = Eigen::Affine3d(extrinsics_matrix);
+ default_extrinsics.emplace_back(ext_H_robot_pi);
+
+ LOG(INFO) << "Got extrinsics for " << node << " as\n"
+ << default_extrinsics.back().matrix();
+
+ frc971::vision::ImageCallback *image_callback =
+ new frc971::vision::ImageCallback(
+ detection_event_loops[i].get(), "/" + node_list[i] + "/camera",
+ [&reader, &charuco_extractors, &detection_event_loops, &node_list,
+ i](cv::Mat rgb_image, const aos::monotonic_clock::time_point eof) {
+ aos::distributed_clock::time_point pi_distributed_time =
+ reader.event_loop_factory()
+ ->GetNodeEventLoopFactory(
+ detection_event_loops[i].get()->node())
+ ->ToDistributedClock(eof);
+ ProcessImage(detection_event_loops[i].get(), rgb_image, eof,
+ pi_distributed_time, *charuco_extractors[i],
+ node_list[i]);
+ });
+
+ image_callbacks.emplace_back(image_callback);
+ }
+
+ const auto ext_H_robot_piA = default_extrinsics[0];
+ const auto ext_H_robot_piB = default_extrinsics[1];
+
+ reader.event_loop_factory()->Run();
+
+ for (auto node : node_list) {
+ std::vector<TimestampedPiDetection> pose_list;
+ for (auto ext : two_board_extrinsics_list) {
+ if (ext.pi_name == node) {
+ pose_list.push_back(ext);
+ }
+ }
+ Eigen::Affine3d avg_pose = ComputeAveragePose(pose_list);
+ VLOG(1) << "Estimate from " << node << " with " << pose_list.size()
+ << " observations is:\n"
+ << avg_pose.matrix();
+ }
+ Eigen::Affine3d avg_pose = ComputeAveragePose(two_board_extrinsics_list);
+ LOG(INFO) << "Estimate of two board pose using all nodes with "
+ << two_board_extrinsics_list.size() << " observations is:\n"
+ << avg_pose.matrix() << "\n";
+
+ LOG(INFO) << "Got " << detection_list.size() << " extrinsic observations";
+ Eigen::Affine3d H_target0_target1 = avg_pose;
+ int base_target_id = detection_list[0].first.board_id;
+ std::vector<Eigen::Affine3d> H_cameraA_cameraB_list;
+ std::vector<Eigen::Affine3d> updated_extrinsics;
+ // Use the first node's extrinsics as our base, and fix from there
+ updated_extrinsics.push_back(default_extrinsics[0]);
+ LOG(INFO) << "Default extrinsic for node " << node_list[0] << " is "
+ << default_extrinsics[0].matrix();
+ for (uint i = 0; i < node_list.size() - 1; i++) {
+ H_cameraA_cameraB_list.clear();
+ for (auto [poseA, poseB] : detection_list) {
+ if ((poseA.pi_name == node_list[i]) &&
+ (poseB.pi_name == node_list[i + 1])) {
+ Eigen::Affine3d H_cameraA_target0 = poseA.H_camera_target;
+ // If this pose isn't referenced to target0, correct that
+ if (poseA.board_id != base_target_id) {
+ H_cameraA_target0 = H_cameraA_target0 * H_target0_target1.inverse();
+ }
+
+ Eigen::Affine3d H_cameraB_target0 = poseB.H_camera_target;
+ if (poseB.board_id != base_target_id) {
+ H_cameraB_target0 = H_cameraB_target0 * H_target0_target1.inverse();
+ }
+ Eigen::Affine3d H_cameraA_cameraB =
+ H_cameraA_target0 * H_cameraB_target0.inverse();
+ H_cameraA_cameraB_list.push_back(H_cameraA_cameraB);
+ }
+ }
+ CHECK(H_cameraA_cameraB_list.size() > 0)
+ << "Failed with zero poses for node " << node_list[i];
+ if (H_cameraA_cameraB_list.size() > 0) {
+ Eigen::Affine3d avg_H_cameraA_cameraB =
+ ComputeAveragePose(H_cameraA_cameraB_list);
+ LOG(INFO) << "From " << node_list[i] << " to " << node_list[i + 1]
+ << " found " << H_cameraA_cameraB_list.size()
+ << " observations, and the average pose is:\n"
+ << avg_H_cameraA_cameraB.matrix();
+ Eigen::Affine3d default_H_cameraA_camera_B =
+ default_extrinsics[i].inverse() * default_extrinsics[i + 1];
+ LOG(INFO) << "Compare this to that from default values:\n"
+ << default_H_cameraA_camera_B.matrix();
+ // Next extrinsic is just previous one * avg_delta_pose
+ Eigen::Affine3d next_extrinsic =
+ updated_extrinsics.back() * avg_H_cameraA_cameraB;
+ LOG(INFO)
+ << "Difference between averaged and default delta poses has |T| = "
+ << (avg_H_cameraA_cameraB * default_H_cameraA_camera_B.inverse())
+ .translation()
+ .norm()
+ << " and |R| = "
+ << Eigen::AngleAxisd(
+ (avg_H_cameraA_cameraB * default_H_cameraA_camera_B.inverse())
+ .rotation())
+ .angle();
+ updated_extrinsics.push_back(next_extrinsic);
+ LOG(INFO) << "Default Extrinsic for " << node_list[i + 1] << " is \n"
+ << default_extrinsics[i + 1].matrix();
+ LOG(INFO) << "--> Updated Extrinsic for " << node_list[i + 1] << " is \n"
+ << next_extrinsic.matrix();
+ }
+ }
+
+ // Cleanup
+ for (uint i = 0; i < image_callbacks.size(); i++) {
+ delete charuco_extractors[i];
+ delete image_callbacks[i];
+ }
+}
+} // namespace vision
+} // namespace y2023
+
+int main(int argc, char **argv) {
+ aos::InitGoogle(&argc, &argv);
+ y2023::vision::ExtrinsicsMain(argc, argv);
+}
diff --git a/y2023/vision/target_mapping.cc b/y2023/vision/target_mapping.cc
index 2ca6f66..e4099b3 100644
--- a/y2023/vision/target_mapping.cc
+++ b/y2023/vision/target_mapping.cc
@@ -20,38 +20,39 @@
#include "y2023/vision/aprilrobotics.h"
#include "y2023/vision/vision_util.h"
-DEFINE_string(json_path, "y2023/vision/maps/target_map.json",
- "Specify path for json with initial pose guesses.");
DEFINE_string(config, "",
"If set, override the log's config file with this one.");
DEFINE_string(constants_path, "y2023/constants/constants.json",
"Path to the constant file");
-DEFINE_string(output_dir, "y2023/vision/maps",
- "Directory to write solved target map to");
+DEFINE_string(dump_constraints_to, "/tmp/mapping_constraints.txt",
+ "Write the target constraints to this path");
+DEFINE_string(dump_stats_to, "/tmp/mapping_stats.txt",
+ "Write the mapping stats to this path");
DEFINE_string(field_name, "charged_up",
"Field name, for the output json filename and flatbuffer field");
-DEFINE_int32(team_number, 0,
- "Required: Use the calibration for a node with this team number");
-DEFINE_string(mcap_output_path, "/tmp/log.mcap", "Log to output.");
-DEFINE_string(pi, "pi1", "Pi name to generate mcap log for; defaults to pi1.");
+DEFINE_string(json_path, "y2023/vision/maps/target_map.json",
+ "Specify path for json with initial pose guesses.");
DEFINE_double(max_pose_error, 1e-6,
"Throw out target poses with a higher pose error than this");
DEFINE_double(
max_pose_error_ratio, 0.4,
"Throw out target poses with a higher pose error ratio than this");
-DEFINE_uint64(wait_key, 0,
- "Time in ms to wait between images, if no click (0 to wait "
- "indefinitely until click).");
+DEFINE_string(mcap_output_path, "", "Log to output.");
+DEFINE_string(output_dir, "y2023/vision/maps",
+ "Directory to write solved target map to");
DEFINE_double(pause_on_distance, 1.0,
"Pause if two consecutive implied robot positions differ by more "
"than this many meters");
+DEFINE_string(pi, "pi1", "Pi name to generate mcap log for; defaults to pi1.");
DEFINE_uint64(skip_to, 1,
"Start at combined image of this number (1 is the first image)");
DEFINE_bool(solve, true, "Whether to solve for the field's target map.");
-DEFINE_string(dump_constraints_to, "/tmp/mapping_constraints.txt",
- "Write the target constraints to this path");
-DEFINE_string(dump_stats_to, "/tmp/mapping_stats.txt",
- "Write the target constraints to this path");
+DEFINE_int32(team_number, 0,
+ "Required: Use the calibration for a node with this team number");
+DEFINE_uint64(wait_key, 1,
+ "Time in ms to wait between images, if no click (0 to wait "
+ "indefinitely until click).");
+
DECLARE_int32(frozen_target_id);
DECLARE_int32(min_target_id);
DECLARE_int32(max_target_id);
@@ -73,7 +74,6 @@
class TargetMapperReplay {
public:
TargetMapperReplay(aos::logger::LogReader *reader);
- ~TargetMapperReplay();
// Solves for the target poses with the accumulated detections if FLAGS_solve.
void MaybeSolve();
@@ -102,7 +102,6 @@
aos::logger::LogReader *reader_;
// April tag detections from all pis
std::vector<DataAdapter::TimestampedDetection> timestamped_target_detections_;
- std::vector<std::unique_ptr<AprilRoboticsDetector>> detectors_;
VisualizeRobot vis_robot_;
// Set of node names which are currently drawn on the display
@@ -146,7 +145,6 @@
TargetMapperReplay::TargetMapperReplay(aos::logger::LogReader *reader)
: reader_(reader),
timestamped_target_detections_(),
- detectors_(),
vis_robot_(cv::Size(1280, 1000)),
drawn_nodes_(),
display_count_(0),
@@ -202,13 +200,6 @@
}
}
-TargetMapperReplay::~TargetMapperReplay() {
- // Clean up all the pointers
- for (auto &detector_ptr : detectors_) {
- detector_ptr.reset();
- }
-}
-
// Add detected apriltag poses relative to the robot to
// timestamped_target_detections
void TargetMapperReplay::HandleAprilTags(
@@ -268,8 +259,8 @@
.id = static_cast<TargetMapper::TargetId>(target_pose.id)});
if (FLAGS_visualize_solver) {
- // If we've already drawn in the current image,
- // display it before clearing and adding the new poses
+ // If we've already drawn this node_name in the current image,
+ // display the image before clearing and adding the new poses
if (drawn_nodes_.count(node_name) != 0) {
display_count_++;
cv::putText(vis_robot_.image_,
@@ -278,10 +269,20 @@
cv::Scalar(255, 255, 255));
if (display_count_ >= FLAGS_skip_to) {
+ VLOG(1) << "Showing image for node " << node_name
+ << " since we've drawn it already";
cv::imshow("View", vis_robot_.image_);
- cv::waitKey(max_delta_T_world_robot_ > FLAGS_pause_on_distance
- ? 0
- : FLAGS_wait_key);
+ // Pause if delta_T is too large, but only after first image (to make
+ // sure the delta's are correct
+ if (max_delta_T_world_robot_ > FLAGS_pause_on_distance &&
+ display_count_ > 1) {
+ LOG(INFO) << "Pausing since the delta between robot estimates is "
+ << max_delta_T_world_robot_ << " which is > threshold of "
+ << FLAGS_pause_on_distance;
+ cv::waitKey(0);
+ } else {
+ cv::waitKey(FLAGS_wait_key);
+ }
max_delta_T_world_robot_ = 0.0;
} else {
VLOG(1) << "At poses #" << std::to_string(display_count_);
@@ -293,21 +294,19 @@
Eigen::Affine3d H_world_target = PoseUtils::Pose3dToAffine3d(
kFixedTargetMapper.GetTargetPoseById(target_pose_fbs->id())->pose);
Eigen::Affine3d H_world_robot = H_world_target * H_robot_target.inverse();
- VLOG(2) << node_name << ", " << target_pose_fbs->id()
+ VLOG(2) << node_name << ", id " << target_pose_fbs->id()
<< ", t = " << pi_distributed_time
<< ", pose_error = " << target_pose_fbs->pose_error()
<< ", pose_error_ratio = " << target_pose_fbs->pose_error_ratio()
- << ", robot_pos (x,y,z) + "
+ << ", robot_pos (x,y,z) = "
<< H_world_robot.translation().transpose();
- label << "id " << target_pose_fbs->id() << ": err "
+ label << "id " << target_pose_fbs->id() << ": err (% of max): "
<< (target_pose_fbs->pose_error() / FLAGS_max_pose_error)
- << " ratio " << target_pose_fbs->pose_error_ratio() << " ";
+ << " err_ratio: " << target_pose_fbs->pose_error_ratio() << " ";
- vis_robot_.DrawRobotOutline(H_world_robot,
- std::to_string(target_pose_fbs->id()),
+ vis_robot_.DrawRobotOutline(H_world_robot, node_name,
kPiColors.at(node_name));
-
vis_robot_.DrawFrameAxes(H_world_target,
std::to_string(target_pose_fbs->id()),
kPiColors.at(node_name));
@@ -318,27 +317,37 @@
max_delta_T_world_robot_ =
std::max(delta_T_world_robot, max_delta_T_world_robot_);
+ VLOG(1) << "Drew in info for robot " << node_name << " and target #"
+ << target_pose_fbs->id();
drew = true;
last_draw_time_ = pi_distributed_time;
last_H_world_robot_ = H_world_robot;
}
}
- if (drew) {
- size_t pi_number =
- static_cast<size_t>(node_name[node_name.size() - 1] - '0');
- cv::putText(vis_robot_.image_, label.str(),
- cv::Point(10, 10 + 20 * pi_number), cv::FONT_HERSHEY_PLAIN, 1.0,
- kPiColors.at(node_name));
- drawn_nodes_.emplace(node_name);
- } else if (pi_distributed_time - last_draw_time_ >
- std::chrono::milliseconds(30) &&
- display_count_ >= FLAGS_skip_to) {
- // Clear the image if we haven't draw in a while
- vis_robot_.ClearImage();
- cv::imshow("View", vis_robot_.image_);
- cv::waitKey(FLAGS_wait_key);
- max_delta_T_world_robot_ = 0.0;
+ if (FLAGS_visualize_solver) {
+ if (drew) {
+ // Collect all the labels from a given node, and add the text
+ size_t pi_number =
+ static_cast<size_t>(node_name[node_name.size() - 1] - '0');
+ cv::putText(vis_robot_.image_, label.str(),
+ cv::Point(10, 10 + 20 * pi_number), cv::FONT_HERSHEY_PLAIN,
+ 1.0, kPiColors.at(node_name));
+
+ drawn_nodes_.emplace(node_name);
+ } else if (pi_distributed_time - last_draw_time_ >
+ std::chrono::milliseconds(30) &&
+ display_count_ >= FLAGS_skip_to) {
+ cv::putText(vis_robot_.image_, "No detections", cv::Point(10, 0),
+ cv::FONT_HERSHEY_PLAIN, 1.0, kPiColors.at(node_name));
+ // Display and clear the image if we haven't draw in a while
+ VLOG(1) << "Displaying image due to time lapse";
+ cv::imshow("View", vis_robot_.image_);
+ cv::waitKey(FLAGS_wait_key);
+ vis_robot_.ClearImage();
+ max_delta_T_world_robot_ = 0.0;
+ drawn_nodes_.clear();
+ }
}
}
@@ -385,6 +394,8 @@
}),
target_constraints.end());
+ LOG(INFO) << "Solving for locations of tags with "
+ << target_constraints.size() << " constraints";
TargetMapper mapper(FLAGS_json_path, target_constraints);
mapper.Solve(FLAGS_field_name, FLAGS_output_dir);
diff --git a/y2023_bot3/control_loops/drivetrain/drivetrain_base.h b/y2023_bot3/control_loops/drivetrain/drivetrain_base.h
index 0c557c8..6922ea6 100644
--- a/y2023_bot3/control_loops/drivetrain/drivetrain_base.h
+++ b/y2023_bot3/control_loops/drivetrain/drivetrain_base.h
@@ -7,8 +7,8 @@
namespace control_loops {
namespace drivetrain {
-const ::frc971::control_loops::drivetrain::DrivetrainConfig<double>
- &GetDrivetrainConfig();
+const ::frc971::control_loops::drivetrain::DrivetrainConfig<double> &
+GetDrivetrainConfig();
} // namespace drivetrain
} // namespace control_loops