diff --git a/cartographer_ros/cartographer_ros/assets_writer.cc b/cartographer_ros/cartographer_ros/assets_writer.cc index 91ecb23..76249a5 100644 --- a/cartographer_ros/cartographer_ros/assets_writer.cc +++ b/cartographer_ros/cartographer_ros/assets_writer.cc @@ -17,6 +17,7 @@ #include "cartographer_ros/assets_writer.h" #include "cartographer/common/make_unique.h" +#include "cartographer/common/time.h" #include "cartographer/io/file_writer.h" #include "cartographer/io/null_points_processor.h" #include "cartographer/io/ply_writing_points_processor.h" @@ -54,19 +55,16 @@ void Write3DAssets(const std::vector<::cartographer::mapping::TrajectoryNode>& carto::io::NullPointsProcessor null_points_processor; carto::io::XRayPointsProcessor xy_xray_points_processor( - voxel_size, - carto::transform::Rigid3f::Rotation( - Eigen::AngleAxisf(-M_PI / 2.f, Eigen::Vector3f::UnitY())), + voxel_size, carto::transform::Rigid3f::Rotation( + Eigen::AngleAxisf(-M_PI / 2.f, Eigen::Vector3f::UnitY())), {}, stem + "_xray_xy", file_writer_factory, &null_points_processor); carto::io::XRayPointsProcessor yz_xray_points_processor( - voxel_size, - carto::transform::Rigid3f::Rotation( - Eigen::AngleAxisf(M_PI, Eigen::Vector3f::UnitZ())), + voxel_size, carto::transform::Rigid3f::Rotation( + Eigen::AngleAxisf(M_PI, Eigen::Vector3f::UnitZ())), {}, stem + "_xray_yz", file_writer_factory, &xy_xray_points_processor); carto::io::XRayPointsProcessor xz_xray_points_processor( - voxel_size, - carto::transform::Rigid3f::Rotation( - Eigen::AngleAxisf(-M_PI / 2.f, Eigen::Vector3f::UnitZ())), + voxel_size, carto::transform::Rigid3f::Rotation( + Eigen::AngleAxisf(-M_PI / 2.f, Eigen::Vector3f::UnitZ())), {}, stem + "_xray_xz", file_writer_factory, &yz_xray_points_processor); carto::io::PlyWritingPointsProcessor ply_writing_points_processor( file_writer_factory(stem + ".ply"), &xz_xray_points_processor); @@ -95,11 +93,18 @@ void Write3DAssets(const std::vector<::cartographer::mapping::TrajectoryNode>& void WriteAssets(const std::vector<::cartographer::mapping::TrajectoryNode>& trajectory_nodes, const NodeOptions& options, const std::string& stem) { + carto::mapping::proto::Trajectory trajectory; + for (const auto& node : trajectory_nodes) { + const auto& data = *node.constant_data; + auto* node_proto = trajectory.add_node(); + node_proto->set_timestamp(carto::common::ToUniversal(data.time)); + *node_proto->mutable_pose() = + carto::transform::ToProto(node.pose * data.tracking_to_pose); + } + // Write the trajectory. std::ofstream proto_file(stem + ".pb", std::ios_base::out | std::ios_base::binary); - const carto::mapping::proto::Trajectory trajectory = - carto::mapping::ToProto(trajectory_nodes); CHECK(trajectory.SerializeToOstream(&proto_file)) << "Could not serialize trajectory."; proto_file.close();