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

Adapt to new Cartographer API. (#169) 

And write by-floor X-Rays in the asset writer.
master
Holger Rapp 2016-11-17 02:17:52 -08:00 committed by GitHub
parent 2ec6001f96
commit 5046bc02cc
3 changed files with 48 additions and 18 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
cartographer_ros/cartographer_ros/assets_writer.cc
View File

@ -51,15 +51,15 @@ void Write3DAssets(const std::vector<::cartographer::mapping::TrajectoryNode>&
carto::io::XRayPointsProcessor xy_xray_points_processor(
voxel_size, carto::transform::Rigid3f::Rotation(
Eigen::AngleAxisf(-M_PI / 2.f, Eigen::Vector3f::UnitY())),
stem + "_xray_xy.png", &null_points_processor);
{}, stem + "_xray_xy.png", &null_points_processor);
carto::io::XRayPointsProcessor yz_xray_points_processor(
voxel_size, carto::transform::Rigid3f::Rotation(
Eigen::AngleAxisf(M_PI, Eigen::Vector3f::UnitZ())),
stem + "_xray_yz.png", &xy_xray_points_processor);
{}, stem + "_xray_yz.png", &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())),
stem + "_xray_xz.png", &yz_xray_points_processor);
{}, stem + "_xray_xz.png", &yz_xray_points_processor);
carto::io::PlyWritingPointsProcessor ply_writing_points_processor(
stem + ".ply", &xz_xray_points_processor);

22
cartographer_ros/cartographer_ros/assets_writer_main.cc
View File

@ -56,15 +56,6 @@ namespace {
namespace carto = ::cartographer;
std::unique_ptr<carto::transform::TransformInterpolationBuffer> ReadTrajectory(
const std::string& trajectory_filename) {
std::ifstream stream(trajectory_filename.c_str());
carto::mapping::proto::Trajectory trajectory_proto;
CHECK(trajectory_proto.ParseFromIstream(&stream));
return carto::transform::TransformInterpolationBuffer::FromTrajectory(
trajectory_proto);
}
void ReadStaticTransformsFromUrdf(const string& urdf_filename,
::tf2_ros::Buffer* const buffer) {
urdf::Model model;
@ -99,9 +90,16 @@ void Run(const string& trajectory_filename, const string& bag_filename,
::tf2_ros::Buffer buffer;
ReadStaticTransformsFromUrdf(urdf_filename, &buffer);
auto transform_interpolation_buffer = ReadTrajectory(trajectory_filename);
std::ifstream stream(trajectory_filename.c_str());
carto::mapping::proto::Trajectory trajectory_proto;
CHECK(trajectory_proto.ParseFromIstream(&stream));
auto* builder = carto::io::PointsProcessorPipelineBuilder::instance();
auto transform_interpolation_buffer =
carto::transform::TransformInterpolationBuffer::FromTrajectory(
trajectory_proto);
carto::io::PointsProcessorPipelineBuilder builder;
carto::io::RegisterBuiltInPointsProcessors(trajectory_proto, &builder);
auto file_resolver =
carto::common::make_unique<carto::common::ConfigurationFileResolver>(
@ -114,7 +112,7 @@ void Run(const string& trajectory_filename, const string& bag_filename,
lua_parameter_dictionary.GetString("tracking_frame");
std::vector<std::unique_ptr<carto::io::PointsProcessor>> pipeline =
builder->CreatePipeline(
builder.CreatePipeline(
lua_parameter_dictionary.GetDictionary("pipeline").get());
do {

38
cartographer_ros/configuration_files/assets_writer.lua
View File

@ -1,4 +1,6 @@
VOXEL_SIZE = 5e-2
options = {
tracking_frame = "base_link",
pipeline = {
@ -27,7 +29,7 @@ options = {
{
action = "write_xray_image",
voxel_size = VOXEL_SIZE,
filename = "xray_yz.png",
filename = "xray_yz_all",
transform = {
translation = { 0., 0., 0. },
rotation = { 0. , 0., math.pi, },
@ -36,7 +38,7 @@ options = {
{
action = "write_xray_image",
voxel_size = VOXEL_SIZE,
filename = "xray_xy.png",
filename = "xray_xy_all",
transform = {
translation = { 0., 0., 0. },
rotation = { 0., -math.pi / 2., 0., },
@ -45,7 +47,37 @@ options = {
{
action = "write_xray_image",
voxel_size = VOXEL_SIZE,
filename = "xray_xz.png",
filename = "xray_xz_all",
transform = {
translation = { 0., 0., 0. },
rotation = { 0. , 0., -math.pi / 2, },
},
},
{
action = "write_xray_image",
voxel_size = VOXEL_SIZE,
separate_floors = true,
filename = "xray_yz_level_",
transform = {
translation = { 0., 0., 0. },
rotation = { 0. , 0., math.pi, },
},
},
{
action = "write_xray_image",
voxel_size = VOXEL_SIZE,
separate_floors = true,
filename = "xray_xy_level_",
transform = {
translation = { 0., 0., 0. },
rotation = { 0., -math.pi / 2., 0., },
},
},
{
action = "write_xray_image",
voxel_size = VOXEL_SIZE,
separate_floors = true,
filename = "xray_xz_level_",
transform = {
translation = { 0., 0., 0. },
rotation = { 0. , 0., -math.pi / 2, },