From 414afcda201e133dba405916a198e01f94459a99 Mon Sep 17 00:00:00 2001 From: Ethan Fahnestock Date: Mon, 2 Dec 2024 12:14:37 -0500 Subject: [PATCH 1/4] changing name --- common/check.hh | 2 +- common/check_test.cc | 2 +- common/linux/get_memory_usage.cc | 2 +- common/math/cubic_hermite_spline.hh | 4 ++-- common/math/multivariate_normal_cdf.cc | 6 +++--- common/sqlite3/sqlite3.cc | 6 +++--- experimental/beacon_sim/beacon_potential.cc | 2 +- experimental/beacon_sim/beacon_potential.hh | 4 ++-- .../beacon_sim/beacon_potential_to_proto.cc | 2 +- experimental/beacon_sim/beacon_sim.cc | 8 ++++---- .../beacon_sim/belief_road_map_planner.cc | 12 ++++++------ .../beacon_sim/compute_oracle_results.cc | 16 ++++++++-------- .../beacon_sim/correlated_beacon_potential.cc | 4 ++-- experimental/beacon_sim/ekf_slam.cc | 2 +- .../information_lower_bound_search.cc | 2 +- experimental/beacon_sim/make_belief_updater.cc | 16 ++++++++-------- .../beacon_sim/precision_matrix_potential.cc | 2 +- experimental/beacon_sim/reprocess_result.cc | 12 ++++++------ experimental/beacon_sim/run_experiment.cc | 18 +++++++++--------- experimental/beacon_sim/work_server.cc | 10 +++++----- experimental/beacon_sim/world_map.cc | 2 +- .../learn_descriptors/symphony_lake_parser.cc | 2 +- .../overhead_matching/spectacular_log.cc | 8 ++++---- planning/road_map.cc | 8 ++++---- 24 files changed, 76 insertions(+), 76 deletions(-) diff --git a/common/check.hh b/common/check.hh index bab7ad8d..70cf23de 100644 --- a/common/check.hh +++ b/common/check.hh @@ -1,7 +1,7 @@ #include "assert/assert.hpp" -#define CHECK(expr, ...) ASSERT_INVOKE(expr, false, true, "CHECK", verification, , __VA_ARGS__) +#define ROBOT_CHECK(expr, ...) ASSERT_INVOKE(expr, false, true, "ROBOT_CHECK", verification, , __VA_ARGS__) namespace robot { using check_failure = libassert::verification_failure; diff --git a/common/check_test.cc b/common/check_test.cc index f8e5c570..64a2c91b 100644 --- a/common/check_test.cc +++ b/common/check_test.cc @@ -3,4 +3,4 @@ #include "gtest/gtest.h" -TEST(AssertTest, assert_test) { EXPECT_THROW(CHECK(false), robot::check_failure); } +TEST(AssertTest, assert_test) { EXPECT_THROW(ROBOT_CHECK(false), robot::check_failure); } diff --git a/common/linux/get_memory_usage.cc b/common/linux/get_memory_usage.cc index b64be09c..9fa1f9d1 100644 --- a/common/linux/get_memory_usage.cc +++ b/common/linux/get_memory_usage.cc @@ -15,7 +15,7 @@ int get_memory_usage_kb() { std::smatch match; std::string contents = stream.str(); const bool match_found = std::regex_search(contents, match, memory_matcher); - CHECK(match_found); + ROBOT_CHECK(match_found); return std::stoi(match[1]); } } // namespace robot::linux diff --git a/common/math/cubic_hermite_spline.hh b/common/math/cubic_hermite_spline.hh index 1365f599..fa97369e 100644 --- a/common/math/cubic_hermite_spline.hh +++ b/common/math/cubic_hermite_spline.hh @@ -17,11 +17,11 @@ class CubicHermiteSpline { CubicHermiteSpline(std::vector ts, std::vector xs) : ts_(std::move(ts)), xs_(std::move(xs)) { - CHECK(std::is_sorted(ts_.begin(), ts_.end()), "Input times must be sorted!", ts_); + ROBOT_CHECK(std::is_sorted(ts_.begin(), ts_.end()), "Input times must be sorted!", ts_); } X operator()(const T &query_time) const { - CHECK(query_time >= ts_.front() && query_time <= ts_.back(), "query_time is out of bounds", + ROBOT_CHECK(query_time >= ts_.front() && query_time <= ts_.back(), "query_time is out of bounds", ts_.front(), ts_.back(), query_time); const auto iter = std::lower_bound(ts_.begin(), ts_.end(), query_time); diff --git a/common/math/multivariate_normal_cdf.cc b/common/math/multivariate_normal_cdf.cc index fb38d899..1dde798a 100644 --- a/common/math/multivariate_normal_cdf.cc +++ b/common/math/multivariate_normal_cdf.cc @@ -11,9 +11,9 @@ std::optional multivariate_normal_cdf(const Eigen::VectorXd &mu, const Eigen::MatrixXd &sigma, const Eigen::VectorXd &upper_bound, const bool return_log_p) { - CHECK(sigma.rows() == sigma.cols()); - CHECK(sigma.rows() == mu.rows()); - CHECK(sigma.rows() == upper_bound.rows()); + ROBOT_CHECK(sigma.rows() == sigma.cols()); + ROBOT_CHECK(sigma.rows() == mu.rows()); + ROBOT_CHECK(sigma.rows() == upper_bound.rows()); const int dim = mu.rows(); constexpr bool NONSTANDARD = false; diff --git a/common/sqlite3/sqlite3.cc b/common/sqlite3/sqlite3.cc index 8fa2cb71..3a080480 100644 --- a/common/sqlite3/sqlite3.cc +++ b/common/sqlite3/sqlite3.cc @@ -20,7 +20,7 @@ struct Database::Impl { template void check_result(const int error_code, Ts... args) { - CHECK(error_code == SQLITE_OK, sqlite3_errmsg(db_), args...); + ROBOT_CHECK(error_code == SQLITE_OK, sqlite3_errmsg(db_), args...); }; Database::Statement prepare(const std::string &statement) { @@ -37,7 +37,7 @@ struct Database::Impl { void bind(const Statement &stmt, const std::unordered_map &args) { sqlite3_stmt *stmt_ptr = stmt.impl_->stmt.get(); - CHECK(args.size() == sqlite3_bind_parameter_count(stmt_ptr), + ROBOT_CHECK(args.size() == sqlite3_bind_parameter_count(stmt_ptr), "insufficient number of arguments", sqlite3_sql(stmt_ptr), args); for (const auto &[key, value] : args) { const int param_idx = sqlite3_bind_parameter_index(stmt_ptr, key.c_str()); @@ -69,7 +69,7 @@ struct Database::Impl { if (result == SQLITE_DONE) { return std::nullopt; } - CHECK(result == SQLITE_ROW, sqlite3_errmsg(db_)); + ROBOT_CHECK(result == SQLITE_ROW, sqlite3_errmsg(db_)); const int num_columns = sqlite3_column_count(stmt_ptr); if (stmt.impl_->column_names == nullptr) { std::vector column_names; diff --git a/experimental/beacon_sim/beacon_potential.cc b/experimental/beacon_sim/beacon_potential.cc index 73170550..799cf78f 100644 --- a/experimental/beacon_sim/beacon_potential.cc +++ b/experimental/beacon_sim/beacon_potential.cc @@ -122,7 +122,7 @@ BeaconPotential operator*(const BeaconPotential &a, const BeaconPotential &b) { std::set_intersection(a_sorted_members.begin(), a_sorted_members.end(), b_sorted_members.begin(), b_sorted_members.end(), std::back_inserter(common_elements)); - CHECK(common_elements.empty(), "Found overlap in members of potentials", a_sorted_members, + ROBOT_CHECK(common_elements.empty(), "Found overlap in members of potentials", a_sorted_members, b_sorted_members, common_elements); return CombinedPotential({a, b}); diff --git a/experimental/beacon_sim/beacon_potential.hh b/experimental/beacon_sim/beacon_potential.hh index 02a24708..fe306d50 100644 --- a/experimental/beacon_sim/beacon_potential.hh +++ b/experimental/beacon_sim/beacon_potential.hh @@ -63,11 +63,11 @@ class BeaconPotential { double log_prob(const std::unordered_map &assignments, const bool allow_partial_assignment = false) const { - CHECK(impl_ != nullptr); + ROBOT_CHECK(impl_ != nullptr); return impl_->log_prob_(assignments, allow_partial_assignment); }; double log_prob(const std::vector &present_beacons) const { - CHECK(impl_ != nullptr); + ROBOT_CHECK(impl_ != nullptr); return impl_->log_prob_(present_beacons); } diff --git a/experimental/beacon_sim/beacon_potential_to_proto.cc b/experimental/beacon_sim/beacon_potential_to_proto.cc index ec75198f..9095b660 100644 --- a/experimental/beacon_sim/beacon_potential_to_proto.cc +++ b/experimental/beacon_sim/beacon_potential_to_proto.cc @@ -28,7 +28,7 @@ beacon_sim::BeaconPotential unpack_from(const BeaconPotential &in) { case BeaconPotential::kAnticorrelatedPotential: return unpack_from(in.anticorrelated_potential()); } - CHECK(false, "Unhandled potential type"); + ROBOT_CHECK(false, "Unhandled potential type"); return beacon_sim::BeaconPotential(); } diff --git a/experimental/beacon_sim/beacon_sim.cc b/experimental/beacon_sim/beacon_sim.cc index 82f59c97..a0abaaa3 100644 --- a/experimental/beacon_sim/beacon_sim.cc +++ b/experimental/beacon_sim/beacon_sim.cc @@ -363,7 +363,7 @@ void run_simulation(const SimConfig &sim_config) { const auto maybe_world_map_config_proto = robot::proto::load_from_file( sim_config.world_map_config.value()); - CHECK(maybe_world_map_config_proto.has_value()); + ROBOT_CHECK(maybe_world_map_config_proto.has_value()); return WorldMap(unpack_from(maybe_world_map_config_proto.value())); } else { return WorldMap(world_map_config(sim_config.correlated_beacons_configuration)); @@ -375,7 +375,7 @@ void run_simulation(const SimConfig &sim_config) { const auto maybe_road_map_config_proto = robot::proto::load_from_file( sim_config.road_map_config.value()); - CHECK(maybe_road_map_config_proto); + ROBOT_CHECK(maybe_road_map_config_proto); return unpack_from(maybe_road_map_config_proto.value()); } else { return create_road_map(map); @@ -605,14 +605,14 @@ int main(int argc, char **argv) { const Eigen::Vector2d goal_in_map = [&]() { const auto goal_in_map_vec = args["goal_in_map"].as>(); - CHECK(goal_in_map_vec.size() == 2); + ROBOT_CHECK(goal_in_map_vec.size() == 2); return Eigen::Vector2d(goal_in_map_vec.data()); }(); const robot::liegroups::SE2 map_from_initial_robot = [&]() { const auto map_from_initial_robot_vec = args["map_from_initial_robot"].as>(); - CHECK(map_from_initial_robot_vec.size() == 3); + ROBOT_CHECK(map_from_initial_robot_vec.size() == 3); const Eigen::Vector2d robot_in_map(map_from_initial_robot_vec.data()); const double theta_rad = map_from_initial_robot_vec.back(); return robot::liegroups::SE2(theta_rad, robot_in_map); diff --git a/experimental/beacon_sim/belief_road_map_planner.cc b/experimental/beacon_sim/belief_road_map_planner.cc index 4e3a3dad..eeb0d8a6 100644 --- a/experimental/beacon_sim/belief_road_map_planner.cc +++ b/experimental/beacon_sim/belief_road_map_planner.cc @@ -187,7 +187,7 @@ template <> std::function make_uncertainty_size( const UncertaintySizeOptions &options) { if (std::holds_alternative(options)) { - CHECK(false, "ValueAtRiskDeterminant is not supported for unimodal beliefs", options); + ROBOT_CHECK(false, "ValueAtRiskDeterminant is not supported for unimodal beliefs", options); } else if (std::holds_alternative(options)) { return [opt = std::get(options)](const RobotBelief &belief) -> double { if (opt.position_only) { @@ -267,7 +267,7 @@ std::function make_uncertainty_size( for (const auto &eval_pt : eval_pts) { const auto result = math::multivariate_normal_cdf(Eigen::Vector3d::Zero(), cov, eval_pt); - CHECK(result.has_value(), "Could not compute cdf at eval pt", eval_pt); + ROBOT_CHECK(result.has_value(), "Could not compute cdf at eval pt", eval_pt); probs.push_back(result.value()); } return probs.at(0) - probs.at(1) + probs.at(2) - probs.at(3) - probs.at(4) + @@ -279,7 +279,7 @@ std::function make_uncertainty_size( }; } - CHECK(false, "Unknown Uncertainty Size Options", options); + ROBOT_CHECK(false, "Unknown Uncertainty Size Options", options); return [](const auto &) { return 0.0; }; } @@ -305,7 +305,7 @@ std::function make_uncertainty_size( return elem.cov_in_robot.determinant(); } } - CHECK(false, "Landmark Belief has insufficient probability mass to get to threshold", + ROBOT_CHECK(false, "Landmark Belief has insufficient probability mass to get to threshold", accumulated_prob); return elements.back().cov_in_robot.determinant(); }; @@ -406,7 +406,7 @@ std::function make_uncertainty_size( for (const auto &eval_pt : eval_pts) { const auto result = math::multivariate_normal_cdf(Eigen::Vector3d::Zero(), cov, eval_pt); - CHECK(result.has_value(), "Could not compute cdf at eval pt", eval_pt); + ROBOT_CHECK(result.has_value(), "Could not compute cdf at eval pt", eval_pt); probs.push_back(result.value()); } return probs.at(0) - probs.at(1) + probs.at(2) - probs.at(3) - probs.at(4) + @@ -422,7 +422,7 @@ std::function make_uncertainty_size( }; } - CHECK(false, "Unknown Uncertainty Size Options", options); + ROBOT_CHECK(false, "Unknown Uncertainty Size Options", options); return [](const auto &) { return 0.0; }; } diff --git a/experimental/beacon_sim/compute_oracle_results.cc b/experimental/beacon_sim/compute_oracle_results.cc index 05a63913..c4e78e76 100644 --- a/experimental/beacon_sim/compute_oracle_results.cc +++ b/experimental/beacon_sim/compute_oracle_results.cc @@ -87,7 +87,7 @@ std::optional get_job(const std::string &worker_name, proto::GetJobRequest job_request; job_request.mutable_worker()->set_name(worker_name); proto::GetJobResponse job_response; - CHECK(client.get_job(&context, job_request, &job_response).ok()); + ROBOT_CHECK(client.get_job(&context, job_request, &job_response).ok()); if (!job_response.has_job_id()) { return std::nullopt; } @@ -123,19 +123,19 @@ std::tuple load_environment( // Load Map Config const auto maybe_map_config = robot::proto::load_from_file(config_path / config.map_config_path()); - CHECK(maybe_map_config.has_value(), config_path / config.map_config_path()); + ROBOT_CHECK(maybe_map_config.has_value(), config_path / config.map_config_path()); const WorldMap world_map(unpack_from(maybe_map_config.value())); // Load EKF State const auto maybe_mapped_landmarks = robot::proto::load_from_file(config_path / config.ekf_state_path()); - CHECK(maybe_mapped_landmarks.has_value()); + ROBOT_CHECK(maybe_mapped_landmarks.has_value()); const auto mapped_landmarks = unpack_from(maybe_mapped_landmarks.value()); // Create a road map const auto maybe_road_map = robot::proto::load_from_file( config_path / config.road_map_path()); - CHECK(maybe_road_map.has_value()); + ROBOT_CHECK(maybe_road_map.has_value()); const planning::RoadMap road_map = unpack_from(maybe_road_map.value()); return {world_map.beacon_potential(), road_map, load_ekf_slam(mapped_landmarks)}; @@ -196,7 +196,7 @@ std::vector compute_plans( const std::function &progress_function) { const auto maybe_results_file = robot::proto::load_from_file(results_file_path); - CHECK(maybe_results_file.has_value()); + ROBOT_CHECK(maybe_results_file.has_value()); const auto &results_file = maybe_results_file.value(); const auto &experiment_config = results_file.experiment_config(); @@ -269,7 +269,7 @@ std::vector compute_plans( }}; const auto maybe_plan = compute_belief_road_map_plan(road_map, ekf, conditioned_potential, options); - CHECK(maybe_plan.has_value()); + ROBOT_CHECK(maybe_plan.has_value()); proto::OraclePlan plan_out; plan_out.set_trial_id(trial_idx); plan_out.set_eval_trial_id(eval_trial_idx); @@ -322,7 +322,7 @@ void compute_oracle_results(const std::string server_address, const int server_p update.set_progress(static_cast(plans_completed) / total_plans); pack_into(job_start_time, update.mutable_start_time()); pack_into(time::current_robot_time(), update.mutable_current_time()); - CHECK(client.update_job_status(&client_context, update_request, &update_response).ok()); + ROBOT_CHECK(client.update_job_status(&client_context, update_request, &update_response).ok()); }; // Compute the results @@ -337,7 +337,7 @@ void compute_oracle_results(const std::string server_address, const int server_p result_request.mutable_job_result()->mutable_plan()->Add(plans.begin(), plans.end()); result_request.set_job_id(maybe_job_response->job_id()); proto::JobResultResponse result_response; - CHECK(client.submit_job_result(&client_context, result_request, &result_response).ok()); + ROBOT_CHECK(client.submit_job_result(&client_context, result_request, &result_response).ok()); } } } diff --git a/experimental/beacon_sim/correlated_beacon_potential.cc b/experimental/beacon_sim/correlated_beacon_potential.cc index 192f6ae6..743faa19 100644 --- a/experimental/beacon_sim/correlated_beacon_potential.cc +++ b/experimental/beacon_sim/correlated_beacon_potential.cc @@ -72,7 +72,7 @@ double compute_log_prob(const CorrelatedBeaconPotential &pot, std::set_intersection(sorted_members.begin(), sorted_members.end(), keys.begin(), keys.end(), std::back_inserter(keys_to_keep)); - CHECK(allow_partial_assignment || missing_keys.empty(), + ROBOT_CHECK(allow_partial_assignment || missing_keys.empty(), "partial assignment specified when not enabled", assignment, missing_keys, pot.members); const int num_beacons_present = @@ -209,7 +209,7 @@ CorrelatedBeaconPotential condition_on(const CorrelatedBeaconPotential &pot, new_assignment = pot.conditioning->conditioned_members; for (const auto &[id, value] : assignment) { const auto &existing_conditioned = pot.conditioning->conditioned_members; - CHECK(!existing_conditioned.contains(id) || (existing_conditioned.at(id) == value), + ROBOT_CHECK(!existing_conditioned.contains(id) || (existing_conditioned.at(id) == value), "Inconsistent conditioning", existing_conditioned, assignment); new_assignment[id] = value; diff --git a/experimental/beacon_sim/ekf_slam.cc b/experimental/beacon_sim/ekf_slam.cc index 9ecde90f..9ad3434d 100644 --- a/experimental/beacon_sim/ekf_slam.cc +++ b/experimental/beacon_sim/ekf_slam.cc @@ -27,7 +27,7 @@ std::optional find_beacon_matrix_idx_or_add(const int id, InOutmean.rows() - ROBOT_STATE_DIM) / BEACON_DIM; - CHECK(static_cast(est->beacon_ids.size()) < max_num_landmarks, + ROBOT_CHECK(static_cast(est->beacon_ids.size()) < max_num_landmarks, "Not enough capacity to add beacon to EKF, increase max_num_beacons in ekf_config"); est->beacon_ids.push_back(id); diff --git a/experimental/beacon_sim/information_lower_bound_search.cc b/experimental/beacon_sim/information_lower_bound_search.cc index 68619c3a..c814e2b3 100644 --- a/experimental/beacon_sim/information_lower_bound_search.cc +++ b/experimental/beacon_sim/information_lower_bound_search.cc @@ -53,7 +53,7 @@ MergeResult should_merge(const std::vector &existing, InformationLowerBoundResult information_lower_bound_search( const planning::RoadMap &road_map, const double start_information, const double end_information_lower_bound, const LowerBoundReversePropagator &rev_propagator) { - CHECK(road_map.has_start_goal()); + ROBOT_CHECK(road_map.has_start_goal()); std::unordered_map> paths_from_node_id; std::priority_queue, diff --git a/experimental/beacon_sim/make_belief_updater.cc b/experimental/beacon_sim/make_belief_updater.cc index b748d6a3..4ec4885f 100644 --- a/experimental/beacon_sim/make_belief_updater.cc +++ b/experimental/beacon_sim/make_belief_updater.cc @@ -76,13 +76,13 @@ std::string configuration_to_key(const std::vector> &beaco std::string merge_configurations(const std::string &a, const std::string &b) { std::string out = a; - CHECK(a.size() == b.size()); + ROBOT_CHECK(a.size() == b.size()); for (int i = 0; i < static_cast(a.size()); i++) { const bool a_is_set = a.at(i) != '?'; const bool b_is_set = b.at(i) != '?'; - CHECK(a_is_set ^ b_is_set || a.at(i) == b.at(i), "a and b are not mergable", i, a, b); + ROBOT_CHECK(a_is_set ^ b_is_set || a.at(i) == b.at(i), "a and b are not mergable", i, a, b); if (b_is_set) { out.at(i) = b.at(i); } @@ -102,8 +102,8 @@ using UnappliedLandmarkBeliefMap = std::unordered_map downsize_and_normalize_belief( const UnappliedLandmarkBeliefMap &belief, const std::optional max_num_components, InOut> gen) { - CHECK(belief.size() > 0); - CHECK(!max_num_components.has_value() || max_num_components.value() > 0); + ROBOT_CHECK(belief.size() > 0); + ROBOT_CHECK(!max_num_components.has_value() || max_num_components.value() > 0); UnappliedLandmarkBeliefMap out; double log_normalizer = 0; @@ -223,7 +223,7 @@ std::vector sample_log_marginals(const std::vector all std::unordered_map assignment_from_config(const std::string &config, const std::vector &members) { - CHECK(config.size() == members.size()); + ROBOT_CHECK(config.size() == members.size()); std::unordered_map out; for (int i = 0; i < static_cast(config.size()); i++) { if (config.at(i) == '?') { @@ -233,7 +233,7 @@ std::unordered_map assignment_from_config(const std::string &config, } else if (config.at(i) == '0') { out[members.at(i)] = false; } else { - CHECK(false, "unable to parse config string", config, i, config.at(i)); + ROBOT_CHECK(false, "unable to parse config string", config, i, config.at(i)); } } return out; @@ -279,7 +279,7 @@ std::tuple sample_beliefs_without_replacemen }; const double belief_sum = math::logsumexp(downselected_unapplied_belief_from_config, log_prob_accessor); - CHECK(std::abs(belief_sum) < 1e-6); + ROBOT_CHECK(std::abs(belief_sum) < 1e-6); } return {downselected_unapplied_belief_from_config, log_probability_mass_tracked}; } @@ -299,7 +299,7 @@ std::tuple sample_beliefs_with_replacement( for (int i = 0; i < max_num_components; i++) { // Sample a component from the belief while (counter > 0.0) { - CHECK(component_iter != initial_belief.belief_from_config.end()); + ROBOT_CHECK(component_iter != initial_belief.belief_from_config.end()); counter -= std::exp(component_iter->second.log_config_prob); if (counter > 0.0) { component_iter++; diff --git a/experimental/beacon_sim/precision_matrix_potential.cc b/experimental/beacon_sim/precision_matrix_potential.cc index 31a45e22..563c2b4f 100644 --- a/experimental/beacon_sim/precision_matrix_potential.cc +++ b/experimental/beacon_sim/precision_matrix_potential.cc @@ -59,7 +59,7 @@ double compute_log_prob(const PrecisionMatrixPotential &pot, std::set_intersection(sorted_members.begin(), sorted_members.end(), keys.begin(), keys.end(), std::back_inserter(keys_to_keep)); - CHECK(allow_partial_assignment || missing_keys.empty(), + ROBOT_CHECK(allow_partial_assignment || missing_keys.empty(), "partial assignment specified when not enabled", assignment, missing_keys, pot.members); const std::vector to_marginalize = missing_keys; diff --git a/experimental/beacon_sim/reprocess_result.cc b/experimental/beacon_sim/reprocess_result.cc index e1c98525..9904a384 100644 --- a/experimental/beacon_sim/reprocess_result.cc +++ b/experimental/beacon_sim/reprocess_result.cc @@ -59,19 +59,19 @@ std::tuple load_environment( // Load Map Config const auto maybe_map_config = robot::proto::load_from_file( config_path.parent_path() / config.map_config_path()); - CHECK(maybe_map_config.has_value()); + ROBOT_CHECK(maybe_map_config.has_value()); const WorldMap world_map(unpack_from(maybe_map_config.value())); // Load EKF State const auto maybe_mapped_landmarks = robot::proto::load_from_file( config_path.parent_path() / config.ekf_state_path()); - CHECK(maybe_mapped_landmarks.has_value()); + ROBOT_CHECK(maybe_mapped_landmarks.has_value()); const auto mapped_landmarks = unpack_from(maybe_mapped_landmarks.value()); // Create a road map const auto maybe_road_map = robot::proto::load_from_file( config_path.parent_path() / config.road_map_path()); - CHECK(maybe_road_map.has_value()); + ROBOT_CHECK(maybe_road_map.has_value()); const planning::RoadMap road_map = unpack_from(maybe_road_map.value()); return {world_map.beacon_potential(), road_map, load_ekf_slam(mapped_landmarks)}; @@ -102,7 +102,7 @@ std::vector> load_oracle_metrics( if (trial_id == 0) { out.push_back({}); } - CHECK(out.size() == eval_trial_id + 1); + ROBOT_CHECK(out.size() == eval_trial_id + 1); out.back().push_back({}); auto &to_fill = out.back().back(); @@ -265,7 +265,7 @@ void reprocess_result(const proto::ExperimentResult &result, std::filesystem::create_directories(output_path.parent_path()); { std::ofstream os(output_path); - CHECK(os.good()); + ROBOT_CHECK(os.good()); out.SerializeToOstream(&os); } } @@ -310,7 +310,7 @@ int main(int argc, const char **argv) { const auto maybe_results_file = robot::proto::load_from_file( results_file_path); - CHECK(maybe_results_file.has_value()); + ROBOT_CHECK(maybe_results_file.has_value()); robot::experimental::beacon_sim::reprocess_result( maybe_results_file.value(), diff --git a/experimental/beacon_sim/run_experiment.cc b/experimental/beacon_sim/run_experiment.cc index c7c1d5ec..ebd9b09c 100644 --- a/experimental/beacon_sim/run_experiment.cc +++ b/experimental/beacon_sim/run_experiment.cc @@ -132,13 +132,13 @@ UncertaintySizeOptions from_proto(const proto::UncertaintySize &config) { }; } case proto::UncertaintySize::UNCERTAINTY_SIZE_ONEOF_NOT_SET: { - CHECK(false, "uncertainty size not set"); + ROBOT_CHECK(false, "uncertainty size not set"); return ExpectedDeterminant{ .position_only = false, }; } } - CHECK(false, "uncertainty size not set"); + ROBOT_CHECK(false, "uncertainty size not set"); return ExpectedDeterminant{.position_only = false}; } @@ -318,7 +318,7 @@ proto::ExperimentResult to_proto(const ExperimentConfig &config, for (int planner_idx = 0; planner_idx < static_cast(names.size()); planner_idx++) { std::cout << "Working on planner: " << names.at(planner_idx) << std::endl; - CHECK(result.results.contains(names.at(planner_idx)), "uh oh!", result.results); + ROBOT_CHECK(result.results.contains(names.at(planner_idx)), "uh oh!", result.results); const auto &trial_result = result.results.at(names.at(planner_idx)); proto::PlannerResult &planner_result = *out.add_results(); @@ -351,19 +351,19 @@ void run_experiment(const proto::ExperimentConfig &config, const std::filesystem // Load Map Config const auto maybe_map_config = robot::proto::load_from_file(base_path / config.map_config_path()); - CHECK(maybe_map_config.has_value()); + ROBOT_CHECK(maybe_map_config.has_value()); const WorldMap world_map(unpack_from(maybe_map_config.value())); // Load EKF State const auto maybe_mapped_landmarks = robot::proto::load_from_file(base_path / config.ekf_state_path()); - CHECK(maybe_mapped_landmarks.has_value()); + ROBOT_CHECK(maybe_mapped_landmarks.has_value()); const auto mapped_landmarks = unpack_from(maybe_mapped_landmarks.value()); // Create a road map const auto maybe_road_map = robot::proto::load_from_file(base_path / config.road_map_path()); - CHECK(maybe_road_map.has_value()); + ROBOT_CHECK(maybe_road_map.has_value()); const planning::RoadMap road_map = unpack_from(maybe_road_map.value()); std::mt19937 gen(config.start_goal_seed()); @@ -402,7 +402,7 @@ void run_experiment(const proto::ExperimentConfig &config, const std::filesystem std::unordered_map results; for (const auto &planner_config : config.planner_configs()) { - CHECK(planner_config.planner_config_oneof_case() != + ROBOT_CHECK(planner_config.planner_config_oneof_case() != proto::PlannerConfig::PLANNER_CONFIG_ONEOF_NOT_SET); switch (planner_config.planner_config_oneof_case()) { case proto::PlannerConfig::kLandmarkBrmConfig: { @@ -424,7 +424,7 @@ void run_experiment(const proto::ExperimentConfig &config, const std::filesystem break; } default: { - CHECK(false, "Unhandled Planner Config type", + ROBOT_CHECK(false, "Unhandled Planner Config type", planner_config.planner_config_oneof_case()); } } @@ -479,7 +479,7 @@ int main(int argc, const char **argv) { std::filesystem::path config_file_path = args["config_file"].as(); const auto maybe_config_file = robot::proto::load_from_file(config_file_path); - CHECK(maybe_config_file.has_value()); + ROBOT_CHECK(maybe_config_file.has_value()); robot::experimental::beacon_sim::run_experiment( maybe_config_file.value(), config_file_path.remove_filename(), diff --git a/experimental/beacon_sim/work_server.cc b/experimental/beacon_sim/work_server.cc index dab146f7..7ae0c9c2 100644 --- a/experimental/beacon_sim/work_server.cc +++ b/experimental/beacon_sim/work_server.cc @@ -17,7 +17,7 @@ constexpr int CHUNK_SIZE = 200; std::vector create_inputs_from_result( const std::filesystem::path &result_path, const std::filesystem::path &experiment_config_path) { const auto maybe_proto = robot::proto::load_from_file(result_path); - CHECK(maybe_proto.has_value(), "Failed to load proto", result_path); + ROBOT_CHECK(maybe_proto.has_value(), "Failed to load proto", result_path); const int num_eval_trials = maybe_proto.value().experiment_config().num_eval_trials(); std::vector out; @@ -57,7 +57,7 @@ WorkServer::WorkServer(const std::filesystem::path &db_path, // Check if the table exists const auto rows = db_.query("SELECT count(*) FROM sqlite_master WHERE type='table' and name='" + JOB_TABLE_NAME + "'"); - CHECK(rows.size() == 1); + ROBOT_CHECK(rows.size() == 1); const bool does_table_exist = std::get(rows.at(0).value(0)); if (does_table_exist) { // Table exists @@ -183,19 +183,19 @@ grpc::Status WorkServer::get_progress(grpc::ServerContext *, const proto::Progre { const auto rows = db_.query("SELECT count(*) FROM " + JOB_TABLE_NAME + " WHERE job_result IS NOT NULL;"); - CHECK(!rows.empty()); + ROBOT_CHECK(!rows.empty()); jobs_completed = std::get(rows.at(0).value(0)); } { const auto rows = db_.query("SELECT count(*) FROM " + JOB_TABLE_NAME + " WHERE job_status IS NULL;"); - CHECK(!rows.empty()); + ROBOT_CHECK(!rows.empty()); jobs_remaining = std::get(rows.at(0).value(0)); } { const auto rows = db_.query("SELECT count(*) FROM " + JOB_TABLE_NAME + " WHERE job_status IS NOT NULL AND job_result IS NULL;"); - CHECK(!rows.empty()); + ROBOT_CHECK(!rows.empty()); jobs_in_progress = std::get(rows.at(0).value(0)); } response->set_jobs_completed(jobs_completed); diff --git a/experimental/beacon_sim/world_map.cc b/experimental/beacon_sim/world_map.cc index 0e00dea5..790c2ab3 100644 --- a/experimental/beacon_sim/world_map.cc +++ b/experimental/beacon_sim/world_map.cc @@ -25,7 +25,7 @@ std::vector get_beacon_configuration(const CorrelatedBeaconsConfig &config std::vector out(all_members.size(), false); for (const int beacon_id : present_beacon_ids) { const auto iter = std::find(all_members.begin(), all_members.end(), beacon_id); - CHECK(iter != all_members.end()); + ROBOT_CHECK(iter != all_members.end()); const int idx = std::distance(all_members.begin(), iter); out.at(idx) = true; } diff --git a/experimental/learn_descriptors/symphony_lake_parser.cc b/experimental/learn_descriptors/symphony_lake_parser.cc index 4a68951d..b3e69599 100644 --- a/experimental/learn_descriptors/symphony_lake_parser.cc +++ b/experimental/learn_descriptors/symphony_lake_parser.cc @@ -7,7 +7,7 @@ namespace robot::experimental::learn_descriptors { DataParser::DataParser(const std::filesystem::path &image_root_dir, const std::vector &survey_list) { - CHECK(std::filesystem::exists(image_root_dir), "Image root dir does not exist!", + ROBOT_CHECK(std::filesystem::exists(image_root_dir), "Image root dir does not exist!", image_root_dir); surveys_.load(image_root_dir.string(), survey_list); } diff --git a/experimental/overhead_matching/spectacular_log.cc b/experimental/overhead_matching/spectacular_log.cc index 52a4f823..5e5b1620 100644 --- a/experimental/overhead_matching/spectacular_log.cc +++ b/experimental/overhead_matching/spectacular_log.cc @@ -42,9 +42,9 @@ struct LogData { }; LogData read_jsonl(const fs::path &file_path) { - CHECK(fs::exists(file_path), "jsonl path does not exist", file_path); + ROBOT_CHECK(fs::exists(file_path), "jsonl path does not exist", file_path); std::ifstream in_file(file_path); - CHECK(in_file.is_open(), "Could not open file", file_path); + ROBOT_CHECK(in_file.is_open(), "Could not open file", file_path); LogData out; for (std::string line; std::getline(in_file, line);) { @@ -73,7 +73,7 @@ LogData read_jsonl(const fs::path &file_path) { { const auto &frame = j["frames"].at(0); const auto &calib_info = frame["calibration"]; - CHECK(frame["colorFormat"] == "rgb"); + ROBOT_CHECK(frame["colorFormat"] == "rgb"); calibration[0].focal_length = {calib_info["focalLengthX"].get(), calib_info["focalLengthY"].get()}; @@ -84,7 +84,7 @@ LogData read_jsonl(const fs::path &file_path) { { const auto &frame = j["frames"].at(1); const auto &calib_info = frame["calibration"]; - CHECK(frame["colorFormat"] == "gray"); + ROBOT_CHECK(frame["colorFormat"] == "gray"); calibration[1].focal_length = {calib_info["focalLengthX"].get(), calib_info["focalLengthY"].get()}; diff --git a/planning/road_map.cc b/planning/road_map.cc index c0798ff1..67e6b92c 100644 --- a/planning/road_map.cc +++ b/planning/road_map.cc @@ -33,9 +33,9 @@ void RoadMap::add_start_goal(const StartGoalPair &start_goal_pair) { } } - CHECK(!start_neighbors.empty(), "No roadmap points near start", + ROBOT_CHECK(!start_neighbors.empty(), "No roadmap points near start", start_goal_pair.start.transpose(), start_goal_pair.connection_radius_m); - CHECK(!goal_neighbors.empty(), "No roadmap points near goal", start_goal_pair.goal.transpose(), + ROBOT_CHECK(!goal_neighbors.empty(), "No roadmap points near goal", start_goal_pair.goal.transpose(), start_goal_pair.connection_radius_m); start_goal_ = { @@ -47,7 +47,7 @@ void RoadMap::add_start_goal(const StartGoalPair &start_goal_pair) { } const Eigen::Vector2d &RoadMap::point(const int idx) const { - CHECK(idx >= RoadMap::GOAL_IDX, "Invalid index"); + ROBOT_CHECK(idx >= RoadMap::GOAL_IDX, "Invalid index"); if (idx >= 0) { return points_.at(idx); } else if (idx == RoadMap::START_IDX) { @@ -58,7 +58,7 @@ const Eigen::Vector2d &RoadMap::point(const int idx) const { } std::vector> RoadMap::neighbors(const int idx) const { - CHECK(idx >= RoadMap::GOAL_IDX, "Invalid index"); + ROBOT_CHECK(idx >= RoadMap::GOAL_IDX, "Invalid index"); std::vector> out; if (idx >= 0) { for (int other_idx = 0; other_idx < static_cast(points_.size()); other_idx++) { From fbfe9420358253bb4197c104dd2380dbe1c73ed9 Mon Sep 17 00:00:00 2001 From: Ethan Fahnestock Date: Mon, 2 Dec 2024 12:18:00 -0500 Subject: [PATCH 2/4] lint --- common/check.hh | 3 +- .../kimera_spectacular_data_provider.cc | 527 ++++++++++++++++++ planning/road_map.cc | 6 +- 3 files changed, 532 insertions(+), 4 deletions(-) create mode 100644 experimental/overhead_matching/kimera_spectacular_data_provider.cc diff --git a/common/check.hh b/common/check.hh index 70cf23de..a6e03163 100644 --- a/common/check.hh +++ b/common/check.hh @@ -1,7 +1,8 @@ #include "assert/assert.hpp" -#define ROBOT_CHECK(expr, ...) ASSERT_INVOKE(expr, false, true, "ROBOT_CHECK", verification, , __VA_ARGS__) +#define ROBOT_CHECK(expr, ...) \ + ASSERT_INVOKE(expr, false, true, "ROBOT_CHECK", verification, , __VA_ARGS__) namespace robot { using check_failure = libassert::verification_failure; diff --git a/experimental/overhead_matching/kimera_spectacular_data_provider.cc b/experimental/overhead_matching/kimera_spectacular_data_provider.cc new file mode 100644 index 00000000..fe662d83 --- /dev/null +++ b/experimental/overhead_matching/kimera_spectacular_data_provider.cc @@ -0,0 +1,527 @@ +#include "experimental/overhead_matching/kimera_spectacular_data_provider.hh" + +#include // for max +#include +#include +#include +#include // for pair<> +#include + +#include +#include +#include + +#include +#include +#include + +#include "kimera-vio/frontend/StereoFrame.h" +#include "kimera-vio/imu-frontend/ImuFrontend-definitions.h" +#include "kimera-vio/logging/Logger.h" +#include "kimera-vio/utils/YamlParser.h" + +DEFINE_string(dataset_path, + "/Users/Luca/data/MH_01_easy", + "Path of dataset (i.e. Euroc, /Users/Luca/data/MH_01_easy)."); +DEFINE_int64(initial_k, + 0, + "Initial frame to start processing dataset, " + "previous frames will not be used."); +DEFINE_int64(final_k, + 100000, + "Final frame to finish processing dataset, " + "subsequent frames will not be used."); + +namespace robot::experimental::overhead_matching { + +SpectacularLogDataProvider::SpectacularLogDataProvider(const std::string& dataset_path, + const int& initial_k, + const int& final_k, + const VioParams& vio_params) + : DataProviderInterface(), + vio_params_(vio_params), + dataset_path_(dataset_path), + current_k_(std::numeric_limits::max()), + initial_k_(initial_k), + final_k_(final_k), + imu_measurements_()) { + + ROBOT_CHECK(!dataset_path_.empty()) + << "Dataset path for SpectacularLogDataProvider is empty."; + // Start processing dataset from frame initial_k. + // Useful to skip a bunch of images at the beginning (imu calibration). + ROBOT_CHECK_GE(initial_k_, 0); + + // Finish processing dataset at frame final_k. + // Last frame to process (to avoid processing the entire dataset), + // skip last frames. + ROBOT_CHECK_GT(final_k_, 0); + + ROBOT_CHECK_GT(final_k_, initial_k_) << "Value for final_k (" << final_k_ + << ") is smaller than value for" + << " initial_k (" << initial_k_ << ")."; + current_k_ = initial_k_; + + // Parse the actual dataset first, then run it. + if (!shutdown_ && !dataset_parsed_) { + LOG(INFO) << "Parsing Spectacular dataset..."; + parse(); + ROBOT_CHECK_GT(imu_measurements_.size(), 0u); + dataset_parsed_ = true; + } +} + +/* -------------------------------------------------------------------------- */ +SpectacularLogDataProvider::SpectacularLogDataProvider(const VioParams& vio_params) + : SpectacularLogDataProvider(FLAGS_dataset_path, + FLAGS_initial_k, + FLAGS_final_k, + vio_params) {} + +/* -------------------------------------------------------------------------- */ +SpectacularLogDataProvider::~SpectacularLogDataProvider() { + LOG(INFO) << "SpectacularLogDataProvider destructor called."; +} + +/* -------------------------------------------------------------------------- */ +bool SpectacularLogDataProvider::spin() { + if (dataset_parsed_) { + if (!is_imu_data_sent_) { + // First, send all the IMU data. The flag is to avoid sending it several + // times if we are running in sequential mode. + if (imu_single_callback_) { + sendImuData(); + } else { + LOG(ERROR) << "Imu callback not registered! Not sending IMU data."; + } + is_imu_data_sent_ = true; + } + + // Spin. + ROBOT_CHECK_EQ(vio_params_.camera_params_.size(), 2u); + ROBOT_CHECK_GT(final_k_, initial_k_); + // We log only the first one, because we may be running in sequential mode. + LOG_FIRST_N(INFO, 1) << "Running dataset between frame " << initial_k_ + << " and frame " << final_k_; + while (!shutdown_ && spinOnce()) { + if (!vio_params_.parallel_run_) { + // Return, instead of blocking, when running in sequential mode. + return true; + } + } + } else { + LOG(ERROR) << "Euroc dataset was not parsed."; + } + LOG_IF(INFO, shutdown_) << "SpectacularLogDataProvider shutdown requested."; + return false; +} + +bool SpectacularLogDataProvider::hasData() const { + return current_k_ < final_k_; +} + +/* -------------------------------------------------------------------------- */ +bool SpectacularLogDataProvider::spinOnce() { + ROBOT_CHECK_LT(current_k_, std::numeric_limits::max()) + << "Are you sure you've initialized current_k_?"; + if (current_k_ >= final_k_) { + LOG(INFO) << "Finished spinning Euroc dataset."; + return false; + } + + const CameraParams& left_cam_info = vio_params_.camera_params_.at(0); + const CameraParams& right_cam_info = vio_params_.camera_params_.at(1); + const bool& equalize_image = + vio_params_.frontend_params_.stereo_matching_params_.equalize_image_; + + const Timestamp& timestamp_frame_k = timestampAtFrame(current_k_); + VLOG(10) << "Sending left/right frames k= " << current_k_ + << " with timestamp: " << timestamp_frame_k; + + // TODO(Toni): ideally only send cv::Mat raw images...: + // - pass params to vio_pipeline ctor + // - make vio_pipeline actually equalize or transform images as necessary. + std::string left_img_filename; + bool available_left_img = getLeftImgName(current_k_, &left_img_filename); + std::string right_img_filename; + bool available_right_img = getRightImgName(current_k_, &right_img_filename); + if (available_left_img && available_right_img) { + // Both stereo images are available, send data to VIO + ROBOT_CHECK(left_frame_callback_); + left_frame_callback_( + std::make_unique(current_k_, + timestamp_frame_k, + // TODO(Toni): this info should be passed to + // the camera... not all the time here... + left_cam_info, + UtilsOpenCV::ReadAndConvertToGrayScale( + left_img_filename, equalize_image))); + ROBOT_CHECK(right_frame_callback_); + right_frame_callback_( + std::make_unique(current_k_, + timestamp_frame_k, + // TODO(Toni): this info should be passed to + // the camera... not all the time here... + right_cam_info, + UtilsOpenCV::ReadAndConvertToGrayScale( + right_img_filename, equalize_image))); + } else { + LOG(ERROR) << "Missing left/right stereo pair, proceeding to the next one."; + } + + // This is done directly when parsing the Imu data. + // imu_single_callback_(imu_meas); + + VLOG(10) << "Finished VIO processing for frame k = " << current_k_; + current_k_++; + return true; +} + +void SpectacularLogDataProvider::sendImuData() const { + ROBOT_CHECK(imu_single_callback_) << "Did you forget to register the IMU callback?"; + Timestamp previous_timestamp = -1; + for (const ImuMeasurement& imu_meas : imu_measurements_) { + ROBOT_CHECK_GT(imu_meas.timestamp_, previous_timestamp) + << "Euroc IMU data is not in chronological order!"; + previous_timestamp = imu_meas.timestamp_; + imu_single_callback_(imu_meas); + } +} + +/* -------------------------------------------------------------------------- */ +void SpectacularLogDataProvider::parse() { + VLOG(100) << "Using dataset path: " << dataset_path_; + // Parse the dataset (ETH format). + parseDataset(); + if (VLOG_IS_ON(1)) print(); + + // Send first ground-truth pose to VIO for initialization if requested. + if (vio_params_.backend_params_->autoInitialize_ == 0) { + // We want to initialize from ground-truth. + vio_params_.backend_params_->initial_ground_truth_state_ = + getGroundTruthState(timestampAtFrame(initial_k_)); + } +} + +/* -------------------------------------------------------------------------- */ +bool SpectacularLogDataProvider::parseImuData(const std::string& input_dataset_path, + const std::string& imuName) { + ///////////////// PARSE ACTUAL DATA ////////////////////////////////////////// + //#timestamp [ns],w_RS_S_x [rad s^-1],w_RS_S_y [rad s^-1],w_RS_S_z [rad s^-1], + // a_RS_S_x [m s^-2],a_RS_S_y [m s^-2],a_RS_S_z [m s^-2] + std::string filename_data = + input_dataset_path + "/mav0/" + imuName + "/data.csv"; + std::ifstream fin(filename_data.c_str()); + LOG_IF(FATAL, !fin.is_open()) << "Cannot open file: " << filename_data; + + // Skip the first line, containing the header. + std::string line; + std::getline(fin, line); + + size_t deltaCount = 0u; + Timestamp sumOfDelta = 0; + double stdDelta = 0; + double imu_rate_maxMismatch = 0; + double maxNormAcc = 0, maxNormRotRate = 0; // only for debugging + Timestamp previous_timestamp = -1; + + // Read/store imu measurements, line by line. + while (std::getline(fin, line)) { + Timestamp timestamp = 0; + gtsam::Vector6 gyr_acc_data; + for (int i = 0u; i < gyr_acc_data.size() + 1u; i++) { + int idx = line.find_first_of(','); + if (i == 0) { + timestamp = std::stoll(line.substr(0, idx)); + } else { + gyr_acc_data(i - 1) = std::stod(line.substr(0, idx)); + } + line = line.substr(idx + 1); + } + ROBOT_CHECK_GT(timestamp, previous_timestamp) + << "Euroc IMU data is not in chronological order!"; + Vector6 imu_accgyr; + // Acceleration first! + imu_accgyr << gyr_acc_data.tail(3), gyr_acc_data.head(3); + + double normAcc = gyr_acc_data.tail(3).norm(); + if (normAcc > maxNormAcc) maxNormAcc = normAcc; + + double normRotRate = gyr_acc_data.head(3).norm(); + if (normRotRate > maxNormRotRate) maxNormRotRate = normRotRate; + + //! Store imu measurements + imu_measurements_.push_back(ImuMeasurement(timestamp, imu_accgyr)); + + if (previous_timestamp != -1) { + sumOfDelta += (timestamp - previous_timestamp); + double deltaMismatch = + std::fabs(static_cast( + timestamp - previous_timestamp - + vio_params_.imu_params_.nominal_sampling_time_s_) * + 1e-9); + stdDelta += std::pow(deltaMismatch, 2); + imu_rate_maxMismatch = std::max(imu_rate_maxMismatch, deltaMismatch); + deltaCount += 1u; + } + previous_timestamp = timestamp; + } + + // Converted to seconds. + VLOG(10) << "IMU rate: " + << (static_cast(sumOfDelta) / + static_cast(deltaCount)) * + 1e-9 + << '\n' + << "IMU rate std: " + << std::sqrt(stdDelta / static_cast(deltaCount - 1u)) << '\n' + << "IMU rate max mismatch: " << imu_rate_maxMismatch << '\n' + << "Maximum measured rotation rate (norm):" << maxNormRotRate << '\n' + << "Maximum measured acceleration (norm): " << maxNormAcc; + fin.close(); + + return true; +} + +/* -------------------------------------------------------------------------- */ +bool SpectacularLogDataProvider::parseDataset() { + // Parse IMU data. + ROBOT_CHECK(parseImuData(dataset_path_, kImuName)); + + // Parse Camera data. + CameraImageLists left_cam_image_list; + CameraImageLists right_cam_image_list; + parseCameraData(kLeftCamName, &left_cam_image_list); + if (VLOG_IS_ON(1)) left_cam_image_list.print(); + parseCameraData(kRightCamName, &right_cam_image_list); + if (VLOG_IS_ON(1)) right_cam_image_list.print(); + // TODO(Toni): remove camera_names_ and camera_image_lists_... + camera_names_.push_back(kLeftCamName); + camera_names_.push_back(kRightCamName); + // WARNING Use [x] not .at() because we are adding entries that do not exist. + camera_image_lists_[kLeftCamName] = left_cam_image_list; + camera_image_lists_[kRightCamName] = right_cam_image_list; + // ROBOT_CHECK(sanityCheckCameraData(camera_names_, &camera_image_lists_)); + + // Parse Ground-Truth data. + static const std::string ground_truth_name = "state_groundtruth_estimate0"; + is_gt_available_ = parseGtData(dataset_path_, ground_truth_name); + + clipFinalFrame(); + + // Log Ground-Truth data. + if (logger_) { + if (is_gt_available_) { + logger_->logGtData(dataset_path_ + "/mav0/" + ground_truth_name + + "/data.csv"); + } else { + LOG(ERROR) << "Requested ground-truth data logging but no ground-truth " + "data available."; + } + } + + return true; +} + +/* -------------------------------------------------------------------------- */ +bool SpectacularLogDataProvider::parseCameraData(const std::string& cam_name, + CameraImageLists* cam_list_i) { + ROBOT_CHECK_NOTNULL(cam_list_i) + ->parseCamImgList(dataset_path_ + "/mav0/" + cam_name, "data.csv"); + return true; +} + +std::string SpectacularLogDataProvider::getDatasetName() { + if (dataset_name_.empty()) { + // Find and store actual name (rather than path) of the dataset. + size_t found_last_slash = dataset_path_.find_last_of("/\\"); + std::string dataset_path_tmp = dataset_path_; + dataset_name_ = dataset_path_tmp.substr(found_last_slash + 1); + // The dataset name has a slash at the very end + if (found_last_slash >= dataset_path_tmp.size() - 1) { + // Cut the last slash. + dataset_path_tmp = dataset_path_tmp.substr(0, found_last_slash); + // Repeat the search. + found_last_slash = dataset_path_tmp.find_last_of("/\\"); + // Try to pick right name. + dataset_name_ = dataset_path_tmp.substr(found_last_slash + 1); + } + LOG(INFO) << "Dataset name: " << dataset_name_; + } + return dataset_name_; +} + +/* -------------------------------------------------------------------------- */ +size_t SpectacularLogDataProvider::getNumImages() const { + ROBOT_CHECK_GT(camera_names_.size(), 0u); + const std::string& left_cam_name = camera_names_.at(0); + const std::string& right_cam_name = camera_names_.at(0); + size_t n_left_images = getNumImagesForCamera(left_cam_name); + size_t n_right_images = getNumImagesForCamera(right_cam_name); + ROBOT_CHECK_EQ(n_left_images, n_right_images); + return n_left_images; +} + +/* -------------------------------------------------------------------------- */ +size_t SpectacularLogDataProvider::getNumImagesForCamera( + const std::string& camera_name) const { + const auto& iter = camera_image_lists_.find(camera_name); + ROBOT_CHECK(iter != camera_image_lists_.end()); + return iter->second.getNumImages(); +} + +/* -------------------------------------------------------------------------- */ +bool SpectacularLogDataProvider::getImgName(const std::string& camera_name, + const size_t& k, + std::string* img_filename) const { + ROBOT_CHECK_NOTNULL(img_filename); + const auto& iter = camera_image_lists_.find(camera_name); + ROBOT_CHECK(iter != camera_image_lists_.end()); + const auto& img_lists = iter->second.img_lists_; + if (k < img_lists.size()) { + *img_filename = img_lists.at(k).second; + return true; + } else { + LOG(ERROR) << "Requested image #: " << k << " but we only have " + << img_lists.size() << " images."; + } + return false; +} + +/* -------------------------------------------------------------------------- */ +Timestamp SpectacularLogDataProvider::timestampAtFrame(const FrameId& frame_number) { + ROBOT_CHECK_GT(camera_names_.size(), 0); + ROBOT_CHECK_LT(frame_number, + camera_image_lists_.at(camera_names_[0]).img_lists_.size()); + return camera_image_lists_.at(camera_names_[0]) + .img_lists_[frame_number] + .first; +} + +void SpectacularLogDataProvider::clipFinalFrame() { + // Clip final_k_ to the total number of images. + const size_t& nr_images = getNumImages(); + if (final_k_ > nr_images) { + LOG(WARNING) << "Value for final_k, " << final_k_ << " is larger than total" + << " number of frames in dataset " << nr_images; + final_k_ = nr_images; + LOG(WARNING) << "Using final_k = " << final_k_; + } + ROBOT_CHECK_LE(final_k_, nr_images); +} +/* -------------------------------------------------------------------------- */ +void SpectacularLogDataProvider::print() const { + LOG(INFO) << "------------------ ETHDatasetParser::print ------------------\n" + << "Displaying info for dataset: " << dataset_path_; + // For each of the 2 cameras. + ROBOT_CHECK_EQ(vio_params_.camera_params_.size(), camera_names_.size()); + for (size_t i = 0; i < camera_names_.size(); i++) { + LOG(INFO) << "\n" + << (i == 0 ? "Left" : "Right") + << " camera name: " << camera_names_[i] << ", with params:\n"; + vio_params_.camera_params_.at(i).print(); + camera_image_lists_.at(camera_names_[i]).print(); + } + if (FLAGS_minloglevel < 1) { + gt_data_.print(); + } + LOG(INFO) << "-------------------------------------------------------------"; +} + +////////////////////////////////////////////////////////////////////////////// + +/* -------------------------------------------------------------------------- */ +MonoEurocDataProvider::MonoEurocDataProvider(const std::string& dataset_path, + const int& initial_k, + const int& final_k, + const VioParams& vio_params) + : SpectacularLogDataProvider(dataset_path, initial_k, final_k, vio_params) {} + +/* -------------------------------------------------------------------------- */ +MonoEurocDataProvider::MonoEurocDataProvider(const VioParams& vio_params) + : SpectacularLogDataProvider(vio_params) {} + +/* -------------------------------------------------------------------------- */ +MonoEurocDataProvider::~MonoEurocDataProvider() { + LOG(INFO) << "Mono ETHDataParser destructor called."; +} + +/* -------------------------------------------------------------------------- */ +bool MonoEurocDataProvider::spin() { + if (dataset_parsed_) { + if (!is_imu_data_sent_) { + // First, send all the IMU data. The flag is to avoid sending it several + // times if we are running in sequential mode. + if (imu_single_callback_) { + sendImuData(); + } else { + LOG(ERROR) << "Imu callback not registered! Not sending IMU data."; + } + is_imu_data_sent_ = true; + } + + // Spin. + // ROBOT_CHECK_EQ(vio_params_.camera_params_.size(), 2u); + ROBOT_CHECK_GT(final_k_, initial_k_); + // We log only the first one, because we may be running in sequential mode. + LOG_FIRST_N(INFO, 1) << "Running dataset between frame " << initial_k_ + << " and frame " << final_k_; + while (!shutdown_ && spinOnce()) { + if (!vio_params_.parallel_run_) { + // Return, instead of blocking, when running in sequential mode. + return true; + } + } + } else { + LOG(ERROR) << "Euroc dataset was not parsed."; + } + LOG_IF(INFO, shutdown_) << "SpectacularLogDataProvider shutdown requested."; + return false; +} + +/* -------------------------------------------------------------------------- */ +bool MonoEurocDataProvider::spinOnce() { + ROBOT_CHECK_LT(current_k_, std::numeric_limits::max()) + << "Are you sure you've initialized current_k_?"; + if (current_k_ >= final_k_) { + LOG(INFO) << "Finished spinning Euroc dataset."; + return false; + } + + const CameraParams& left_cam_info = vio_params_.camera_params_.at(0); + const bool& equalize_image = + vio_params_.frontend_params_.stereo_matching_params_.equalize_image_; + + const Timestamp& timestamp_frame_k = timestampAtFrame(current_k_); + VLOG(10) << "Sending left frame k= " << current_k_ + << " with timestamp: " << timestamp_frame_k; + + // TODO(Toni): ideally only send cv::Mat raw images...: + // - pass params to vio_pipeline ctor + // - make vio_pipeline actually equalize or transform images as necessary. + std::string left_img_filename; + bool available_left_img = getLeftImgName(current_k_, &left_img_filename); + if (available_left_img) { + // Both stereo images are available, send data to VIO + ROBOT_CHECK(left_frame_callback_); + left_frame_callback_( + std::make_unique(current_k_, + timestamp_frame_k, + // TODO(Toni): this info should be passed to + // the camera... not all the time here... + left_cam_info, + UtilsOpenCV::ReadAndConvertToGrayScale( + left_img_filename, equalize_image))); + } else { + LOG(ERROR) << "Missing left image, proceeding to the next one."; + } + + // This is done directly when parsing the Imu data. + // imu_single_callback_(imu_meas); + + VLOG(10) << "Finished VIO processing for frame k = " << current_k_; + current_k_++; + return true; +} + +} // namespace diff --git a/planning/road_map.cc b/planning/road_map.cc index 67e6b92c..55b7f3bf 100644 --- a/planning/road_map.cc +++ b/planning/road_map.cc @@ -34,9 +34,9 @@ void RoadMap::add_start_goal(const StartGoalPair &start_goal_pair) { } ROBOT_CHECK(!start_neighbors.empty(), "No roadmap points near start", - start_goal_pair.start.transpose(), start_goal_pair.connection_radius_m); - ROBOT_CHECK(!goal_neighbors.empty(), "No roadmap points near goal", start_goal_pair.goal.transpose(), - start_goal_pair.connection_radius_m); + start_goal_pair.start.transpose(), start_goal_pair.connection_radius_m); + ROBOT_CHECK(!goal_neighbors.empty(), "No roadmap points near goal", + start_goal_pair.goal.transpose(), start_goal_pair.connection_radius_m); start_goal_ = { .start = start_goal_pair.start, From 561580db4a11389733acc0091f7eacefa5134b80 Mon Sep 17 00:00:00 2001 From: efahnestock <13023468+efahnestock@users.noreply.github.com> Date: Mon, 2 Dec 2024 12:19:51 -0500 Subject: [PATCH 3/4] Delete experimental/overhead_matching/kimera_spectacular_data_provider.cc --- .../kimera_spectacular_data_provider.cc | 527 ------------------ 1 file changed, 527 deletions(-) delete mode 100644 experimental/overhead_matching/kimera_spectacular_data_provider.cc diff --git a/experimental/overhead_matching/kimera_spectacular_data_provider.cc b/experimental/overhead_matching/kimera_spectacular_data_provider.cc deleted file mode 100644 index fe662d83..00000000 --- a/experimental/overhead_matching/kimera_spectacular_data_provider.cc +++ /dev/null @@ -1,527 +0,0 @@ -#include "experimental/overhead_matching/kimera_spectacular_data_provider.hh" - -#include // for max -#include -#include -#include -#include // for pair<> -#include - -#include -#include -#include - -#include -#include -#include - -#include "kimera-vio/frontend/StereoFrame.h" -#include "kimera-vio/imu-frontend/ImuFrontend-definitions.h" -#include "kimera-vio/logging/Logger.h" -#include "kimera-vio/utils/YamlParser.h" - -DEFINE_string(dataset_path, - "/Users/Luca/data/MH_01_easy", - "Path of dataset (i.e. Euroc, /Users/Luca/data/MH_01_easy)."); -DEFINE_int64(initial_k, - 0, - "Initial frame to start processing dataset, " - "previous frames will not be used."); -DEFINE_int64(final_k, - 100000, - "Final frame to finish processing dataset, " - "subsequent frames will not be used."); - -namespace robot::experimental::overhead_matching { - -SpectacularLogDataProvider::SpectacularLogDataProvider(const std::string& dataset_path, - const int& initial_k, - const int& final_k, - const VioParams& vio_params) - : DataProviderInterface(), - vio_params_(vio_params), - dataset_path_(dataset_path), - current_k_(std::numeric_limits::max()), - initial_k_(initial_k), - final_k_(final_k), - imu_measurements_()) { - - ROBOT_CHECK(!dataset_path_.empty()) - << "Dataset path for SpectacularLogDataProvider is empty."; - // Start processing dataset from frame initial_k. - // Useful to skip a bunch of images at the beginning (imu calibration). - ROBOT_CHECK_GE(initial_k_, 0); - - // Finish processing dataset at frame final_k. - // Last frame to process (to avoid processing the entire dataset), - // skip last frames. - ROBOT_CHECK_GT(final_k_, 0); - - ROBOT_CHECK_GT(final_k_, initial_k_) << "Value for final_k (" << final_k_ - << ") is smaller than value for" - << " initial_k (" << initial_k_ << ")."; - current_k_ = initial_k_; - - // Parse the actual dataset first, then run it. - if (!shutdown_ && !dataset_parsed_) { - LOG(INFO) << "Parsing Spectacular dataset..."; - parse(); - ROBOT_CHECK_GT(imu_measurements_.size(), 0u); - dataset_parsed_ = true; - } -} - -/* -------------------------------------------------------------------------- */ -SpectacularLogDataProvider::SpectacularLogDataProvider(const VioParams& vio_params) - : SpectacularLogDataProvider(FLAGS_dataset_path, - FLAGS_initial_k, - FLAGS_final_k, - vio_params) {} - -/* -------------------------------------------------------------------------- */ -SpectacularLogDataProvider::~SpectacularLogDataProvider() { - LOG(INFO) << "SpectacularLogDataProvider destructor called."; -} - -/* -------------------------------------------------------------------------- */ -bool SpectacularLogDataProvider::spin() { - if (dataset_parsed_) { - if (!is_imu_data_sent_) { - // First, send all the IMU data. The flag is to avoid sending it several - // times if we are running in sequential mode. - if (imu_single_callback_) { - sendImuData(); - } else { - LOG(ERROR) << "Imu callback not registered! Not sending IMU data."; - } - is_imu_data_sent_ = true; - } - - // Spin. - ROBOT_CHECK_EQ(vio_params_.camera_params_.size(), 2u); - ROBOT_CHECK_GT(final_k_, initial_k_); - // We log only the first one, because we may be running in sequential mode. - LOG_FIRST_N(INFO, 1) << "Running dataset between frame " << initial_k_ - << " and frame " << final_k_; - while (!shutdown_ && spinOnce()) { - if (!vio_params_.parallel_run_) { - // Return, instead of blocking, when running in sequential mode. - return true; - } - } - } else { - LOG(ERROR) << "Euroc dataset was not parsed."; - } - LOG_IF(INFO, shutdown_) << "SpectacularLogDataProvider shutdown requested."; - return false; -} - -bool SpectacularLogDataProvider::hasData() const { - return current_k_ < final_k_; -} - -/* -------------------------------------------------------------------------- */ -bool SpectacularLogDataProvider::spinOnce() { - ROBOT_CHECK_LT(current_k_, std::numeric_limits::max()) - << "Are you sure you've initialized current_k_?"; - if (current_k_ >= final_k_) { - LOG(INFO) << "Finished spinning Euroc dataset."; - return false; - } - - const CameraParams& left_cam_info = vio_params_.camera_params_.at(0); - const CameraParams& right_cam_info = vio_params_.camera_params_.at(1); - const bool& equalize_image = - vio_params_.frontend_params_.stereo_matching_params_.equalize_image_; - - const Timestamp& timestamp_frame_k = timestampAtFrame(current_k_); - VLOG(10) << "Sending left/right frames k= " << current_k_ - << " with timestamp: " << timestamp_frame_k; - - // TODO(Toni): ideally only send cv::Mat raw images...: - // - pass params to vio_pipeline ctor - // - make vio_pipeline actually equalize or transform images as necessary. - std::string left_img_filename; - bool available_left_img = getLeftImgName(current_k_, &left_img_filename); - std::string right_img_filename; - bool available_right_img = getRightImgName(current_k_, &right_img_filename); - if (available_left_img && available_right_img) { - // Both stereo images are available, send data to VIO - ROBOT_CHECK(left_frame_callback_); - left_frame_callback_( - std::make_unique(current_k_, - timestamp_frame_k, - // TODO(Toni): this info should be passed to - // the camera... not all the time here... - left_cam_info, - UtilsOpenCV::ReadAndConvertToGrayScale( - left_img_filename, equalize_image))); - ROBOT_CHECK(right_frame_callback_); - right_frame_callback_( - std::make_unique(current_k_, - timestamp_frame_k, - // TODO(Toni): this info should be passed to - // the camera... not all the time here... - right_cam_info, - UtilsOpenCV::ReadAndConvertToGrayScale( - right_img_filename, equalize_image))); - } else { - LOG(ERROR) << "Missing left/right stereo pair, proceeding to the next one."; - } - - // This is done directly when parsing the Imu data. - // imu_single_callback_(imu_meas); - - VLOG(10) << "Finished VIO processing for frame k = " << current_k_; - current_k_++; - return true; -} - -void SpectacularLogDataProvider::sendImuData() const { - ROBOT_CHECK(imu_single_callback_) << "Did you forget to register the IMU callback?"; - Timestamp previous_timestamp = -1; - for (const ImuMeasurement& imu_meas : imu_measurements_) { - ROBOT_CHECK_GT(imu_meas.timestamp_, previous_timestamp) - << "Euroc IMU data is not in chronological order!"; - previous_timestamp = imu_meas.timestamp_; - imu_single_callback_(imu_meas); - } -} - -/* -------------------------------------------------------------------------- */ -void SpectacularLogDataProvider::parse() { - VLOG(100) << "Using dataset path: " << dataset_path_; - // Parse the dataset (ETH format). - parseDataset(); - if (VLOG_IS_ON(1)) print(); - - // Send first ground-truth pose to VIO for initialization if requested. - if (vio_params_.backend_params_->autoInitialize_ == 0) { - // We want to initialize from ground-truth. - vio_params_.backend_params_->initial_ground_truth_state_ = - getGroundTruthState(timestampAtFrame(initial_k_)); - } -} - -/* -------------------------------------------------------------------------- */ -bool SpectacularLogDataProvider::parseImuData(const std::string& input_dataset_path, - const std::string& imuName) { - ///////////////// PARSE ACTUAL DATA ////////////////////////////////////////// - //#timestamp [ns],w_RS_S_x [rad s^-1],w_RS_S_y [rad s^-1],w_RS_S_z [rad s^-1], - // a_RS_S_x [m s^-2],a_RS_S_y [m s^-2],a_RS_S_z [m s^-2] - std::string filename_data = - input_dataset_path + "/mav0/" + imuName + "/data.csv"; - std::ifstream fin(filename_data.c_str()); - LOG_IF(FATAL, !fin.is_open()) << "Cannot open file: " << filename_data; - - // Skip the first line, containing the header. - std::string line; - std::getline(fin, line); - - size_t deltaCount = 0u; - Timestamp sumOfDelta = 0; - double stdDelta = 0; - double imu_rate_maxMismatch = 0; - double maxNormAcc = 0, maxNormRotRate = 0; // only for debugging - Timestamp previous_timestamp = -1; - - // Read/store imu measurements, line by line. - while (std::getline(fin, line)) { - Timestamp timestamp = 0; - gtsam::Vector6 gyr_acc_data; - for (int i = 0u; i < gyr_acc_data.size() + 1u; i++) { - int idx = line.find_first_of(','); - if (i == 0) { - timestamp = std::stoll(line.substr(0, idx)); - } else { - gyr_acc_data(i - 1) = std::stod(line.substr(0, idx)); - } - line = line.substr(idx + 1); - } - ROBOT_CHECK_GT(timestamp, previous_timestamp) - << "Euroc IMU data is not in chronological order!"; - Vector6 imu_accgyr; - // Acceleration first! - imu_accgyr << gyr_acc_data.tail(3), gyr_acc_data.head(3); - - double normAcc = gyr_acc_data.tail(3).norm(); - if (normAcc > maxNormAcc) maxNormAcc = normAcc; - - double normRotRate = gyr_acc_data.head(3).norm(); - if (normRotRate > maxNormRotRate) maxNormRotRate = normRotRate; - - //! Store imu measurements - imu_measurements_.push_back(ImuMeasurement(timestamp, imu_accgyr)); - - if (previous_timestamp != -1) { - sumOfDelta += (timestamp - previous_timestamp); - double deltaMismatch = - std::fabs(static_cast( - timestamp - previous_timestamp - - vio_params_.imu_params_.nominal_sampling_time_s_) * - 1e-9); - stdDelta += std::pow(deltaMismatch, 2); - imu_rate_maxMismatch = std::max(imu_rate_maxMismatch, deltaMismatch); - deltaCount += 1u; - } - previous_timestamp = timestamp; - } - - // Converted to seconds. - VLOG(10) << "IMU rate: " - << (static_cast(sumOfDelta) / - static_cast(deltaCount)) * - 1e-9 - << '\n' - << "IMU rate std: " - << std::sqrt(stdDelta / static_cast(deltaCount - 1u)) << '\n' - << "IMU rate max mismatch: " << imu_rate_maxMismatch << '\n' - << "Maximum measured rotation rate (norm):" << maxNormRotRate << '\n' - << "Maximum measured acceleration (norm): " << maxNormAcc; - fin.close(); - - return true; -} - -/* -------------------------------------------------------------------------- */ -bool SpectacularLogDataProvider::parseDataset() { - // Parse IMU data. - ROBOT_CHECK(parseImuData(dataset_path_, kImuName)); - - // Parse Camera data. - CameraImageLists left_cam_image_list; - CameraImageLists right_cam_image_list; - parseCameraData(kLeftCamName, &left_cam_image_list); - if (VLOG_IS_ON(1)) left_cam_image_list.print(); - parseCameraData(kRightCamName, &right_cam_image_list); - if (VLOG_IS_ON(1)) right_cam_image_list.print(); - // TODO(Toni): remove camera_names_ and camera_image_lists_... - camera_names_.push_back(kLeftCamName); - camera_names_.push_back(kRightCamName); - // WARNING Use [x] not .at() because we are adding entries that do not exist. - camera_image_lists_[kLeftCamName] = left_cam_image_list; - camera_image_lists_[kRightCamName] = right_cam_image_list; - // ROBOT_CHECK(sanityCheckCameraData(camera_names_, &camera_image_lists_)); - - // Parse Ground-Truth data. - static const std::string ground_truth_name = "state_groundtruth_estimate0"; - is_gt_available_ = parseGtData(dataset_path_, ground_truth_name); - - clipFinalFrame(); - - // Log Ground-Truth data. - if (logger_) { - if (is_gt_available_) { - logger_->logGtData(dataset_path_ + "/mav0/" + ground_truth_name + - "/data.csv"); - } else { - LOG(ERROR) << "Requested ground-truth data logging but no ground-truth " - "data available."; - } - } - - return true; -} - -/* -------------------------------------------------------------------------- */ -bool SpectacularLogDataProvider::parseCameraData(const std::string& cam_name, - CameraImageLists* cam_list_i) { - ROBOT_CHECK_NOTNULL(cam_list_i) - ->parseCamImgList(dataset_path_ + "/mav0/" + cam_name, "data.csv"); - return true; -} - -std::string SpectacularLogDataProvider::getDatasetName() { - if (dataset_name_.empty()) { - // Find and store actual name (rather than path) of the dataset. - size_t found_last_slash = dataset_path_.find_last_of("/\\"); - std::string dataset_path_tmp = dataset_path_; - dataset_name_ = dataset_path_tmp.substr(found_last_slash + 1); - // The dataset name has a slash at the very end - if (found_last_slash >= dataset_path_tmp.size() - 1) { - // Cut the last slash. - dataset_path_tmp = dataset_path_tmp.substr(0, found_last_slash); - // Repeat the search. - found_last_slash = dataset_path_tmp.find_last_of("/\\"); - // Try to pick right name. - dataset_name_ = dataset_path_tmp.substr(found_last_slash + 1); - } - LOG(INFO) << "Dataset name: " << dataset_name_; - } - return dataset_name_; -} - -/* -------------------------------------------------------------------------- */ -size_t SpectacularLogDataProvider::getNumImages() const { - ROBOT_CHECK_GT(camera_names_.size(), 0u); - const std::string& left_cam_name = camera_names_.at(0); - const std::string& right_cam_name = camera_names_.at(0); - size_t n_left_images = getNumImagesForCamera(left_cam_name); - size_t n_right_images = getNumImagesForCamera(right_cam_name); - ROBOT_CHECK_EQ(n_left_images, n_right_images); - return n_left_images; -} - -/* -------------------------------------------------------------------------- */ -size_t SpectacularLogDataProvider::getNumImagesForCamera( - const std::string& camera_name) const { - const auto& iter = camera_image_lists_.find(camera_name); - ROBOT_CHECK(iter != camera_image_lists_.end()); - return iter->second.getNumImages(); -} - -/* -------------------------------------------------------------------------- */ -bool SpectacularLogDataProvider::getImgName(const std::string& camera_name, - const size_t& k, - std::string* img_filename) const { - ROBOT_CHECK_NOTNULL(img_filename); - const auto& iter = camera_image_lists_.find(camera_name); - ROBOT_CHECK(iter != camera_image_lists_.end()); - const auto& img_lists = iter->second.img_lists_; - if (k < img_lists.size()) { - *img_filename = img_lists.at(k).second; - return true; - } else { - LOG(ERROR) << "Requested image #: " << k << " but we only have " - << img_lists.size() << " images."; - } - return false; -} - -/* -------------------------------------------------------------------------- */ -Timestamp SpectacularLogDataProvider::timestampAtFrame(const FrameId& frame_number) { - ROBOT_CHECK_GT(camera_names_.size(), 0); - ROBOT_CHECK_LT(frame_number, - camera_image_lists_.at(camera_names_[0]).img_lists_.size()); - return camera_image_lists_.at(camera_names_[0]) - .img_lists_[frame_number] - .first; -} - -void SpectacularLogDataProvider::clipFinalFrame() { - // Clip final_k_ to the total number of images. - const size_t& nr_images = getNumImages(); - if (final_k_ > nr_images) { - LOG(WARNING) << "Value for final_k, " << final_k_ << " is larger than total" - << " number of frames in dataset " << nr_images; - final_k_ = nr_images; - LOG(WARNING) << "Using final_k = " << final_k_; - } - ROBOT_CHECK_LE(final_k_, nr_images); -} -/* -------------------------------------------------------------------------- */ -void SpectacularLogDataProvider::print() const { - LOG(INFO) << "------------------ ETHDatasetParser::print ------------------\n" - << "Displaying info for dataset: " << dataset_path_; - // For each of the 2 cameras. - ROBOT_CHECK_EQ(vio_params_.camera_params_.size(), camera_names_.size()); - for (size_t i = 0; i < camera_names_.size(); i++) { - LOG(INFO) << "\n" - << (i == 0 ? "Left" : "Right") - << " camera name: " << camera_names_[i] << ", with params:\n"; - vio_params_.camera_params_.at(i).print(); - camera_image_lists_.at(camera_names_[i]).print(); - } - if (FLAGS_minloglevel < 1) { - gt_data_.print(); - } - LOG(INFO) << "-------------------------------------------------------------"; -} - -////////////////////////////////////////////////////////////////////////////// - -/* -------------------------------------------------------------------------- */ -MonoEurocDataProvider::MonoEurocDataProvider(const std::string& dataset_path, - const int& initial_k, - const int& final_k, - const VioParams& vio_params) - : SpectacularLogDataProvider(dataset_path, initial_k, final_k, vio_params) {} - -/* -------------------------------------------------------------------------- */ -MonoEurocDataProvider::MonoEurocDataProvider(const VioParams& vio_params) - : SpectacularLogDataProvider(vio_params) {} - -/* -------------------------------------------------------------------------- */ -MonoEurocDataProvider::~MonoEurocDataProvider() { - LOG(INFO) << "Mono ETHDataParser destructor called."; -} - -/* -------------------------------------------------------------------------- */ -bool MonoEurocDataProvider::spin() { - if (dataset_parsed_) { - if (!is_imu_data_sent_) { - // First, send all the IMU data. The flag is to avoid sending it several - // times if we are running in sequential mode. - if (imu_single_callback_) { - sendImuData(); - } else { - LOG(ERROR) << "Imu callback not registered! Not sending IMU data."; - } - is_imu_data_sent_ = true; - } - - // Spin. - // ROBOT_CHECK_EQ(vio_params_.camera_params_.size(), 2u); - ROBOT_CHECK_GT(final_k_, initial_k_); - // We log only the first one, because we may be running in sequential mode. - LOG_FIRST_N(INFO, 1) << "Running dataset between frame " << initial_k_ - << " and frame " << final_k_; - while (!shutdown_ && spinOnce()) { - if (!vio_params_.parallel_run_) { - // Return, instead of blocking, when running in sequential mode. - return true; - } - } - } else { - LOG(ERROR) << "Euroc dataset was not parsed."; - } - LOG_IF(INFO, shutdown_) << "SpectacularLogDataProvider shutdown requested."; - return false; -} - -/* -------------------------------------------------------------------------- */ -bool MonoEurocDataProvider::spinOnce() { - ROBOT_CHECK_LT(current_k_, std::numeric_limits::max()) - << "Are you sure you've initialized current_k_?"; - if (current_k_ >= final_k_) { - LOG(INFO) << "Finished spinning Euroc dataset."; - return false; - } - - const CameraParams& left_cam_info = vio_params_.camera_params_.at(0); - const bool& equalize_image = - vio_params_.frontend_params_.stereo_matching_params_.equalize_image_; - - const Timestamp& timestamp_frame_k = timestampAtFrame(current_k_); - VLOG(10) << "Sending left frame k= " << current_k_ - << " with timestamp: " << timestamp_frame_k; - - // TODO(Toni): ideally only send cv::Mat raw images...: - // - pass params to vio_pipeline ctor - // - make vio_pipeline actually equalize or transform images as necessary. - std::string left_img_filename; - bool available_left_img = getLeftImgName(current_k_, &left_img_filename); - if (available_left_img) { - // Both stereo images are available, send data to VIO - ROBOT_CHECK(left_frame_callback_); - left_frame_callback_( - std::make_unique(current_k_, - timestamp_frame_k, - // TODO(Toni): this info should be passed to - // the camera... not all the time here... - left_cam_info, - UtilsOpenCV::ReadAndConvertToGrayScale( - left_img_filename, equalize_image))); - } else { - LOG(ERROR) << "Missing left image, proceeding to the next one."; - } - - // This is done directly when parsing the Imu data. - // imu_single_callback_(imu_meas); - - VLOG(10) << "Finished VIO processing for frame k = " << current_k_; - current_k_++; - return true; -} - -} // namespace From aba789687a8044849c276e5e6dc05a098275d81b Mon Sep 17 00:00:00 2001 From: Ethan Fahnestock Date: Mon, 2 Dec 2024 12:22:25 -0500 Subject: [PATCH 4/4] lint pt 2 --- common/math/cubic_hermite_spline.hh | 4 ++-- common/sqlite3/sqlite3.cc | 2 +- experimental/beacon_sim/beacon_potential.cc | 2 +- experimental/beacon_sim/belief_road_map_planner.cc | 5 +++-- experimental/beacon_sim/compute_oracle_results.cc | 6 ++++-- experimental/beacon_sim/correlated_beacon_potential.cc | 8 +++++--- experimental/beacon_sim/ekf_slam.cc | 2 +- experimental/beacon_sim/precision_matrix_potential.cc | 3 ++- experimental/beacon_sim/run_experiment.cc | 4 ++-- experimental/learn_descriptors/symphony_lake_parser.cc | 2 +- 10 files changed, 22 insertions(+), 16 deletions(-) diff --git a/common/math/cubic_hermite_spline.hh b/common/math/cubic_hermite_spline.hh index fa97369e..94f60867 100644 --- a/common/math/cubic_hermite_spline.hh +++ b/common/math/cubic_hermite_spline.hh @@ -21,8 +21,8 @@ class CubicHermiteSpline { } X operator()(const T &query_time) const { - ROBOT_CHECK(query_time >= ts_.front() && query_time <= ts_.back(), "query_time is out of bounds", - ts_.front(), ts_.back(), query_time); + ROBOT_CHECK(query_time >= ts_.front() && query_time <= ts_.back(), + "query_time is out of bounds", ts_.front(), ts_.back(), query_time); const auto iter = std::lower_bound(ts_.begin(), ts_.end(), query_time); if (iter == ts_.end()) { diff --git a/common/sqlite3/sqlite3.cc b/common/sqlite3/sqlite3.cc index 3a080480..6b0e4156 100644 --- a/common/sqlite3/sqlite3.cc +++ b/common/sqlite3/sqlite3.cc @@ -38,7 +38,7 @@ struct Database::Impl { void bind(const Statement &stmt, const std::unordered_map &args) { sqlite3_stmt *stmt_ptr = stmt.impl_->stmt.get(); ROBOT_CHECK(args.size() == sqlite3_bind_parameter_count(stmt_ptr), - "insufficient number of arguments", sqlite3_sql(stmt_ptr), args); + "insufficient number of arguments", sqlite3_sql(stmt_ptr), args); for (const auto &[key, value] : args) { const int param_idx = sqlite3_bind_parameter_index(stmt_ptr, key.c_str()); std::visit( diff --git a/experimental/beacon_sim/beacon_potential.cc b/experimental/beacon_sim/beacon_potential.cc index 799cf78f..0315a3c0 100644 --- a/experimental/beacon_sim/beacon_potential.cc +++ b/experimental/beacon_sim/beacon_potential.cc @@ -123,7 +123,7 @@ BeaconPotential operator*(const BeaconPotential &a, const BeaconPotential &b) { b_sorted_members.begin(), b_sorted_members.end(), std::back_inserter(common_elements)); ROBOT_CHECK(common_elements.empty(), "Found overlap in members of potentials", a_sorted_members, - b_sorted_members, common_elements); + b_sorted_members, common_elements); return CombinedPotential({a, b}); } diff --git a/experimental/beacon_sim/belief_road_map_planner.cc b/experimental/beacon_sim/belief_road_map_planner.cc index eeb0d8a6..d67ff427 100644 --- a/experimental/beacon_sim/belief_road_map_planner.cc +++ b/experimental/beacon_sim/belief_road_map_planner.cc @@ -305,8 +305,9 @@ std::function make_uncertainty_size( return elem.cov_in_robot.determinant(); } } - ROBOT_CHECK(false, "Landmark Belief has insufficient probability mass to get to threshold", - accumulated_prob); + ROBOT_CHECK(false, + "Landmark Belief has insufficient probability mass to get to threshold", + accumulated_prob); return elements.back().cov_in_robot.determinant(); }; } else if (std::holds_alternative(options)) { diff --git a/experimental/beacon_sim/compute_oracle_results.cc b/experimental/beacon_sim/compute_oracle_results.cc index c4e78e76..759a7048 100644 --- a/experimental/beacon_sim/compute_oracle_results.cc +++ b/experimental/beacon_sim/compute_oracle_results.cc @@ -322,7 +322,8 @@ void compute_oracle_results(const std::string server_address, const int server_p update.set_progress(static_cast(plans_completed) / total_plans); pack_into(job_start_time, update.mutable_start_time()); pack_into(time::current_robot_time(), update.mutable_current_time()); - ROBOT_CHECK(client.update_job_status(&client_context, update_request, &update_response).ok()); + ROBOT_CHECK( + client.update_job_status(&client_context, update_request, &update_response).ok()); }; // Compute the results @@ -337,7 +338,8 @@ void compute_oracle_results(const std::string server_address, const int server_p result_request.mutable_job_result()->mutable_plan()->Add(plans.begin(), plans.end()); result_request.set_job_id(maybe_job_response->job_id()); proto::JobResultResponse result_response; - ROBOT_CHECK(client.submit_job_result(&client_context, result_request, &result_response).ok()); + ROBOT_CHECK( + client.submit_job_result(&client_context, result_request, &result_response).ok()); } } } diff --git a/experimental/beacon_sim/correlated_beacon_potential.cc b/experimental/beacon_sim/correlated_beacon_potential.cc index 743faa19..1abad05e 100644 --- a/experimental/beacon_sim/correlated_beacon_potential.cc +++ b/experimental/beacon_sim/correlated_beacon_potential.cc @@ -73,7 +73,8 @@ double compute_log_prob(const CorrelatedBeaconPotential &pot, std::back_inserter(keys_to_keep)); ROBOT_CHECK(allow_partial_assignment || missing_keys.empty(), - "partial assignment specified when not enabled", assignment, missing_keys, pot.members); + "partial assignment specified when not enabled", assignment, missing_keys, + pot.members); const int num_beacons_present = std::count_if(keys_to_keep.begin(), keys_to_keep.end(), @@ -209,8 +210,9 @@ CorrelatedBeaconPotential condition_on(const CorrelatedBeaconPotential &pot, new_assignment = pot.conditioning->conditioned_members; for (const auto &[id, value] : assignment) { const auto &existing_conditioned = pot.conditioning->conditioned_members; - ROBOT_CHECK(!existing_conditioned.contains(id) || (existing_conditioned.at(id) == value), - "Inconsistent conditioning", existing_conditioned, assignment); + ROBOT_CHECK( + !existing_conditioned.contains(id) || (existing_conditioned.at(id) == value), + "Inconsistent conditioning", existing_conditioned, assignment); new_assignment[id] = value; } diff --git a/experimental/beacon_sim/ekf_slam.cc b/experimental/beacon_sim/ekf_slam.cc index 9ad3434d..24d01827 100644 --- a/experimental/beacon_sim/ekf_slam.cc +++ b/experimental/beacon_sim/ekf_slam.cc @@ -28,7 +28,7 @@ std::optional find_beacon_matrix_idx_or_add(const int id, InOutmean.rows() - ROBOT_STATE_DIM) / BEACON_DIM; ROBOT_CHECK(static_cast(est->beacon_ids.size()) < max_num_landmarks, - "Not enough capacity to add beacon to EKF, increase max_num_beacons in ekf_config"); + "Not enough capacity to add beacon to EKF, increase max_num_beacons in ekf_config"); est->beacon_ids.push_back(id); return find_beacon_matrix_idx(est->beacon_ids, id); diff --git a/experimental/beacon_sim/precision_matrix_potential.cc b/experimental/beacon_sim/precision_matrix_potential.cc index 563c2b4f..58b48df3 100644 --- a/experimental/beacon_sim/precision_matrix_potential.cc +++ b/experimental/beacon_sim/precision_matrix_potential.cc @@ -60,7 +60,8 @@ double compute_log_prob(const PrecisionMatrixPotential &pot, std::back_inserter(keys_to_keep)); ROBOT_CHECK(allow_partial_assignment || missing_keys.empty(), - "partial assignment specified when not enabled", assignment, missing_keys, pot.members); + "partial assignment specified when not enabled", assignment, missing_keys, + pot.members); const std::vector to_marginalize = missing_keys; diff --git a/experimental/beacon_sim/run_experiment.cc b/experimental/beacon_sim/run_experiment.cc index ebd9b09c..ea2a202d 100644 --- a/experimental/beacon_sim/run_experiment.cc +++ b/experimental/beacon_sim/run_experiment.cc @@ -403,7 +403,7 @@ void run_experiment(const proto::ExperimentConfig &config, const std::filesystem for (const auto &planner_config : config.planner_configs()) { ROBOT_CHECK(planner_config.planner_config_oneof_case() != - proto::PlannerConfig::PLANNER_CONFIG_ONEOF_NOT_SET); + proto::PlannerConfig::PLANNER_CONFIG_ONEOF_NOT_SET); switch (planner_config.planner_config_oneof_case()) { case proto::PlannerConfig::kLandmarkBrmConfig: { results[planner_config.name()] = run_planner( @@ -425,7 +425,7 @@ void run_experiment(const proto::ExperimentConfig &config, const std::filesystem } default: { ROBOT_CHECK(false, "Unhandled Planner Config type", - planner_config.planner_config_oneof_case()); + planner_config.planner_config_oneof_case()); } } } diff --git a/experimental/learn_descriptors/symphony_lake_parser.cc b/experimental/learn_descriptors/symphony_lake_parser.cc index b3e69599..5f93ab17 100644 --- a/experimental/learn_descriptors/symphony_lake_parser.cc +++ b/experimental/learn_descriptors/symphony_lake_parser.cc @@ -8,7 +8,7 @@ namespace robot::experimental::learn_descriptors { DataParser::DataParser(const std::filesystem::path &image_root_dir, const std::vector &survey_list) { ROBOT_CHECK(std::filesystem::exists(image_root_dir), "Image root dir does not exist!", - image_root_dir); + image_root_dir); surveys_.load(image_root_dir.string(), survey_list); } DataParser::~DataParser() {}