Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Enforce g2o formatting #5

Open
wants to merge 3 commits into
base: main
Choose a base branch
from
Open

Enforce g2o formatting #5

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
acc8274
update read_g2o_file to enforce g2o formatting requirements
adthoms Jun 17, 2024
f728f47
add log error message when odometry is discontinuous
adthoms Jun 17, 2024
130d0ed
update ais2klinik.g2o for continuous odometry
adthoms Jun 17, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
1 change: 1 addition & 0 deletions data/ais2klinik.g2o
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
Copy link
Collaborator

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I'm not sure about adding this edge because it will change the optimal cost and solution (although probably in a negligible way), and so might confuse others who just want to access the original dataset to test other solvers.

For now, I think leaving the dataset unchanged but printing the errors in dpgo as you have implemented is a better way to go.

Copy link
Collaborator

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Also, it's possible that the consecutive ID assumption is only needed by odometry initialization and chordal initialization. So one could potentially just improve these initialization implementations without changing the dataset, but I will need to confirm this is the case.

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
Expand Down
4 changes: 2 additions & 2 deletions src/DPGO_solver.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down
48 changes: 37 additions & 11 deletions src/DPGO_utils.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -113,10 +113,10 @@ Cartan-Sync: https://bitbucket.org/jesusbriales/cartan-sync/src
std::vector<RelativeSEMeasurement> read_g2o_file(const std::string &filename,
size_t &num_poses) {
// Preallocate output vector
std::vector<DPGO::RelativeSEMeasurement> measurements;
std::vector<RelativeSEMeasurement> 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
Expand All @@ -128,13 +128,13 @@ std::vector<RelativeSEMeasurement> 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<size_t> pose_ids;

while (std::getline(infile, line)) {
// Construct a stream from the string
Expand Down Expand Up @@ -172,7 +172,6 @@ std::vector<RelativeSEMeasurement> 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) {
Expand Down Expand Up @@ -224,7 +223,6 @@ std::vector<RelativeSEMeasurement> 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());
Expand All @@ -238,20 +236,48 @@ std::vector<RelativeSEMeasurement> 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<double>(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;
Copy link
Collaborator

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

@adthoms This will cause overflow because prev_pose_id is unsigned and first_pose_id is nominally zero.

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;
}
Expand Down