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

Follow Cartographer API. (#250)

master
Holger Rapp 2017-01-17 13:54:33 +01:00 committed by GitHub
parent e6e3730dde
commit 0783398634
2 changed files with 16 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

13
cartographer_ros/cartographer_ros/assets_writer.cc
View File

@ -17,6 +17,7 @@
#include "cartographer_ros/assets_writer.h" #include "cartographer_ros/assets_writer.h"
#include "cartographer/common/make_unique.h" #include "cartographer/common/make_unique.h"
#include "cartographer/io/file_writer.h"
#include "cartographer/io/null_points_processor.h" #include "cartographer/io/null_points_processor.h"
#include "cartographer/io/ply_writing_points_processor.h" #include "cartographer/io/ply_writing_points_processor.h"
#include "cartographer/io/points_processor.h" #include "cartographer/io/points_processor.h"
@ -47,21 +48,25 @@ void Write2DAssets(const std::vector<::cartographer::mapping::TrajectoryNode>&
void Write3DAssets(const std::vector<::cartographer::mapping::TrajectoryNode>& void Write3DAssets(const std::vector<::cartographer::mapping::TrajectoryNode>&
trajectory_nodes, trajectory_nodes,
const double voxel_size, const std::string& stem) { const double voxel_size, const std::string& stem) {
const auto file_writer_factory = [](const string& filename) {
return carto::common::make_unique<carto::io::StreamFileWriter>(filename);
};
carto::io::NullPointsProcessor null_points_processor; carto::io::NullPointsProcessor null_points_processor;
carto::io::XRayPointsProcessor xy_xray_points_processor( carto::io::XRayPointsProcessor xy_xray_points_processor(
voxel_size, carto::transform::Rigid3f::Rotation( voxel_size, carto::transform::Rigid3f::Rotation(
Eigen::AngleAxisf(-M_PI / 2.f, Eigen::Vector3f::UnitY())), Eigen::AngleAxisf(-M_PI / 2.f, Eigen::Vector3f::UnitY())),
{}, stem + "_xray_xy", &null_points_processor); {}, stem + "_xray_xy", file_writer_factory, &null_points_processor);
carto::io::XRayPointsProcessor yz_xray_points_processor( carto::io::XRayPointsProcessor yz_xray_points_processor(
voxel_size, carto::transform::Rigid3f::Rotation( voxel_size, carto::transform::Rigid3f::Rotation(
Eigen::AngleAxisf(M_PI, Eigen::Vector3f::UnitZ())), Eigen::AngleAxisf(M_PI, Eigen::Vector3f::UnitZ())),
{}, stem + "_xray_yz", &xy_xray_points_processor); {}, stem + "_xray_yz", file_writer_factory, &xy_xray_points_processor);
carto::io::XRayPointsProcessor xz_xray_points_processor( carto::io::XRayPointsProcessor xz_xray_points_processor(
voxel_size, carto::transform::Rigid3f::Rotation( voxel_size, carto::transform::Rigid3f::Rotation(
Eigen::AngleAxisf(-M_PI / 2.f, Eigen::Vector3f::UnitZ())), Eigen::AngleAxisf(-M_PI / 2.f, Eigen::Vector3f::UnitZ())),
{}, stem + "_xray_xz", &yz_xray_points_processor); {}, stem + "_xray_xz", file_writer_factory, &yz_xray_points_processor);
carto::io::PlyWritingPointsProcessor ply_writing_points_processor( carto::io::PlyWritingPointsProcessor ply_writing_points_processor(
stem + ".ply", &xz_xray_points_processor); file_writer_factory(stem + ".ply"), &xz_xray_points_processor);
for (const auto& node : trajectory_nodes) { for (const auto& node : trajectory_nodes) {
const carto::sensor::LaserFan laser_fan = carto::sensor::TransformLaserFan( const carto::sensor::LaserFan laser_fan = carto::sensor::TransformLaserFan(

8
cartographer_ros/cartographer_ros/assets_writer_main.cc
View File

@ -23,6 +23,7 @@
#include "cartographer/common/configuration_file_resolver.h" #include "cartographer/common/configuration_file_resolver.h"
#include "cartographer/common/make_unique.h" #include "cartographer/common/make_unique.h"
#include "cartographer/common/math.h" #include "cartographer/common/math.h"
#include "cartographer/io/file_writer.h"
#include "cartographer/io/points_processor.h" #include "cartographer/io/points_processor.h"
#include "cartographer/io/points_processor_pipeline_builder.h" #include "cartographer/io/points_processor_pipeline_builder.h"
#include "cartographer/sensor/laser.h" #include "cartographer/sensor/laser.h"
@ -156,8 +157,13 @@ void Run(const string& trajectory_filename, const string& bag_filename,
carto::mapping::proto::Trajectory trajectory_proto; carto::mapping::proto::Trajectory trajectory_proto;
CHECK(trajectory_proto.ParseFromIstream(&stream)); CHECK(trajectory_proto.ParseFromIstream(&stream));
const auto file_writer_factory = [](const string& filename) {
return carto::common::make_unique<carto::io::StreamFileWriter>(filename);
};
carto::io::PointsProcessorPipelineBuilder builder; carto::io::PointsProcessorPipelineBuilder builder;
carto::io::RegisterBuiltInPointsProcessors(trajectory_proto, &builder); carto::io::RegisterBuiltInPointsProcessors(trajectory_proto, file_writer_factory,
&builder);
std::vector<std::unique_ptr<carto::io::PointsProcessor>> pipeline = std::vector<std::unique_ptr<carto::io::PointsProcessor>> pipeline =
builder.CreatePipeline( builder.CreatePipeline(
lua_parameter_dictionary.GetDictionary("pipeline").get()); lua_parameter_dictionary.GetDictionary("pipeline").get());