12345qiupeng
/
cartographer_ros
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Correct for googlecartographer/cartographer#227. (#303) 

Correct for googlecartographer/cartographer#227.
master
Mac Mason 2017-04-19 10:24:13 -04:00 committed by Wolfgang Hess
parent 319be1ad8c
commit 1babc6982a
1 changed files with 16 additions and 11 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

27
cartographer_ros/cartographer_ros/assets_writer.cc
View File

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