Submap Frame Question

[alexmillane]

I have a (hopefully simple) question about the frames associated with cartographer submaps (in 2D).

I will try to explain with an example, hopefully, this will make things clearer. I am trying to perform some analysis on submaps produced in the Deutsches Museum. I would like to overlay pairs of submaps (in Matlab for argument's sake) displaced by the cartographer estimated pose. I can read the submaps from an optimized map from a protofile into MATLAB and plot them (below I'm showing submaps 0 and 1 of b2-2016-04-05-14-44-52.bag (chosen at random))

The global pose is correct (because things look great in RVIZ) but so far I am unable to interpret it. For the two maps above we have

pose: { t: [0, 0], r: [0] } pose: { t: [-7.91739, 1.36326], r: [-0.0226751] }

Looking at the example above the (global) transform cannot describe the transformation between the submap's corner or center-point and the global coordinate frame. It must describe the transformation to some point within the submap. So (I think) my question is: which point in each submap does the global pose refer to?

Cheers for the help team, I'm thoroughly confused.

[MichaelGrupp]

Each submap has its own coordinate frame origin that corresponds to the position of the first (accumulated) range scan data that was inserted into it. This means that global submap pose that you are seeing lies on the SLAM trajectory and is not at some special point like the center or corner.

Here's the code: https://github.com/googlecartographer/cartographer/blob/07b92072a06e10fb4fb3d8fbe8f6fbb1815728d3/cartographer/mapping/2d/submap_2d.cc#L163-L166

This means: if we have no submap yet (i.e. we are starting to map) or if it's time to create a new submap, insert a submap at the position of the origin of the range data that we want to insert into it.

But, it's a position, not a pose, so the submap origins won't have the orientation of the trajectory nodes:

[alexmillane]

Thanks so much for the reply. This makes things at least half clearer. The remaining question is how are the grid points are located relative to the to the submap origin.

In the end, I'm looking for the coordinates of grid points in the global coordinate frame (let's just say the top left grid point for argument's sake). I would have expected the submap origin to define the (0,0) coordinate of its grid, but from looking at the code, that's not the case.

[JakeInit]

I know this issue is nearly a year old, I have questions similar to this. I am taking pixels from the global image and trying to compare its position to a position in the submap frame.

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());
  }
}

This should give me a pose for the submap which I believe is relative to the global map origin (which is the same as the origin of submap 0). My approach was to use the global position of the pixel I'm looking at and to use the proto data from pbstream file made from mapping to get a transform from the global frame to the local. Am I correct in assuming the above code achieves this?

Inside the deserializer while loop, I have:

auto submap = proto.submap();
int submapHeight_pix = submap.submap_2d().grid().limits().cell_limits().num_x_cells();
int submapWidth_pix = submap.submap_2d().grid().limits().cell_limits().num_y_cells();
double submapMaxX_m = submap.submap_2d().grid().limits().max().x();
double submapMaxY_m = submap.submap_2d().grid().limits().max().y();
double submapMinX_m = submapMaxX_m - submapWidth_m;
double submapMinY_m = submapMaxY_m - submapHeight_m ;

My assumption is the above will give me the physical bounds of the submap that I'm analyzing. By using the found transformation data above, I can convert a pixel location in the global map, to a pixel location in a local submap frame. Then I can use the bounds to determine if that pixel would even exist in the local submap.

I am later trying to determine which cell in the mutable submap, corresponds to the global pixel that I'm manipulating.

proto.mutable_submap()->mutable_submap_2d()->mutable_grid()->mutable_cells()->at((int) cellIndexOfGlobalPixel)

My issue is that if I try to manipulate the value of the global pixel and set the change in the pbstream, and then redraw my map, my edited pixel is not anywhere I expect it to be.

My issue stems from a similar question asked in Editing Cell Values From a known Global pose #1792.