#include <map>
#include <string>
#include "cartographer/io/proto_stream.h"
#include "cartographer/io/proto_stream_deserializer.h"
#include "cartographer/io/submap_painter.h"
#include "cartographer/mapping/2d/probability_grid.h"
#include "cartographer/mapping/2d/submap_2d.h"
#include "cartographer/mapping/3d/submap_3d.h"
#include "cartographer/mapping/proto/pose_graph.pb.h"
#include "cartographer/mapping/proto/serialization.pb.h"
#include "cartographer/mapping/proto/submap.pb.h"
#include "cartographer/mapping/proto/trajectory_builder_options.pb.h"
#include "cartographer_ros/ros_map.h"
#include "cartographer_ros/submap.h"
#include "gflags/gflags.h"
#include "glog/logging.h"
DEFINE_string(pbstream_filename, "",
"Filename of a pbstream to draw a map from.");
DEFINE_string(map_filestem, "map", "Stem of the output files.");
DEFINE_double(resolution, 0.05, "Resolution of a grid cell in the drawn map.");
//找到这个类是根据生成全局图的命令pbstream_to_ros_map
namespace cartographer_ros {
namespace {
void Run(const std::string& pbstream_filename, const std::string& map_filestem,
const double resolution) {
//reader建立内存到pbstream数据的连接
::cartographer::io::ProtoStreamReader reader(pbstream_filename);
//将pbstream中的数据反序列化
::cartographer::io::ProtoStreamDeserializer deserializer(&reader);
//得到反序列化后的pose_graph
const auto& pose_graph = deserializer.pose_graph();
LOG(INFO) << "Loading submap slices from serialized data.";
std::map<::cartographer::mapping::SubmapId, ::cartographer::io::SubmapSlice>
submap_slices;
::cartographer::mapping::proto::SerializedData proto;
把反序列化的数据读到proto中去
while (deserializer.ReadNextSerializedData(&proto)) {
if (proto.has_submap()) {
//得到submap数据
const auto& submap = proto.submap();
const ::cartographer::mapping::SubmapId id{
submap.submap_id().trajectory_id(),
submap.submap_id().submap_index()};
//得到submap在全局地图的位姿
const ::cartographer::transform::Rigid3d global_submap_pose =
::cartographer::transform::ToRigid3(
pose_graph.trajectory(id.trajectory_id)
.submap(id.submap_index)
.pose());
FillSubmapSlice(global_submap_pose, submap, &submap_slices[id]);
}
}
CHECK(reader.eof());
LOG(INFO) << "Generating combined map image from submap slices.";
auto result =
::cartographer::io::PaintSubmapSlices(submap_slices, resolution);
//最后生成的图片,可以直接看的就是pgm
::cartographer::io::StreamFileWriter pgm_writer(map_filestem + ".pgm");
::cartographer::io::Image image(std::move(result.surface));
WritePgm(image, resolution, &pgm_writer);
const Eigen::Vector2d origin(
-result.origin.x() * resolution,
(result.origin.y() - image.height()) * resolution);
//yaml打不开。是种类似xml格式的文件
::cartographer::io::StreamFileWriter yaml_writer(map_filestem + ".yaml");
WriteYaml(resolution, origin, pgm_writer.GetFilename(), &yaml_writer);
}
} // namespace
} // namespace cartographer_ros
int main(int argc, char** argv) {
FLAGS_alsologtostderr = true;
google::InitGoogleLogging(argv[0]);
google::ParseCommandLineFlags(&argc, &argv, true);
CHECK(!FLAGS_pbstream_filename.empty()) << "-pbstream_filename is missing.";
CHECK(!FLAGS_map_filestem.empty()) << "-map_filestem is missing.";
::cartographer_ros::Run(FLAGS_pbstream_filename, FLAGS_map_filestem,
FLAGS_resolution);
}
cartographer_ros中全局图生成的源码分析
最新推荐文章于 2024-04-27 09:32:15 发布