Skip to content

Commit

Permalink
2d ate in comparison eval, report bad trajectory distance ratios to h…
Browse files Browse the repository at this point in the history 
…elp debugging
  • Loading branch information
@goldbattle
goldbattle committed May 22, 2023
1 parent 2b506ee commit 7e4dd43
Show file tree
Hide file tree
Showing 2 changed files with 58 additions and 3 deletions.
7 changes: 7 additions & 0 deletions ov_eval/src/calc/ResultTrajectory.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -44,6 +44,13 @@ ResultTrajectory::ResultTrajectory(std::string path_est, std::string path_gt, st
PRINT_ERROR(RED "[TRAJ]: does the estimated trajectory publish the rosbag timestamps??\n" RESET);
std::exit(EXIT_FAILURE);
}
double len_gt = ov_eval::Loader::get_total_length(gt_poses);
double len_est = ov_eval::Loader::get_total_length(est_poses);
double ratio = len_est / len_gt;
if (ratio > 1.1 || ratio < 0.9) {
PRINT_WARNING(YELLOW "[TRAJ]: Trajectory is a bad ratio of %.2f length (est %.2f, gt %.2f)\n", ratio, len_est, len_gt);
PRINT_WARNING(YELLOW "[TRAJ]: %s\n", path_est.c_str());
}

