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

Writing Image File from Pbstream #1790

Closed JakeInit closed 3 years ago

JakeInit commented 3 years ago

For writing a pgm file, I have the following

io::ProtoStreamReader reader(fileToOpen);
  io::ProtoStreamDeserializer deserializer(&reader);
  const mapping::proto::AllTrajectoryBuilderOptions trajectoryOptions = deserializer.all_trajectory_builder_options();
  mapping::proto::PoseGraph poseGraph = deserializer.pose_graph();
  mapping::proto::Trajectory trajectory = *poseGraph.trajectory().begin();
  int trajectoryID = trajectory.trajectory_id();
  float resolution = trajectoryOptions.options_with_sensor_ids(trajectoryID).trajectory_builder_options().
      trajectory_builder_2d_options().submaps_options().grid_options_2d().resolution();

  std::map<mapping::SubmapId, io::SubmapSlice> submap_slices;
  mapping::ValueConversionTables value_conversion_tables;
  value_conversion_tables.GetConversionTable(0.1f, 0.1f, 0.5f);
  mapping::proto::SerializedData proto;
  while (deserializer.ReadNextSerializedData(&proto)) {
    if (proto.has_submap()) {
      const auto &submap = proto.submap();
      auto submapId = submap.submap_id().submap_index();
      const mapping::SubmapId id{trajectoryID, submapId};
      const transform::Rigid3d global_submap_pose = transform::ToRigid3(trajectory.submap(submapId).pose());

      io::FillSubmapSlice(global_submap_pose, submap, &submap_slices[id], &value_conversion_tables);
    }
  } // end while

  auto result = io::PaintSubmapSlices(submap_slices, resolution);
  io::StreamFileWriter pgm_writer((std::string) PROJECTDIR + "maps/" + filename + ".pgm");
  io::Image image(std::move(result.surface));
  writePgm(image, resolution, &pgm_writer);

// This is a separate function for writePgm
void writePgm(const io::Image &image, const double resolution, io::FileWriter *file_writer) {
  std::string header = "P5\n# Cartographer map; " + std::to_string(resolution) + "m/pixel\n"
      + std::to_string(image.width()) + " " + std::to_string(image.height()) + "\n255\n";
  char data[header.size()];
  strcpy(data, header.c_str());
  file_writer->WriteHeader(data, sizeof(data));

  for (int y = 0; y < image.height(); ++y) {
    for (int x = 0; x < image.width(); ++x) {
      const char color = image.GetPixel(x, y)[0];
      file_writer->Write(&color, 1);
    }
  }
}

This works without any issue. I want to know how the cairo data is calculated, specifically submap_slices.at(id).cairo_data.at(i). This is the information for the pixel that goes into the PGM correct? I'm pretty certain it is based off of the data found when deserializing the information with proto. The information would be from

'proto.mutable_submap()->mutable_submap_2d()->mutable_grid()->mutable_cells()->at(i);'

I have tried following FillSubmapSlice and all the GetProbability functions and though I had the conversion down, but the calculation I have does not turn out correct when comparing existing values. It looks like the Cairo value is a concatenation of alpha, intensity, observed, and then 0x00. How do I get each of the values from the cell value?