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.09k stars 2.25k forks source link

Converting between Global Pose and Submap Pose in the protobuf #1802

Closed JakeInit closed 3 years ago

JakeInit commented 3 years ago

The issue stems from a similar question I asked in Editing Cell Values From a known Global pose #1792. I figured I would simply the question here for what may provide a quicker response. Answering this question may allow me to close both of these questions.

If I have a pixel in the global frame with a given x, y, how do I know the x, y value in each submap frame? I have started with:

io::ProtoStreamReader reader(pbstreamFileToOpen);
io::ProtoStreamDeserializer deserializer(&reader);
mapping::proto::PoseGraph pose_graph_proto = deserializer.pose_graph();
mapping::proto::Trajectory trajectory_proto = *pose_graph_proto.mutable_trajectory(trajectoryID);

mapping::proto::SerializedData proto;
while (deserializer.ReadNextSerializedData(&proto)) {
  if(proto.has_submap()) {
    auto submapGlobalPose = transform::ToRigid3(trajectory_proto.submap(submapId).pose());  // Is global the correct assumption?
  }
}

The assumption is that this should give me a transformation from the global map, to the submap. The global map origin from what I understand is the same as the origin of submap 0. Therefore there is no transformation from global map to submap 0, but on other submaps, there should be. From the linked issue at the top, I believe this is where the origin of my issue exists. I have no problem pinpointing cells in submap 0 from the global map, but once I move onto other submaps, I start getting many errors. It leads me to believe that my assumptions about the code above are wrong.

UPDATE: It looks like I may need to update the Global map frame to the local, and then to the submap frame. It looks like I can get the local pose from inside the while above by:

auto submap = proto.submap();
auto localSubmapPose = transform::ToRigid3(submap.submap_2d().local_pose());

The question is then, how do I determine the global to local transform, or vice versa, and how to determine the local to submap? I assume I am supposed to do transforms in the order of Global -> Local -> Submap. In short, how do I go from Global position to a submap frame position.

JakeInit commented 3 years ago

I found the solution to my own issue. It seems that no transform is needed at all. My suspicion is that the pbstream already holds the matched version of the submaps.