Merge changes Ie1db72c4,Ice8501f4
* changes:
Adjust log_to_mcap to use LogReader::OnStart
Add extra comments for various start/end handlers
diff --git a/WORKSPACE b/WORKSPACE
index 0ab6bd5..9c4718b 100644
--- a/WORKSPACE
+++ b/WORKSPACE
@@ -1138,8 +1138,8 @@
"aarch64-unknown-linux-gnu",
],
register_toolchain = False,
- rustfmt_version = "1.62.0",
- version = "1.62.0",
+ rustfmt_version = "1.70.0",
+ version = "1.70.0",
)
# Flatbuffers
diff --git a/aos/events/event_loop_param_test.cc.rej b/aos/events/event_loop_param_test.cc.rej
deleted file mode 100644
index c03b83d..0000000
--- a/aos/events/event_loop_param_test.cc.rej
+++ /dev/null
@@ -1,300 +0,0 @@
-diff a/aos/events/event_loop_param_test.cc b/aos/events/event_loop_param_test.cc (rejected hunks)
-@@ -1923,6 +1923,298 @@
- EXPECT_GT(expected_times[expected_times.size() / 2], average_time - kEpsilon);
- }
-
-+// Tests that a phased loop responds correctly to a changing offset; sweep
-+// across a variety of potential offset changes, to ensure that we are
-+// exercising a variety of potential cases.
-+TEST_P(AbstractEventLoopTest, PhasedLoopChangingOffsetSweep) {
-+ const chrono::milliseconds kInterval = chrono::milliseconds(1000);
-+ const int kCount = 5;
-+
-+ auto loop1 = MakePrimary();
-+
-+ std::vector<aos::monotonic_clock::duration> offset_options;
-+ for (int ii = 0; ii < kCount; ++ii) {
-+ offset_options.push_back(ii * kInterval / kCount);
-+ }
-+ std::vector<aos::monotonic_clock::duration> offset_sweep;
-+ // Run over all the pair-wise combinations of offsets.
-+ for (int ii = 0; ii < kCount; ++ii) {
-+ for (int jj = 0; jj < kCount; ++jj) {
-+ offset_sweep.push_back(offset_options.at(ii));
-+ offset_sweep.push_back(offset_options.at(jj));
-+ }
-+ }
-+
-+ std::vector<::aos::monotonic_clock::time_point> expected_times;
-+
-+ PhasedLoopHandler *phased_loop;
-+
-+ // Run kCount iterations.
-+ int counter = 0;
-+ phased_loop = loop1->AddPhasedLoop(
-+ [&phased_loop, &expected_times, &loop1, this, kInterval, &counter,
-+ offset_sweep](int count) {
-+ EXPECT_EQ(count, 1);
-+ expected_times.push_back(loop1->context().monotonic_event_time);
-+
-+ counter++;
-+
-+ if (counter == offset_sweep.size()) {
-+ LOG(INFO) << "Exiting";
-+ this->Exit();
-+ return;
-+ }
-+
-+ phased_loop->set_interval_and_offset(kInterval,
-+ offset_sweep.at(counter));
-+ },
-+ kInterval, offset_sweep.at(0));
-+
-+ Run();
-+ ASSERT_EQ(expected_times.size(), offset_sweep.size());
-+ for (size_t ii = 1; ii < expected_times.size(); ++ii) {
-+ EXPECT_LE(expected_times.at(ii) - expected_times.at(ii - 1), kInterval);
-+ }
-+}
-+
-+// Tests that a phased loop responds correctly to being rescheduled with now
-+// equal to a time in the past.
-+TEST_P(AbstractEventLoopTest, PhasedLoopRescheduleInPast) {
-+ const chrono::milliseconds kOffset = chrono::milliseconds(400);
-+ const chrono::milliseconds kInterval = chrono::milliseconds(1000);
-+
-+ auto loop1 = MakePrimary();
-+
-+ std::vector<::aos::monotonic_clock::time_point> expected_times;
-+
-+ PhasedLoopHandler *phased_loop;
-+
-+ int expected_count = 1;
-+
-+ // Set up a timer that will get run immediately after the phased loop and
-+ // which will attempt to reschedule the phased loop to just before now. This
-+ // should succeed, but will result in 0 cycles elapsing.
-+ TimerHandler *manager_timer =
-+ loop1->AddTimer([&phased_loop, &loop1, &expected_count, this]() {
-+ if (expected_count == 0) {
-+ LOG(INFO) << "Exiting";
-+ this->Exit();
-+ return;
-+ }
-+ phased_loop->Reschedule(loop1->context().monotonic_event_time -
-+ std::chrono::nanoseconds(1));
-+ expected_count = 0;
-+ });
-+
-+ phased_loop = loop1->AddPhasedLoop(
-+ [&expected_count, &expected_times, &loop1, manager_timer](int count) {
-+ EXPECT_EQ(count, expected_count);
-+ expected_times.push_back(loop1->context().monotonic_event_time);
-+
-+ manager_timer->Setup(loop1->context().monotonic_event_time);
-+ },
-+ kInterval, kOffset);
-+ phased_loop->set_name("Test loop");
-+ manager_timer->set_name("Manager timer");
-+
-+ Run();
-+
-+ ASSERT_EQ(2u, expected_times.size());
-+ ASSERT_EQ(expected_times[0], expected_times[1]);
-+}
-+
-+// Tests that a phased loop responds correctly to being rescheduled at the time
-+// when it should be triggering (it should kick the trigger to the next cycle).
-+TEST_P(AbstractEventLoopTest, PhasedLoopRescheduleNow) {
-+ const chrono::milliseconds kOffset = chrono::milliseconds(400);
-+ const chrono::milliseconds kInterval = chrono::milliseconds(1000);
-+
-+ auto loop1 = MakePrimary();
-+
-+ std::vector<::aos::monotonic_clock::time_point> expected_times;
-+
-+ PhasedLoopHandler *phased_loop;
-+
-+ bool should_exit = false;
-+ // Set up a timer that will get run immediately after the phased loop and
-+ // which will attempt to reschedule the phased loop to now. This should
-+ // succeed, but will result in no change to the expected behavior (since this
-+ // is the same thing that is actually done internally).
-+ TimerHandler *manager_timer =
-+ loop1->AddTimer([&phased_loop, &loop1, &should_exit, this]() {
-+ if (should_exit) {
-+ LOG(INFO) << "Exiting";
-+ this->Exit();
-+ return;
-+ }
-+ phased_loop->Reschedule(loop1->context().monotonic_event_time);
-+ should_exit = true;
-+ });
-+
-+ phased_loop = loop1->AddPhasedLoop(
-+ [&expected_times, &loop1, manager_timer](int count) {
-+ EXPECT_EQ(count, 1);
-+ expected_times.push_back(loop1->context().monotonic_event_time);
-+
-+ manager_timer->Setup(loop1->context().monotonic_event_time);
-+ },
-+ kInterval, kOffset);
-+ phased_loop->set_name("Test loop");
-+ manager_timer->set_name("Manager timer");
-+
-+ Run();
-+
-+ ASSERT_EQ(2u, expected_times.size());
-+ ASSERT_EQ(expected_times[0] + kInterval, expected_times[1]);
-+}
-+
-+// Tests that a phased loop responds correctly to being rescheduled at a time in
-+// the distant future.
-+TEST_P(AbstractEventLoopTest, PhasedLoopRescheduleFuture) {
-+ const chrono::milliseconds kOffset = chrono::milliseconds(400);
-+ const chrono::milliseconds kInterval = chrono::milliseconds(1000);
-+
-+ auto loop1 = MakePrimary();
-+
-+ std::vector<::aos::monotonic_clock::time_point> expected_times;
-+
-+ PhasedLoopHandler *phased_loop;
-+
-+ bool should_exit = false;
-+ int expected_count = 1;
-+ TimerHandler *manager_timer = loop1->AddTimer(
-+ [&expected_count, &phased_loop, &loop1, &should_exit, this, kInterval]() {
-+ if (should_exit) {
-+ LOG(INFO) << "Exiting";
-+ this->Exit();
-+ return;
-+ }
-+ expected_count = 10;
-+ // Knock off 1 ns, since the scheduler rounds up when it is
-+ // scheduled to exactly a loop time.
-+ phased_loop->Reschedule(loop1->context().monotonic_event_time +
-+ kInterval * expected_count -
-+ std::chrono::nanoseconds(1));
-+ should_exit = true;
-+ });
-+
-+ phased_loop = loop1->AddPhasedLoop(
-+ [&expected_times, &loop1, manager_timer, &expected_count](int count) {
-+ EXPECT_EQ(count, expected_count);
-+ expected_times.push_back(loop1->context().monotonic_event_time);
-+
-+ manager_timer->Setup(loop1->context().monotonic_event_time);
-+ },
-+ kInterval, kOffset);
-+ phased_loop->set_name("Test loop");
-+ manager_timer->set_name("Manager timer");
-+
-+ Run();
-+
-+ ASSERT_EQ(2u, expected_times.size());
-+ ASSERT_EQ(expected_times[0] + expected_count * kInterval, expected_times[1]);
-+}
-+
-+// Tests that a phased loop responds correctly to having its phase offset
-+// incremented and then being scheduled after a set time, exercising a pattern
-+// where a phased loop's offset is changed while trying to maintain the trigger
-+// at a consistent period.
-+TEST_P(AbstractEventLoopTest, PhasedLoopRescheduleWithLaterOffset) {
-+ const chrono::milliseconds kOffset = chrono::milliseconds(400);
-+ const chrono::milliseconds kInterval = chrono::milliseconds(1000);
-+
-+ auto loop1 = MakePrimary();
-+
-+ std::vector<::aos::monotonic_clock::time_point> expected_times;
-+
-+ PhasedLoopHandler *phased_loop;
-+
-+ bool should_exit = false;
-+ TimerHandler *manager_timer = loop1->AddTimer(
-+ [&phased_loop, &loop1, &should_exit, this, kInterval, kOffset]() {
-+ if (should_exit) {
-+ LOG(INFO) << "Exiting";
-+ this->Exit();
-+ return;
-+ }
-+ // Schedule the next callback to be strictly later than the current time
-+ // + interval / 2, to ensure a consistent frequency.
-+ monotonic_clock::time_point half_time =
-+ loop1->context().monotonic_event_time + kInterval / 2;
-+ phased_loop->set_interval_and_offset(
-+ kInterval, kOffset + std::chrono::nanoseconds(1), half_time);
-+ phased_loop->Reschedule(half_time);
-+ should_exit = true;
-+ });
-+
-+ phased_loop = loop1->AddPhasedLoop(
-+ [&expected_times, &loop1, manager_timer](int count) {
-+ EXPECT_EQ(1, count);
-+ expected_times.push_back(loop1->context().monotonic_event_time);
-+
-+ manager_timer->Setup(loop1->context().monotonic_event_time);
-+ },
-+ kInterval, kOffset);
-+ phased_loop->set_name("Test loop");
-+ manager_timer->set_name("Manager timer");
-+
-+ Run();
-+
-+ ASSERT_EQ(2u, expected_times.size());
-+ ASSERT_EQ(expected_times[0] + kInterval + std::chrono::nanoseconds(1),
-+ expected_times[1]);
-+}
-+
-+// Tests that a phased loop responds correctly to having its phase offset
-+// decremented and then being scheduled after a set time, exercising a pattern
-+// where a phased loop's offset is changed while trying to maintain the trigger
-+// at a consistent period.
-+TEST_P(AbstractEventLoopTest, PhasedLoopRescheduleWithEarlierOffset) {
-+ const chrono::milliseconds kOffset = chrono::milliseconds(400);
-+ const chrono::milliseconds kInterval = chrono::milliseconds(1000);
-+
-+ auto loop1 = MakePrimary();
-+
-+ std::vector<::aos::monotonic_clock::time_point> expected_times;
-+
-+ PhasedLoopHandler *phased_loop;
-+
-+ bool should_exit = false;
-+ TimerHandler *manager_timer = loop1->AddTimer(
-+ [&phased_loop, &loop1, &should_exit, this, kInterval, kOffset]() {
-+ if (should_exit) {
-+ LOG(INFO) << "Exiting";
-+ this->Exit();
-+ return;
-+ }
-+ // Schedule the next callback to be strictly later than the current time
-+ // + interval / 2, to ensure a consistent frequency.
-+ const aos::monotonic_clock::time_point half_time =
-+ loop1->context().monotonic_event_time + kInterval / 2;
-+ phased_loop->set_interval_and_offset(
-+ kInterval, kOffset - std::chrono::nanoseconds(1), half_time);
-+ phased_loop->Reschedule(half_time);
-+ should_exit = true;
-+ });
-+
-+ phased_loop = loop1->AddPhasedLoop(
-+ [&expected_times, &loop1, manager_timer](int count) {
-+ EXPECT_EQ(1, count);
-+ expected_times.push_back(loop1->context().monotonic_event_time);
-+
-+ manager_timer->Setup(loop1->context().monotonic_event_time);
-+ },
-+ kInterval, kOffset);
-+ phased_loop->set_name("Test loop");
-+ manager_timer->set_name("Manager timer");
-+
-+ Run();
-+
-+ ASSERT_EQ(2u, expected_times.size());
-+ ASSERT_EQ(expected_times[0] + kInterval - std::chrono::nanoseconds(1),
-+ expected_times[1]);
-+}
-+
- // Tests that senders count correctly in the timing report.
- TEST_P(AbstractEventLoopTest, SenderTimingReport) {
- gflags::FlagSaver flag_saver;
diff --git a/aos/events/event_loop_runtime.rs b/aos/events/event_loop_runtime.rs
index 023cfb6..7f58dbb 100644
--- a/aos/events/event_loop_runtime.rs
+++ b/aos/events/event_loop_runtime.rs
@@ -61,7 +61,7 @@
};
use cxx::UniquePtr;
use flatbuffers::{root_unchecked, Follow, FollowWith, FullyQualifiedName};
-use futures::{future::FusedFuture, never::Never};
+use futures::{future::pending, future::FusedFuture, never::Never};
use thiserror::Error;
use uuid::Uuid;
@@ -69,6 +69,7 @@
use aos_configuration::{ChannelLookupError, ConfigurationExt};
pub use aos_uuid::UUID;
+pub use ffi::aos::ExitHandle as CppExitHandle;
autocxx::include_cpp! (
#include "aos/events/event_loop_runtime.h"
@@ -82,6 +83,7 @@
generate!("aos::FetcherForRust")
generate!("aos::OnRunForRust")
generate!("aos::EventLoopRuntime")
+generate!("aos::ExitHandle")
subclass!("aos::ApplicationFuture", RustApplicationFuture)
@@ -1194,3 +1196,28 @@
}
use panic_waker::panic_waker;
+
+pub struct ExitHandle(UniquePtr<CppExitHandle>);
+
+impl ExitHandle {
+ /// Exits the EventLoops represented by this handle. You probably want to immediately return
+ /// from the context this is called in. Awaiting [`exit`] instead of using this function is an
+ /// easy way to do that.
+ pub fn exit_sync(mut self) {
+ self.0.as_mut().unwrap().Exit();
+ }
+
+ /// Exits the EventLoops represented by this handle, and never returns. Immediately awaiting
+ /// this from a [`EventLoopRuntime::spawn`]ed task is usually what you want, it will ensure
+ /// that no more code from that task runs.
+ pub async fn exit(self) -> Never {
+ self.exit_sync();
+ pending().await
+ }
+}
+
+impl From<UniquePtr<CppExitHandle>> for ExitHandle {
+ fn from(inner: UniquePtr<ffi::aos::ExitHandle>) -> Self {
+ Self(inner)
+ }
+}
diff --git a/aos/events/simulated_event_loop.rs b/aos/events/simulated_event_loop.rs
index 06fcee8..e0be773 100644
--- a/aos/events/simulated_event_loop.rs
+++ b/aos/events/simulated_event_loop.rs
@@ -8,12 +8,11 @@
use autocxx::WithinBox;
use cxx::UniquePtr;
-use futures::{future::pending, never::Never};
pub use aos_configuration::{Channel, Configuration, ConfigurationExt, Node};
use aos_configuration_fbs::aos::Configuration as RustConfiguration;
-pub use aos_events_event_loop_runtime::EventLoop;
use aos_events_event_loop_runtime::EventLoopRuntime;
+pub use aos_events_event_loop_runtime::{CppExitHandle, EventLoop, ExitHandle};
use aos_flatbuffers::{transmute_table_to, Flatbuffer};
autocxx::include_cpp! (
@@ -22,9 +21,9 @@
safety!(unsafe)
-generate!("aos::ExitHandle")
generate!("aos::SimulatedEventLoopFactoryForRust")
+extern_cpp_type!("aos::ExitHandle", crate::CppExitHandle)
extern_cpp_type!("aos::Configuration", crate::Configuration)
extern_cpp_type!("aos::Node", crate::Node)
extern_cpp_type!("aos::EventLoop", crate::EventLoop)
@@ -92,7 +91,7 @@
}
pub fn make_exit_handle(&mut self) -> ExitHandle {
- ExitHandle(self.as_mut().MakeExitHandle())
+ self.as_mut().MakeExitHandle().into()
}
pub fn run(&mut self) {
@@ -104,27 +103,6 @@
// pub fn spawn_on_startup(&mut self, spawner: impl FnMut());
}
-// TODO(Brian): Move this and the `generate!` somewhere else once we wrap ShmEventLoop, which also
-// uses it.
-pub struct ExitHandle(UniquePtr<ffi::aos::ExitHandle>);
-
-impl ExitHandle {
- /// Exits the EventLoops represented by this handle. You probably want to immediately return
- /// from the context this is called in. Awaiting [`exit`] instead of using this function is an
- /// easy way to do that.
- pub fn exit_sync(mut self) {
- self.0.as_mut().unwrap().Exit();
- }
-
- /// Exits the EventLoops represented by this handle, and never returns. Immediately awaiting
- /// this from a [`EventLoopRuntime::spawn`]ed task is usually what you want, it will ensure
- /// that no more code from that task runs.
- pub async fn exit(self) -> Never {
- self.exit_sync();
- pending().await
- }
-}
-
pub struct SimulatedEventLoopRuntime(ManuallyDrop<EventLoopRuntime<'static>>);
impl SimulatedEventLoopRuntime {
diff --git a/aos/ipc_lib/BUILD b/aos/ipc_lib/BUILD
index 381a273..cc46dac 100644
--- a/aos/ipc_lib/BUILD
+++ b/aos/ipc_lib/BUILD
@@ -212,6 +212,7 @@
name = "lockless_queue_test",
timeout = "eternal",
srcs = ["lockless_queue_test.cc"],
+ shard_count = 10,
target_compatible_with = ["@platforms//os:linux"],
deps = [
":event",
diff --git a/frc971/vision/ceres/pose_graph_3d_error_term.h b/frc971/vision/ceres/pose_graph_3d_error_term.h
index 1f3e8de..c2ebde7 100644
--- a/frc971/vision/ceres/pose_graph_3d_error_term.h
+++ b/frc971/vision/ceres/pose_graph_3d_error_term.h
@@ -69,16 +69,17 @@
// where I is the information matrix which is the inverse of the covariance.
class PoseGraph3dErrorTerm {
public:
- PoseGraph3dErrorTerm(const Pose3d& t_ab_measured,
- const Eigen::Matrix<double, 6, 6>& sqrt_information)
- : t_ab_measured_(t_ab_measured), sqrt_information_(sqrt_information) {}
+ PoseGraph3dErrorTerm(const Pose3d &t_ab_measured,
+ const Eigen::Matrix<double, 6, 6> &sqrt_information,
+ double weight)
+ : t_ab_measured_(t_ab_measured),
+ sqrt_information_(sqrt_information),
+ weight_(weight) {}
template <typename T>
- bool operator()(const T* const p_a_ptr,
- const T* const q_a_ptr,
- const T* const p_b_ptr,
- const T* const q_b_ptr,
- T* residuals_ptr) const {
+ bool operator()(const T *const p_a_ptr, const T *const q_a_ptr,
+ const T *const p_b_ptr, const T *const q_b_ptr,
+ T *residuals_ptr) const {
Eigen::Map<const Eigen::Matrix<T, 3, 1>> p_a(p_a_ptr);
Eigen::Map<const Eigen::Quaternion<T>> q_a(q_a_ptr);
@@ -107,14 +108,17 @@
// Scale the residuals by the measurement uncertainty.
residuals.applyOnTheLeft(sqrt_information_.template cast<T>());
+ // Scale residual by the weight.
+ residuals *= T(weight_);
+
return true;
}
- static ceres::CostFunction* Create(
- const Pose3d& t_ab_measured,
- const Eigen::Matrix<double, 6, 6>& sqrt_information) {
+ static ceres::CostFunction *Create(
+ const Pose3d &t_ab_measured,
+ const Eigen::Matrix<double, 6, 6> &sqrt_information, double weight) {
return new ceres::AutoDiffCostFunction<PoseGraph3dErrorTerm, 6, 3, 4, 3, 4>(
- new PoseGraph3dErrorTerm(t_ab_measured, sqrt_information));
+ new PoseGraph3dErrorTerm(t_ab_measured, sqrt_information, weight));
}
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
@@ -124,6 +128,8 @@
const Pose3d t_ab_measured_;
// The square root of the measurement information matrix.
const Eigen::Matrix<double, 6, 6> sqrt_information_;
+ // Scalar for the cost of the constraint
+ const double weight_;
};
} // namespace examples
diff --git a/frc971/vision/target_mapper.cc b/frc971/vision/target_mapper.cc
index 0d6b9e9..21bd443 100644
--- a/frc971/vision/target_mapper.cc
+++ b/frc971/vision/target_mapper.cc
@@ -16,6 +16,12 @@
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.");
+// 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.");
namespace frc971::vision {
Eigen::Affine3d PoseUtils::Pose3dToAffine3d(
@@ -225,6 +231,7 @@
PoseUtils::TargetPoseFromFbs(*target_pose_fbs).pose;
}
target_poses_ = ideal_target_poses_;
+ CountConstraints();
}
TargetMapper::TargetMapper(
@@ -235,7 +242,28 @@
target_constraints_(target_constraints),
T_frozen_actual_(Eigen::Vector3d::Zero()),
R_frozen_actual_(Eigen::Quaterniond::Identity()),
- vis_robot_(cv::Size(1280, 1000)) {}
+ vis_robot_(cv::Size(1280, 1000)) {
+ CountConstraints();
+}
+
+namespace {
+std::pair<TargetMapper::TargetId, TargetMapper::TargetId> MakeIdPair(
+ const ceres::examples::Constraint3d &constraint) {
+ auto min_id = std::min(constraint.id_begin, constraint.id_end);
+ auto max_id = std::max(constraint.id_begin, constraint.id_end);
+ return std::make_pair(min_id, max_id);
+}
+} // namespace
+
+void TargetMapper::CountConstraints() {
+ for (const auto &constraint : target_constraints_) {
+ auto id_pair = MakeIdPair(constraint);
+ if (constraint_counts_.count(id_pair) == 0) {
+ constraint_counts_[id_pair] = 0;
+ }
+ constraint_counts_[id_pair]++;
+ }
+}
std::optional<TargetMapper::TargetPose> TargetMapper::GetTargetPoseById(
std::vector<TargetMapper::TargetPose> target_poses, TargetId target_id) {
@@ -290,10 +318,23 @@
const Eigen::Matrix<double, 6, 6> sqrt_information =
constraint.information.llt().matrixL();
+
+ auto id_pair = MakeIdPair(constraint);
+ CHECK_GT(constraint_counts_.count(id_pair), 0ul)
+ << "Should have counted constraints for " << id_pair.first << "->"
+ << 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
+ constexpr double kGlobalWeight = 1000.0;
+ double constraint_weight =
+ kGlobalWeight / static_cast<double>(constraint_count);
+
// Ceres will take ownership of the pointer.
ceres::CostFunction *cost_function =
- ceres::examples::PoseGraph3dErrorTerm::Create(constraint.t_be,
- sqrt_information);
+ ceres::examples::PoseGraph3dErrorTerm::Create(
+ constraint.t_be, sqrt_information, constraint_weight);
problem->AddResidualBlock(cost_function, loss_function,
pose_begin_iter->second.p.data(),
@@ -323,7 +364,8 @@
problem->SetParameterBlockConstant(pose_start_iter->second.q.coeffs().data());
}
-void TargetMapper::BuildMapFittingOptimizationProblem(ceres::Problem *problem) {
+std::unique_ptr<ceres::CostFunction>
+TargetMapper::BuildMapFittingOptimizationProblem(ceres::Problem *problem) {
// Setup robot visualization
vis_robot_.ClearImage();
constexpr int kImageWidth = 1280;
@@ -335,19 +377,20 @@
const size_t num_residuals = num_targets * 6;
// Set up the only cost function (also known as residual). This uses
// auto-differentiation to obtain the derivative (jacobian).
- ceres::CostFunction *cost_function =
- new ceres::AutoDiffCostFunction<TargetMapper, ceres::DYNAMIC, 3, 4>(
- this, num_residuals, ceres::DO_NOT_TAKE_OWNERSHIP);
+ std::unique_ptr<ceres::CostFunction> cost_function = std::make_unique<
+ ceres::AutoDiffCostFunction<TargetMapper, ceres::DYNAMIC, 3, 4>>(
+ this, num_residuals, ceres::DO_NOT_TAKE_OWNERSHIP);
ceres::LossFunction *loss_function = new ceres::HuberLoss(2.0);
ceres::LocalParameterization *quaternion_local_parameterization =
new ceres::EigenQuaternionParameterization;
- problem->AddResidualBlock(cost_function, loss_function,
+ problem->AddResidualBlock(cost_function.get(), loss_function,
T_frozen_actual_.vector().data(),
R_frozen_actual_.coeffs().data());
problem->SetParameterization(R_frozen_actual_.coeffs().data(),
quaternion_local_parameterization);
+ return cost_function;
}
// Taken from ceres/examples/slam/pose_graph_3d/pose_graph_3d.cc
@@ -357,7 +400,7 @@
ceres::Solver::Options options;
options.max_num_iterations = FLAGS_max_num_iterations;
options.linear_solver_type = ceres::SPARSE_NORMAL_CHOLESKY;
- options.minimizer_progress_to_stdout = true;
+ options.minimizer_progress_to_stdout = false;
ceres::Solver::Summary summary;
ceres::Solve(options, problem, &summary);
@@ -369,16 +412,28 @@
void TargetMapper::Solve(std::string_view field_name,
std::optional<std::string_view> output_dir) {
- ceres::Problem target_pose_problem;
+ ceres::Problem target_pose_problem_1;
BuildTargetPoseOptimizationProblem(target_constraints_, &target_poses_,
- &target_pose_problem);
- CHECK(SolveOptimizationProblem(&target_pose_problem))
- << "The target pose solve was not successful, exiting.";
+ &target_pose_problem_1);
+ CHECK(SolveOptimizationProblem(&target_pose_problem_1))
+ << "The target pose solve 1 was not successful, exiting.";
- ceres::Problem map_fitting_problem;
- BuildMapFittingOptimizationProblem(&map_fitting_problem);
+ RemoveOutlierConstraints();
+
+ // Solve again once we've thrown out bad constraints
+ ceres::Problem target_pose_problem_2;
+ BuildTargetPoseOptimizationProblem(target_constraints_, &target_poses_,
+ &target_pose_problem_2);
+ CHECK(SolveOptimizationProblem(&target_pose_problem_2))
+ << "The target pose solve 2 was not successful, exiting.";
+
+ ceres::Problem map_fitting_problem(
+ {.loss_function_ownership = ceres::DO_NOT_TAKE_OWNERSHIP});
+ std::unique_ptr<ceres::CostFunction> map_fitting_cost_function =
+ BuildMapFittingOptimizationProblem(&map_fitting_problem);
CHECK(SolveOptimizationProblem(&map_fitting_problem))
<< "The map fitting solve was not successful, exiting.";
+ map_fitting_cost_function.release();
Eigen::Affine3d H_frozen_actual = T_frozen_actual_ * R_frozen_actual_;
LOG(INFO) << "H_frozen_actual: "
@@ -412,6 +467,19 @@
LOG(INFO) << "Writing map to file: " << output_path;
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++) {
+ auto H_start_end =
+ 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";
+ }
+ }
}
std::string TargetMapper::MapToJson(std::string_view field_name) const {
@@ -499,8 +567,10 @@
VLOG(2);
auto R_ideal_actual = Eigen::AngleAxis<S>(H_ideal_actual.rotation());
- constexpr double kTranslationScalar = 100.0;
- constexpr double kRotationScalar = 1000.0;
+ // Weight translation errors higher than rotation.
+ // 1 m in position error = 0.01 radian (or ~0.573 degrees)
+ constexpr double kTranslationScalar = 1000.0;
+ constexpr double kRotationScalar = 100.0;
// Penalize based on how much our actual poses matches the ideal
// ones. We've already solved for the relative poses, now figure out
@@ -537,6 +607,130 @@
return true;
}
+TargetMapper::PoseError TargetMapper::ComputeError(
+ const ceres::examples::Constraint3d &constraint) const {
+ // Compute the difference between the map-based transform of the end target
+ // in the start target frame, to the one from this constraint
+ auto H_start_end_map =
+ PoseUtils::Pose3dToAffine3d(target_poses_.at(constraint.id_begin))
+ .inverse() *
+ PoseUtils::Pose3dToAffine3d(target_poses_.at(constraint.id_end));
+ auto H_start_end_constraint = PoseUtils::Pose3dToAffine3d(constraint.t_be);
+ ceres::examples::Pose3d delta_pose = PoseUtils::Affine3dToPose3d(
+ H_start_end_map.inverse() * H_start_end_constraint);
+ double distance = delta_pose.p.norm();
+ Eigen::AngleAxisd err_angle(delta_pose.q);
+ double angle = std::abs(err_angle.angle());
+ return {.angle = angle, .distance = distance};
+}
+
+TargetMapper::Stats TargetMapper::ComputeStats() const {
+ Stats stats{.avg_err = {.angle = 0.0, .distance = 0.0},
+ .std_dev = {.angle = 0.0, .distance = 0.0},
+ .max_err = {.angle = 0.0, .distance = 0.0}};
+
+ for (const auto &constraint : target_constraints_) {
+ PoseError err = ComputeError(constraint);
+
+ // Update our statistics
+ stats.avg_err.distance += err.distance;
+ if (err.distance > stats.max_err.distance) {
+ stats.max_err.distance = err.distance;
+ }
+
+ stats.avg_err.angle += err.angle;
+ if (err.angle > stats.max_err.angle) {
+ stats.max_err.angle = err.angle;
+ }
+ }
+
+ stats.avg_err.distance /= static_cast<double>(target_constraints_.size());
+ stats.avg_err.angle /= static_cast<double>(target_constraints_.size());
+
+ for (const auto &constraint : target_constraints_) {
+ PoseError err = ComputeError(constraint);
+
+ // Update our statistics
+ stats.std_dev.distance +=
+ std::pow(err.distance - stats.avg_err.distance, 2);
+
+ stats.std_dev.angle += std::pow(err.angle - stats.avg_err.angle, 2);
+ }
+
+ stats.std_dev.distance = std::sqrt(
+ stats.std_dev.distance / static_cast<double>(target_constraints_.size()));
+ stats.std_dev.angle = std::sqrt(
+ stats.std_dev.angle / static_cast<double>(target_constraints_.size()));
+
+ return stats;
+}
+
+void TargetMapper::RemoveOutlierConstraints() {
+ stats_with_outliers_ = ComputeStats();
+ size_t original_size = target_constraints_.size();
+ target_constraints_.erase(
+ std::remove_if(
+ target_constraints_.begin(), target_constraints_.end(),
+ [&](const auto &constraint) {
+ PoseError err = ComputeError(constraint);
+ // Remove constraints with errors significantly above
+ // the average
+ if (err.distance > stats_with_outliers_.avg_err.distance +
+ 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 *
+ stats_with_outliers_.std_dev.angle) {
+ return true;
+ }
+ return false;
+ }),
+ target_constraints_.end());
+
+ LOG(INFO) << "Removed " << (original_size - target_constraints_.size())
+ << " outlier constraints out of " << original_size << " total";
+}
+
+void TargetMapper::DumpStats(std::string_view path) const {
+ LOG(INFO) << "Dumping mapping stats to " << path;
+ Stats stats = ComputeStats();
+ std::ofstream fout(path.data());
+ fout << "Stats after outlier rejection: " << std::endl;
+ fout << "Average error - angle: " << stats.avg_err.angle
+ << ", distance: " << stats.avg_err.distance << std::endl
+ << std::endl;
+ fout << "Standard deviation - angle: " << stats.std_dev.angle
+ << ", distance: " << stats.std_dev.distance << std::endl
+ << std::endl;
+ fout << "Max error - angle: " << stats.max_err.angle
+ << ", distance: " << stats.max_err.distance << std::endl;
+
+ fout << std::endl << "Stats before outlier rejection:" << std::endl;
+ fout << "Average error - angle: " << stats_with_outliers_.avg_err.angle
+ << ", distance: " << stats_with_outliers_.avg_err.distance << std::endl
+ << std::endl;
+ fout << "Standard deviation - angle: " << stats_with_outliers_.std_dev.angle
+ << ", distance: " << stats_with_outliers_.std_dev.distance << std::endl
+ << std::endl;
+ fout << "Max error - angle: " << stats_with_outliers_.max_err.angle
+ << ", distance: " << stats_with_outliers_.max_err.distance << std::endl;
+
+ fout.flush();
+ fout.close();
+}
+
+void TargetMapper::DumpConstraints(std::string_view path) const {
+ LOG(INFO) << "Dumping target constraints to " << path;
+ std::ofstream fout(path.data());
+ for (const auto &constraint : target_constraints_) {
+ fout << constraint << std::endl;
+ }
+ fout.flush();
+ fout.close();
+}
+
} // namespace frc971::vision
std::ostream &operator<<(std::ostream &os, ceres::examples::Pose3d pose) {
diff --git a/frc971/vision/target_mapper.h b/frc971/vision/target_mapper.h
index ca36866..d10d6d8 100644
--- a/frc971/vision/target_mapper.h
+++ b/frc971/vision/target_mapper.h
@@ -58,7 +58,36 @@
bool operator()(const S *const translation, const S *const rotation,
S *residual) const;
+ void DumpConstraints(std::string_view path) const;
+ void DumpStats(std::string_view path) const;
+
private:
+ // Error in an estimated pose
+ struct PoseError {
+ double angle;
+ double distance;
+ };
+
+ // Stores info on how much all the constraints differ from our solved target
+ // map
+ struct Stats {
+ // Average error for translation and rotation
+ PoseError avg_err;
+ // Standard deviation for translation and rotation error
+ PoseError std_dev;
+ // Maximum error for translation and rotation
+ PoseError max_err;
+ };
+
+ // Compute the error of a single constraint
+ PoseError ComputeError(const ceres::examples::Constraint3d &constraint) const;
+ // Compute cumulative stats for all constraints
+ Stats ComputeStats() const;
+ // Removes constraints with very large errors
+ void RemoveOutlierConstraints();
+
+ void CountConstraints();
+
// Constructs the nonlinear least squares optimization problem from the
// pose graph constraints.
void BuildTargetPoseOptimizationProblem(
@@ -67,7 +96,8 @@
// Constructs the nonlinear least squares optimization problem for the solved
// -> actual pose solver.
- void BuildMapFittingOptimizationProblem(ceres::Problem *problem);
+ std::unique_ptr<ceres::CostFunction> BuildMapFittingOptimizationProblem(
+ ceres::Problem *problem);
// Returns true if the solve was successful.
bool SolveOptimizationProblem(ceres::Problem *problem);
@@ -76,12 +106,18 @@
ceres::examples::MapOfPoses target_poses_;
ceres::examples::VectorOfConstraints target_constraints_;
+ // Counts of each pair of target ids we observe, so we can scale cost based on
+ // the inverse of this and remove bias towards certain pairs
+ std::map<std::pair<TargetId, TargetId>, size_t> constraint_counts_;
+
// Transformation moving the target map we solved for to where it actually
// should be in the world
Eigen::Translation3d T_frozen_actual_;
Eigen::Quaterniond R_frozen_actual_;
mutable VisualizeRobot vis_robot_;
+
+ Stats stats_with_outliers_;
};
// Utility functions for dealing with ceres::examples::Pose3d structs
diff --git a/frc971/vision/target_mapper_test.cc b/frc971/vision/target_mapper_test.cc
index 1371f89..9c86965 100644
--- a/frc971/vision/target_mapper_test.cc
+++ b/frc971/vision/target_mapper_test.cc
@@ -480,8 +480,14 @@
};
// Drive in a circle around the 2023 field, and add a bit of randomness to 3d
-// pose detections
-TEST_F(ChargedUpTargetMapperTest, FieldCircleMotion) {
+// pose detections.
+// TODO(milind): use valgrind to debug why this test passes, but then segfaults
+// 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;
+
// Read target map
auto target_map_fbs = aos::JsonFileToFlatbuffer<TargetMap>(
aos::testing::ArtifactPath("frc971/vision/target_map.json"));
diff --git a/tools/lint/BUILD b/tools/lint/BUILD
index 23ba4d6..a881124 100644
--- a/tools/lint/BUILD
+++ b/tools/lint/BUILD
@@ -7,6 +7,17 @@
)
sh_binary(
+ name = "clang_format",
+ srcs = ["clang_format.sh"],
+ data = [
+ "@llvm_k8//:bin",
+ ],
+ deps = [
+ "@bazel_tools//tools/bash/runfiles",
+ ],
+)
+
+sh_binary(
name = "gofmt",
srcs = ["gofmt.sh"],
data = [
@@ -70,6 +81,7 @@
],
data = [
":buildifier",
+ ":clang_format",
":gofmt",
":prettier",
":rustfmt",
diff --git a/tools/lint/clang_format.sh b/tools/lint/clang_format.sh
new file mode 100755
index 0000000..3084520
--- /dev/null
+++ b/tools/lint/clang_format.sh
@@ -0,0 +1,31 @@
+#!/bin/bash
+
+# --- begin runfiles.bash initialization v2 ---
+# Copy-pasted from the Bazel Bash runfiles library v2.
+set -uo pipefail; f=bazel_tools/tools/bash/runfiles/runfiles.bash
+source "${RUNFILES_DIR:-/dev/null}/$f" 2>/dev/null || \
+ source "$(grep -sm1 "^$f " "${RUNFILES_MANIFEST_FILE:-/dev/null}" | cut -f2- -d' ')" 2>/dev/null || \
+ source "$0.runfiles/$f" 2>/dev/null || \
+ source "$(grep -sm1 "^$f " "$0.runfiles_manifest" | cut -f2- -d' ')" 2>/dev/null || \
+ source "$(grep -sm1 "^$f " "$0.exe.runfiles_manifest" | cut -f2- -d' ')" 2>/dev/null || \
+ { echo>&2 "ERROR: cannot find $f"; exit 1; }; f=; set -e
+# --- end runfiles.bash initialization v2 ---
+
+readonly CLANG_FORMAT="$(rlocation llvm_k8/bin/clang-format)"
+
+# Run everything from the root of the tree.
+cd "${BUILD_WORKSPACE_DIRECTORY}"
+
+# Find all the C/C++ files in the repo.
+# Exclude third-party code. Both in //third_party and the third-party code
+# checked in to the main repo directly.
+cc_files=($(git ls-tree --name-only --full-tree -r @ \
+ | grep -v -e '^third_party/' \
+ -e '^motors/core/kinetis.h$' \
+ -e '^y2023/vision/rkisp1-config.h$' \
+ | (grep -e '\.c$' -e '\.cc$' -e '\.h' || :)))
+
+# If we have any C/C++ files, format them.
+if ((${#cc_files[@]} > 0)); then
+ exec "${CLANG_FORMAT}" -i "${cc_files[@]}"
+fi
diff --git a/tools/lint/run-ci.sh b/tools/lint/run-ci.sh
index 6e026ee..1f5fda4 100755
--- a/tools/lint/run-ci.sh
+++ b/tools/lint/run-ci.sh
@@ -23,6 +23,10 @@
export GOCACHE=/tmp/lint_go_cache
fi
+clang_format() {
+ ./tools/lint/clang_format
+}
+
gofmt() {
./tools/lint/gofmt
}
@@ -110,6 +114,7 @@
# All the linters that we are going to run.
readonly -a LINTERS=(
+ #clang_format
gofmt
gomod
update_go_repos
diff --git a/y2023/vision/target_mapping.cc b/y2023/vision/target_mapping.cc
index 45c00ce..804583c 100644
--- a/y2023/vision/target_mapping.cc
+++ b/y2023/vision/target_mapping.cc
@@ -46,7 +46,9 @@
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/constraints.txt",
+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");
DECLARE_int32(frozen_target_id);
DECLARE_int32(min_target_id);
@@ -381,53 +383,15 @@
}),
target_constraints.end());
- if (!FLAGS_dump_constraints_to.empty()) {
- std::ofstream fout(FLAGS_dump_constraints_to);
- for (const auto &constraint : target_constraints) {
- fout << constraint << std::endl;
- }
- fout.flush();
- fout.close();
- }
-
- // Give seed constraints with a higher confidence to ground the solver.
- // This "distance from camera" controls the noise of the seed measurement
- constexpr double kSeedDistanceFromCamera = 1.0;
-
- constexpr double kSeedDistortionFactor = 0.0;
- const DataAdapter::TimestampedDetection frozen_detection_seed = {
- .time = aos::distributed_clock::min_time,
- .H_robot_target = PoseUtils::Pose3dToAffine3d(
- kFixedTargetMapper.GetTargetPoseById(FLAGS_frozen_target_id)
- .value()
- .pose),
- .distance_from_camera = kSeedDistanceFromCamera,
- .distortion_factor = kSeedDistortionFactor,
- .id = FLAGS_frozen_target_id};
-
- constexpr TargetMapper::TargetId kAbsMinTargetId = 1;
- constexpr TargetMapper::TargetId kAbsMaxTargetId = 8;
- for (TargetMapper::TargetId id = kAbsMinTargetId; id <= kAbsMaxTargetId;
- id++) {
- if (id == FLAGS_frozen_target_id) {
- continue;
- }
-
- const DataAdapter::TimestampedDetection detection_seed = {
- .time = aos::distributed_clock::min_time,
- .H_robot_target = PoseUtils::Pose3dToAffine3d(
- kFixedTargetMapper.GetTargetPoseById(id).value().pose),
- .distance_from_camera = kSeedDistanceFromCamera,
- .distortion_factor = kSeedDistortionFactor,
- .id = id};
- target_constraints.emplace_back(DataAdapter::ComputeTargetConstraint(
- frozen_detection_seed, detection_seed,
- DataAdapter::ComputeConfidence(frozen_detection_seed,
- detection_seed)));
- }
-
TargetMapper mapper(FLAGS_json_path, target_constraints);
mapper.Solve(FLAGS_field_name, FLAGS_output_dir);
+
+ if (!FLAGS_dump_constraints_to.empty()) {
+ mapper.DumpConstraints(FLAGS_dump_constraints_to);
+ }
+ if (!FLAGS_dump_stats_to.empty()) {
+ mapper.DumpStats(FLAGS_dump_stats_to);
+ }
}
}