// Perform alignment of the trajectories
Eigen::Matrix3d R_ESTtoGT, R_GTtoEST;
Expand Down
54 changes: 51 additions & 3 deletions ov_eval/src/error_comparison.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -91,20 +91,22 @@ int main(int argc, char **argv) {

// ATE summery information
std::map<std::string, std::vector<std::pair<ov_eval::Statistics, ov_eval::Statistics>>> algo_ate;
std::map<std::string, std::vector<std::pair<ov_eval::Statistics, ov_eval::Statistics>>> algo_ate_2d;
for (const auto &p : path_algorithms) {
std::vector<std::pair<ov_eval::Statistics, ov_eval::Statistics>> temp;
for (size_t i = 0; i < path_groundtruths.size(); i++) {
temp.push_back({ov_eval::Statistics(), ov_eval::Statistics()});
}
algo_ate.insert({p.filename().string(), temp});
algo_ate_2d.insert({p.filename().string(), temp});
}

// Relative pose error segment lengths
std::vector<double> segments = {8.0, 16.0, 24.0, 32.0, 40.0, 48.0};
// std::vector<double> segments = {7.0, 14.0, 21.0, 28.0, 35.0};
// std::vector<double> segments = {10.0, 25.0, 50.0, 75.0, 120.0};
// std::vector<double> segments = {5.0, 15.0, 30.0, 45.0, 60.0};
// std::vector<double> segments = {40.0, 60.0, 80.0, 100.0, 120.0};
// std::vector<double> segments = {40.0, 80.0, 120.0, 160.0, 200.0, 240.0};

// The overall RPE error calculation for each algorithm type
std::map<std::string, std::map<double, std::pair<ov_eval::Statistics, ov_eval::Statistics>>> algo_rpe;
Expand Down Expand Up @@ -150,8 +152,8 @@ int main(int argc, char **argv) {
path_groundtruths.at(j).stem().c_str());

// Errors for this specific dataset (i.e. our averages over the total runs)
ov_eval::Statistics ate_dataset_ori;
ov_eval::Statistics ate_dataset_pos;
ov_eval::Statistics ate_dataset_ori, ate_dataset_pos;
ov_eval::Statistics ate_2d_dataset_ori, ate_2d_dataset_pos;
std::map<double, std::pair<ov_eval::Statistics, ov_eval::Statistics>> rpe_dataset;
for (const auto &len : segments) {
rpe_dataset.insert({len, {ov_eval::Statistics(), ov_eval::Statistics()}});
Expand Down Expand Up @@ -181,6 +183,12 @@ int main(int argc, char **argv) {
ate_dataset_ori.values.push_back(error_ori.rmse);
ate_dataset_pos.values.push_back(error_pos.rmse);

// Calculate ATE 2D error for this dataset
ov_eval::Statistics error_ori_2d, error_pos_2d;
traj.calculate_ate_2d(error_ori_2d, error_pos_2d);
ate_2d_dataset_ori.values.push_back(error_ori_2d.rmse);
ate_2d_dataset_pos.values.push_back(error_pos_2d.rmse);

// Calculate RPE error for this dataset
std::map<double, std::pair<ov_eval::Statistics, ov_eval::Statistics>> error_rpe;
traj.calculate_rpe(segments, error_rpe);
Expand All @@ -199,12 +207,17 @@ int main(int argc, char **argv) {
// Compute our mean ATE score
ate_dataset_ori.calculate();
ate_dataset_pos.calculate();
ate_2d_dataset_ori.calculate();
ate_2d_dataset_pos.calculate();

// Print stats for this specific dataset
std::string prefix = (ate_dataset_ori.mean > 10 || ate_dataset_pos.mean > 10) ? RED : "";
PRINT_DEBUG("%s\tATE: mean_ori = %.3f | mean_pos = %.3f (%d runs)\n" RESET, prefix.c_str(), ate_dataset_ori.mean,
ate_dataset_pos.mean, (int)ate_dataset_pos.values.size());
PRINT_DEBUG("\tATE: std_ori = %.3f | std_pos = %.3f\n", ate_dataset_ori.std, ate_dataset_pos.std);
PRINT_DEBUG("\tATE 2D: mean_ori = %.3f | mean_pos = %.3f (%d runs)\n", ate_2d_dataset_ori.mean, ate_2d_dataset_pos.mean,
(int)ate_2d_dataset_ori.values.size());
PRINT_DEBUG("\tATE 2D: std_ori = %.5f | std_pos = %.5f\n", ate_2d_dataset_ori.std, ate_2d_dataset_pos.std);
for (auto &seg : rpe_dataset) {
seg.second.first.calculate();
seg.second.second.calculate();
Expand All @@ -219,6 +232,8 @@ int main(int argc, char **argv) {
std::string algo = path_algorithms.at(i).filename().string();
algo_ate.at(algo).at(j).first = ate_dataset_ori;
algo_ate.at(algo).at(j).second = ate_dataset_pos;
algo_ate_2d.at(algo).at(j).first = ate_2d_dataset_ori;
algo_ate_2d.at(algo).at(j).second = ate_2d_dataset_pos;

// Update the global RPE error stats
for (const auto &elm : rpe_dataset) {
Expand Down Expand Up @@ -272,6 +287,39 @@ int main(int argc, char **argv) {
}
PRINT_INFO("============================================\n");

// Finally print the ATE for all the runs
PRINT_INFO("============================================\n");
PRINT_INFO("ATE 2D LATEX TABLE\n");
PRINT_INFO("============================================\n");
for (size_t i = 0; i < path_groundtruths.size(); i++) {
std::string gtname = path_groundtruths.at(i).stem().string();
boost::replace_all(gtname, "_", "\\_");
PRINT_INFO(" & \\textbf{%s}", gtname.c_str());
}
PRINT_INFO(" & \\textbf{Average} \\\\\\hline\n");
for (auto &algo : algo_ate_2d) {
std::string algoname = algo.first;
boost::replace_all(algoname, "_", "\\_");
PRINT_INFO(algoname.c_str());
double sum_ori = 0.0;
double sum_pos = 0.0;
int sum_ct = 0;
for (auto &seg : algo.second) {
if (seg.first.values.empty() || seg.second.values.empty()) {
PRINT_INFO(" & - / -");
} else {
seg.first.calculate();
seg.second.calculate();
PRINT_INFO(" & %.3f / %.3f", seg.first.mean, seg.second.mean);
sum_ori += seg.first.mean;
sum_pos += seg.second.mean;
sum_ct++;
}
}
PRINT_INFO(" & %.3f / %.3f \\\\\n", sum_ori / sum_ct, sum_pos / sum_ct);
}
PRINT_INFO("============================================\n");

// Finally print the RPE for all the runs
PRINT_INFO("============================================\n");
PRINT_INFO("RPE LATEX TABLE\n");
Expand Down

0 comments on commit 7e4dd43

Please sign in to comment.