Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017 #include "cartographer/io/draw_trajectories.h"
00018
00019 #include "cartographer/io/image.h"
00020 #include "cartographer/transform/transform.h"
00021
00022 namespace cartographer {
00023 namespace io {
00024
00025 void DrawTrajectory(const mapping::proto::Trajectory& trajectory,
00026 const FloatColor& color,
00027 const PoseToPixelFunction& pose_to_pixel,
00028 cairo_surface_t* surface) {
00029 if (trajectory.node_size() == 0) {
00030 return;
00031 }
00032 constexpr double kTrajectoryWidth = 4.;
00033 constexpr double kTrajectoryEndMarkers = 6.;
00034 constexpr double kAlpha = 0.7;
00035
00036 auto cr = ::cartographer::io::MakeUniqueCairoPtr(cairo_create(surface));
00037
00038 cairo_set_source_rgba(cr.get(), color[0], color[1], color[2], kAlpha);
00039 cairo_set_line_width(cr.get(), kTrajectoryWidth);
00040
00041 for (const auto& node : trajectory.node()) {
00042 const Eigen::Array2i pixel =
00043 pose_to_pixel(transform::ToRigid3(node.pose()));
00044 cairo_line_to(cr.get(), pixel.x(), pixel.y());
00045 }
00046 cairo_stroke(cr.get());
00047
00048
00049 {
00050 const Eigen::Array2i pixel =
00051 pose_to_pixel(transform::ToRigid3(trajectory.node(0).pose()));
00052 cairo_set_source_rgba(cr.get(), 0., 1., 0., kAlpha);
00053 cairo_arc(cr.get(), pixel.x(), pixel.y(), kTrajectoryEndMarkers, 0,
00054 2 * M_PI);
00055 cairo_fill(cr.get());
00056 }
00057 {
00058 const Eigen::Array2i pixel = pose_to_pixel(transform::ToRigid3(
00059 trajectory.node(trajectory.node_size() - 1).pose()));
00060 cairo_set_source_rgba(cr.get(), 1., 0., 0., kAlpha);
00061 cairo_arc(cr.get(), pixel.x(), pixel.y(), kTrajectoryEndMarkers, 0,
00062 2 * M_PI);
00063 cairo_fill(cr.get());
00064 }
00065 cairo_surface_flush(surface);
00066 }
00067
00068 }
00069 }