cartographer-project / cartographer

Cartographer is a system that provides real-time simultaneous localization and mapping (SLAM) in 2D and 3D across multiple platforms and sensor configurations.
Apache License 2.0
7.03k stars 2.24k forks source link

No global constraint added with only one trajectory #1887

Open cjchang925 opened 2 years ago

cjchang925 commented 2 years ago

I ran Cartographer with a single rosbag. However, after adding some codes to check whether global constraints were added, I discovered that no global constraints were added, only local constraints were. (In my rosbag, there must be two or more point clouds that look alike because the car was drived around the campus and back to the origin.) I checked the code and discovered that if there is only one trajectory then no global constraints would be added. Inside cartographer::mapping::PoseGraph3D::ComputeConstraint(const NodeId& node_id, const SubmapId& submap_id)

if (node_id.trajectory_id == submap_id.trajectory_id || node_time < last_connection_time + common::FromSeconds(options_.global_constraint_search_after_n_seconds())) {
    // If the node and the submap belong to the same trajectory or if there
    // has been a recent global constraint that ties that node's trajectory to
    // the submap's trajectory, it suffices to do a match constrained to a
    // local search window.
    maybe_add_local_constraint = true;
} else if (global_localization_samplers_[node_id.trajectory_id]->Pulse()) {
    // In this situation, 'global_node_pose' and 'global_submap_pose' have
    // orientations agreeing on gravity. Their relationship regarding yaw is
    // arbitrary. Finding the correct yaw component will be handled by the
    // matching procedure in the FastCorrelativeScanMatcher, and the given yaw
    // is essentially ignored.
    maybe_add_global_constraint = true;
}

Because I only have one trajectory, node_id.trajectory_id == submap_id.trajectory_id will always be true, so maybe_add_global_constraint will always be false, and no global constraint will be added as a result.

I wonder how to solve the problem. Should the mapping process has several trajectories? I am looking forward to any advice. Thanks in advance.