From acc8274400a9867b403e4c9bee1f4f3e283263be Mon Sep 17 00:00:00 2001 From: adthoms Date: Mon, 17 Jun 2024 12:34:39 -0700 Subject: [PATCH 1/3] update read_g2o_file to enforce g2o formatting requirements cleanup namespace and whitespace --- src/DPGO_utils.cpp | 48 +++++++++++++++++++++++++++++++++++----------- 1 file changed, 37 insertions(+), 11 deletions(-) diff --git a/src/DPGO_utils.cpp b/src/DPGO_utils.cpp index 757efe8..b31733c 100644 --- a/src/DPGO_utils.cpp +++ b/src/DPGO_utils.cpp @@ -113,10 +113,10 @@ Cartan-Sync: https://bitbucket.org/jesusbriales/cartan-sync/src std::vector read_g2o_file(const std::string &filename, size_t &num_poses) { // Preallocate output vector - std::vector measurements; + std::vector measurements; // A single measurement, whose values we will fill in - DPGO::RelativeSEMeasurement measurement; + RelativeSEMeasurement measurement; measurement.weight = 1.0; // A string used to contain the contents of a single line @@ -128,13 +128,13 @@ std::vector read_g2o_file(const std::string &filename, // Preallocate various useful quantities double dx, dy, dz, dtheta, dqx, dqy, dqz, dqw, I11, I12, I13, I14, I15, I16, I22, I23, I24, I25, I26, I33, I34, I35, I36, I44, I45, I46, I55, I56, I66; - size_t i, j; // Open the file for reading std::ifstream infile(filename); - num_poses = 0; + // Create Pose ID set + std::set pose_ids; while (std::getline(infile, line)) { // Construct a stream from the string @@ -172,7 +172,6 @@ std::vector read_g2o_file(const std::string &filename, Eigen::Matrix2d TranCov; TranCov << I11, I12, I12, I22; measurement.tau = 2 / TranCov.inverse().trace(); - measurement.kappa = I33; if (i+1 == j) { @@ -224,7 +223,6 @@ std::vector read_g2o_file(const std::string &filename, // Compute and store the optimal (information-divergence-minimizing value // of the parameter kappa - Eigen::Matrix3d RotCov; RotCov << I44, I45, I46, I45, I55, I56, I46, I56, I66; measurement.kappa = 3 / (2 * RotCov.inverse().trace()); @@ -238,20 +236,48 @@ std::vector read_g2o_file(const std::string &filename, } else if ((token == "VERTEX_SE2") || (token == "VERTEX_SE3:QUAT")) { // This is just initialization information, so do nothing continue; + } else if ((token == "FIX")) { + LOG(WARNING) << "[read_g2o_file] FIX ID_SET is not supported. Skipping line..."; + continue; } else { - LOG(FATAL) << "Error: unrecognized type: " << token << "!"; + LOG(FATAL) << "[read_g2o_file] Unrecognized type: " << token << "!"; } - // Update maximum value of poses found so far - size_t max_pair = std::max(measurement.p1, measurement.p2); + // Update pose IDs + pose_ids.emplace(measurement.p1); + pose_ids.emplace(measurement.p2); - num_poses = ((max_pair > num_poses) ? max_pair : num_poses); measurements.push_back(measurement); } // while infile.close(); - num_poses++; // Account for the use of zero-based indexing + // Get first pose ID + const size_t first_pose_id = *pose_ids.begin(); + + // Check for consecutive sequencing of pose IDs + size_t prev_pose_id = first_pose_id - 1; + for (const size_t& pose_id : pose_ids) { + if (pose_id != prev_pose_id + 1) { + LOG(FATAL) << "[read_g2o_file] Invalid pose ID sequencing: [" << prev_pose_id << "," << pose_id << "]. " + << "The set of pose IDs must be consecutive!"; + } + prev_pose_id = pose_id; + } + + // Reindex Pose IDs from zero if necessary + if (first_pose_id != 0) { + LOG(WARNING) << "[read_g2o_file] Invalid first pose ID: " << first_pose_id << ". " + << "Pose IDs will be re-indexed starting from zero."; + + // Decrement all pose IDs by the first pose ID + for (RelativeSEMeasurement& measurement : measurements) { + measurement.p1 -= first_pose_id; + measurement.p2 -= first_pose_id; + } + } + + num_poses = pose_ids.size(); return measurements; } From f728f47f767246a2eb137929e7d7194ff7d49ab1 Mon Sep 17 00:00:00 2001 From: adthoms Date: Mon, 17 Jun 2024 12:35:17 -0700 Subject: [PATCH 2/3] add log error message when odometry is discontinuous --- src/DPGO_solver.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/DPGO_solver.cpp b/src/DPGO_solver.cpp index 3b17a82..7d2d2bc 100644 --- a/src/DPGO_solver.cpp +++ b/src/DPGO_solver.cpp @@ -291,8 +291,8 @@ PoseArray odometryInitialization( for (size_t dst = next_index; dst < num_poses; ++dst) { size_t src = dst - 1; const RelativeSEMeasurement &m = odometry[src]; - CHECK(m.p1 == src); - CHECK(m.p2 == dst); + CHECK(m.p1 == src) << ": [odometryInitialization] Error: Odometry measurements are discontinuous at pose: " << src; + CHECK(m.p2 == dst) << ": [odometryInitialization] Error: Odometry measurements are discontinuous at pose: " << dst; const Matrix Rsrc = T.rotation(src); const Matrix tsrc = T.translation(src); T.rotation(dst) = Rsrc * m.R; From 130d0ed199dbc06766ab70eecec945691ad58049 Mon Sep 17 00:00:00 2001 From: adthoms Date: Mon, 17 Jun 2024 12:38:59 -0700 Subject: [PATCH 3/3] update ais2klinik.g2o for continuous odometry odometry is ground truth with negligible weight --- data/ais2klinik.g2o | 1 + 1 file changed, 1 insertion(+) diff --git a/data/ais2klinik.g2o b/data/ais2klinik.g2o index a7320f4..3124b25 100644 --- a/data/ais2klinik.g2o +++ b/data/ais2klinik.g2o @@ -22403,6 +22403,7 @@ EDGE_SE2 7285 7286 0.628132 0.0065 0.009066 102.196 3.89037 9.59586 105.601 8.07 EDGE_SE2 7286 7287 0.52826 0.000141 -0.002814 100.995 1.65564 3.57699 105.323 1.86886 248.159 EDGE_SE2 7287 7288 0.481197 0.000464 0.019296 101.345 2.64031 0.816055 106.074 2.28584 273.897 EDGE_SE2 7288 7289 0.620686 0.02638 0.08998 103.967 5.06454 -25.856 108.448 -16.176 302.116 +EDGE_SE2 7289 7290 69.372726 240.060823 -0.504155 1e-06 1e-06 1e-06 1e-04 1e-04 1e-04 EDGE_SE2 7290 7291 0.37082 0.071019 0.25526 100.946 2.45857 -1.64992 115.019 35.988 219.439 EDGE_SE2 7291 7292 0.437501 -0.000257 -0.000247 108.077 0.443883 17.9799 111.043 16.9425 218.789 EDGE_SE2 7292 7293 0.457584 0.029186 0.136393 109.433 -6.66104 18.9376 110.74 7.54229 213.112