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

Fix trajectory start/stop bug and some cleanups. (#184)

master
Damon Kohler 2016-11-25 09:37:11 +01:00 committed by GitHub
parent 4f6f0dc684
commit e5b63e377a
5 changed files with 24 additions and 16 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", &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", &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", &yz_xray_points_processor);
carto::io::PlyWritingPointsProcessor ply_writing_points_processor(
stem + ".ply", &xz_xray_points_processor);

5
cartographer_ros/cartographer_ros/assets_writer_main.cc
View File

@ -44,7 +44,7 @@ DEFINE_string(configuration_basename, "",
"Basename, i.e. not containing any directory prefix, of the "
"configuration file.");
DEFINE_string(
urdf, "",
urdf_filename, "",
"URDF file that contains static links for your sensor configuration.");
DEFINE_string(bag_filename, "", "Bag to process.");
DEFINE_string(
@ -182,8 +182,9 @@ int main(int argc, char** argv) {
CHECK(!FLAGS_bag_filename.empty()) << "-bag_filename is missing.";
CHECK(!FLAGS_trajectory_filename.empty())
<< "-trajectory_filename is missing.";
CHECK(!FLAGS_urdf_filename.empty()) << "-urdf_filename is missing.";
::cartographer_ros::Run(FLAGS_trajectory_filename, FLAGS_bag_filename,
FLAGS_configuration_directory,
FLAGS_configuration_basename, FLAGS_urdf);
FLAGS_configuration_basename, FLAGS_urdf_filename);
}

23
cartographer_ros/cartographer_ros/node_main.cc
View File

@ -299,22 +299,29 @@ bool Node::HandleSubmapQuery(
bool Node::HandleFinishTrajectory(
::cartographer_ros_msgs::FinishTrajectory::Request& request,
::cartographer_ros_msgs::FinishTrajectory::Response& response) {
LOG(INFO) << "Finishing trajectory...";
carto::common::MutexLocker lock(&mutex_);
map_builder_.FinishTrajectory(trajectory_id_);
const int previous_trajectory_id = trajectory_id_;
trajectory_id_ =
map_builder_.AddTrajectoryBuilder(expected_sensor_ids_);
sensor_bridge_ = carto::common::make_unique<SensorBridge>(
&tf_bridge_, map_builder_.GetTrajectoryBuilder(trajectory_id_));
map_builder_.FinishTrajectory(previous_trajectory_id);
map_builder_.sparse_pose_graph()->RunFinalOptimization();
const auto trajectory_nodes =
map_builder_.sparse_pose_graph()->GetTrajectoryNodes();
if (trajectory_nodes.empty()) {
LOG(WARNING) << "Map is empty and will not be saved.";
return true;
}
LOG(WARNING) << "No data collected and no assets will be written.";
} else {
LOG(INFO) << "Writing assets...";
WriteAssets(trajectory_nodes, options_, request.stem);
}
// Start a new trajectory.
trajectory_id_ = map_builder_.AddTrajectoryBuilder(expected_sensor_ids_);
sensor_bridge_ = carto::common::make_unique<SensorBridge>(
&tf_bridge_, map_builder_.GetTrajectoryBuilder(trajectory_id_));
LOG(INFO) << "New trajectory started.";
return true;
}

1
cartographer_ros/configuration_files/assets_writer.lua
View File

@ -1,6 +1,5 @@
VOXEL_SIZE = 5e-2
options = {
tracking_frame = "base_link",
pipeline = {

5
cartographer_ros/configuration_files/taurob_tracker.lua
View File

@ -30,14 +30,15 @@ options = {
pose_publish_period_sec = 5e-3,
}
TRAJECTORY_BUILDER_3D.scans_per_accumulation = 90
TRAJECTORY_BUILDER_3D.scans_per_accumulation = 180
TRAJECTORY_BUILDER_3D.laser_min_range = 0.5
TRAJECTORY_BUILDER_3D.laser_max_range = 20.
TRAJECTORY_BUILDER_3D.submaps.num_laser_fans = 40.
MAP_BUILDER.use_trajectory_builder_3d = true
MAP_BUILDER.num_background_threads = 7
MAP_BUILDER.sparse_pose_graph.optimization_problem.huber_scale = 5e2
MAP_BUILDER.sparse_pose_graph.optimize_every_n_scans = 320
MAP_BUILDER.sparse_pose_graph.optimize_every_n_scans = 40
MAP_BUILDER.sparse_pose_graph.constraint_builder.sampling_ratio = 0.03
MAP_BUILDER.sparse_pose_graph.optimization_problem.ceres_solver_options.max_num_iterations = 10
-- Reuse the coarser 3D voxel filter to speed up the computation of loop closure