Rename options for consistency. (#686)

"scan" is better named "node" if it refers to a node in global SLAM.

"penalty factor" is renamed "weight" for consistency with other similar options.

"scans_per_accumulation" is renamed "num_accumulated_range_data" to match the code and since the accumulated data is not called "scan".
master
Wolfgang Hess 2017-11-17 10:52:22 +01:00 committed by Wally B. Feed
parent 5ee830e8cc
commit 055728af93
26 changed files with 75 additions and 78 deletions

View File

@ -324,7 +324,7 @@ class MapById {
bool empty() const { return begin() == end(); } bool empty() const { return begin() == end(); }
// Returns an iterator to the the first element in the container belonging to // Returns an iterator to the first element in the container belonging to
// trajectory 'trajectory_id' whose time is not considered to go before // trajectory 'trajectory_id' whose time is not considered to go before
// 'time', or EndOfTrajectory(trajectory_id) if all keys are considered to go // 'time', or EndOfTrajectory(trajectory_id) if all keys are considered to go
// before 'time'. // before 'time'.

View File

@ -71,8 +71,8 @@ std::vector<PoseGraph::Constraint> FromProto(
proto::PoseGraphOptions CreatePoseGraphOptions( proto::PoseGraphOptions CreatePoseGraphOptions(
common::LuaParameterDictionary* const parameter_dictionary) { common::LuaParameterDictionary* const parameter_dictionary) {
proto::PoseGraphOptions options; proto::PoseGraphOptions options;
options.set_optimize_every_n_scans( options.set_optimize_every_n_nodes(
parameter_dictionary->GetInt("optimize_every_n_scans")); parameter_dictionary->GetInt("optimize_every_n_nodes"));
*options.mutable_constraint_builder_options() = *options.mutable_constraint_builder_options() =
pose_graph::CreateConstraintBuilderOptions( pose_graph::CreateConstraintBuilderOptions(
parameter_dictionary->GetDictionary("constraint_builder").get()); parameter_dictionary->GetDictionary("constraint_builder").get());

View File

@ -30,12 +30,10 @@ proto::OptimizationProblemOptions CreateOptimizationProblemOptions(
parameter_dictionary->GetDouble("acceleration_weight")); parameter_dictionary->GetDouble("acceleration_weight"));
options.set_rotation_weight( options.set_rotation_weight(
parameter_dictionary->GetDouble("rotation_weight")); parameter_dictionary->GetDouble("rotation_weight"));
options.set_consecutive_scan_translation_penalty_factor( options.set_consecutive_node_translation_weight(
parameter_dictionary->GetDouble( parameter_dictionary->GetDouble("consecutive_node_translation_weight"));
"consecutive_scan_translation_penalty_factor")); options.set_consecutive_node_rotation_weight(
options.set_consecutive_scan_rotation_penalty_factor( parameter_dictionary->GetDouble("consecutive_node_rotation_weight"));
parameter_dictionary->GetDouble(
"consecutive_scan_rotation_penalty_factor"));
options.set_fixed_frame_pose_translation_weight( options.set_fixed_frame_pose_translation_weight(
parameter_dictionary->GetDouble("fixed_frame_pose_translation_weight")); parameter_dictionary->GetDouble("fixed_frame_pose_translation_weight"));
options.set_fixed_frame_pose_rotation_weight( options.set_fixed_frame_pose_rotation_weight(

View File

@ -29,11 +29,11 @@ message OptimizationProblemOptions {
// Scaling parameter for the IMU rotation term. // Scaling parameter for the IMU rotation term.
double rotation_weight = 9; double rotation_weight = 9;
// Penalty factors for translation changes to the relative pose between consecutive scans. // Scaling parameter for translation between consecutive nodes.
double consecutive_scan_translation_penalty_factor = 2; double consecutive_node_translation_weight = 2;
// Penalty factors for rotation changes to the relative pose between consecutive scans. // Scaling parameter for rotation between consecutive nodes.
double consecutive_scan_rotation_penalty_factor = 3; double consecutive_node_rotation_weight = 3;
// Scaling parameter for the FixedFramePose translation. // Scaling parameter for the FixedFramePose translation.
double fixed_frame_pose_translation_weight = 11; double fixed_frame_pose_translation_weight = 11;

View File

@ -22,7 +22,7 @@ import "cartographer/mapping/pose_graph/proto/optimization_problem_options.proto
message PoseGraphOptions { message PoseGraphOptions {
// Online loop closure: If positive, will run the loop closure while the map // Online loop closure: If positive, will run the loop closure while the map
// is built. // is built.
int32 optimize_every_n_scans = 1; int32 optimize_every_n_nodes = 1;
// Options for the constraint builder. // Options for the constraint builder.
mapping.pose_graph.proto.ConstraintBuilderOptions mapping.pose_graph.proto.ConstraintBuilderOptions
@ -44,7 +44,7 @@ message PoseGraphOptions {
// optimization. // optimization.
int32 max_num_final_iterations = 6; int32 max_num_final_iterations = 6;
// Rate at which we sample a single trajectory's scans for global // Rate at which we sample a single trajectory's nodes for global
// localization. // localization.
double global_sampling_ratio = 5; double global_sampling_ratio = 5;

View File

@ -125,7 +125,7 @@ LocalTrajectoryBuilder::AddRangeData(const common::Time time,
} }
++num_accumulated_; ++num_accumulated_;
if (num_accumulated_ >= options_.scans_per_accumulation()) { if (num_accumulated_ >= options_.num_accumulated_range_data()) {
num_accumulated_ = 0; num_accumulated_ = 0;
const transform::Rigid3d gravity_alignment = transform::Rigid3d::Rotation( const transform::Rigid3d gravity_alignment = transform::Rigid3d::Rotation(
extrapolator_->EstimateGravityOrientation(time)); extrapolator_->EstimateGravityOrientation(time));

View File

@ -34,8 +34,8 @@ proto::LocalTrajectoryBuilderOptions CreateLocalTrajectoryBuilderOptions(
options.set_max_z(parameter_dictionary->GetDouble("max_z")); options.set_max_z(parameter_dictionary->GetDouble("max_z"));
options.set_missing_data_ray_length( options.set_missing_data_ray_length(
parameter_dictionary->GetDouble("missing_data_ray_length")); parameter_dictionary->GetDouble("missing_data_ray_length"));
options.set_scans_per_accumulation( options.set_num_accumulated_range_data(
parameter_dictionary->GetInt("scans_per_accumulation")); parameter_dictionary->GetInt("num_accumulated_range_data"));
options.set_voxel_filter_size( options.set_voxel_filter_size(
parameter_dictionary->GetDouble("voxel_filter_size")); parameter_dictionary->GetDouble("voxel_filter_size"));
options.set_use_online_correlative_scan_matching( options.set_use_online_correlative_scan_matching(

View File

@ -269,8 +269,8 @@ void PoseGraph::ComputeConstraintsForNode(
} }
constraint_builder_.NotifyEndOfNode(); constraint_builder_.NotifyEndOfNode();
++num_nodes_since_last_loop_closure_; ++num_nodes_since_last_loop_closure_;
if (options_.optimize_every_n_scans() > 0 && if (options_.optimize_every_n_nodes() > 0 &&
num_nodes_since_last_loop_closure_ > options_.optimize_every_n_scans()) { num_nodes_since_last_loop_closure_ > options_.optimize_every_n_nodes()) {
CHECK(!run_loop_closure_); CHECK(!run_loop_closure_);
run_loop_closure_ = true; run_loop_closure_ = true;
// If there is a 'work_queue_' already, some other thread will take care. // If there is a 'work_queue_' already, some other thread will take care.

View File

@ -189,9 +189,8 @@ void OptimizationProblem::Solve(const std::vector<Constraint>& constraints,
problem.AddResidualBlock( problem.AddResidualBlock(
new ceres::AutoDiffCostFunction<SpaCostFunction, 3, 3, 3>( new ceres::AutoDiffCostFunction<SpaCostFunction, 3, 3, 3>(
new SpaCostFunction(Constraint::Pose{ new SpaCostFunction(Constraint::Pose{
relative_pose, relative_pose, options_.consecutive_node_translation_weight(),
options_.consecutive_scan_translation_penalty_factor(), options_.consecutive_node_rotation_weight()})),
options_.consecutive_scan_rotation_penalty_factor()})),
nullptr /* loss function */, C_nodes.at(first_node_id).data(), nullptr /* loss function */, C_nodes.at(first_node_id).data(),
C_nodes.at(second_node_id).data()); C_nodes.at(second_node_id).data());
} }

View File

@ -65,7 +65,7 @@ class PoseGraphTest : public ::testing::Test {
{ {
auto parameter_dictionary = common::MakeDictionary(R"text( auto parameter_dictionary = common::MakeDictionary(R"text(
return { return {
optimize_every_n_scans = 1000, optimize_every_n_nodes = 1000,
constraint_builder = { constraint_builder = {
sampling_ratio = 1., sampling_ratio = 1.,
max_constraint_distance = 6., max_constraint_distance = 6.,
@ -116,8 +116,8 @@ class PoseGraphTest : public ::testing::Test {
acceleration_weight = 1., acceleration_weight = 1.,
rotation_weight = 1e2, rotation_weight = 1e2,
huber_scale = 1., huber_scale = 1.,
consecutive_scan_translation_penalty_factor = 0., consecutive_node_translation_weight = 0.,
consecutive_scan_rotation_penalty_factor = 0., consecutive_node_rotation_weight = 0.,
fixed_frame_pose_translation_weight = 1e1, fixed_frame_pose_translation_weight = 1e1,
fixed_frame_pose_rotation_weight = 1e2, fixed_frame_pose_rotation_weight = 1e2,
log_solver_summary = true, log_solver_summary = true,

View File

@ -32,9 +32,9 @@ message LocalTrajectoryBuilderOptions {
// Points beyond 'max_range' will be inserted with this length as empty space. // Points beyond 'max_range' will be inserted with this length as empty space.
float missing_data_ray_length = 16; float missing_data_ray_length = 16;
// Number of scans to accumulate into one unwarped, combined scan to use for // Number of range data to accumulate into one unwarped, combined range data
// scan matching. // to use for scan matching.
int32 scans_per_accumulation = 19; int32 num_accumulated_range_data = 19;
// Voxel filter that gets applied to the range data immediately after // Voxel filter that gets applied to the range data immediately after
// cropping. // cropping.

View File

@ -22,9 +22,9 @@ message SubmapsOptions {
// Resolution of the map in meters. // Resolution of the map in meters.
double resolution = 1; double resolution = 1;
// Number of scans before adding a new submap. Each submap will get twice the // Number of range data before adding a new submap. Each submap will get twice
// number of scans inserted: First for initialization without being matched // the number of range data inserted: First for initialization without being
// against, then while being matched. // matched against, then while being matched.
int32 num_range_data = 3; int32 num_range_data = 3;
RangeDataInserterOptions range_data_inserter_options = 5; RangeDataInserterOptions range_data_inserter_options = 5;

View File

@ -98,7 +98,7 @@ LocalTrajectoryBuilder::AddRangeData(const common::Time time,
} }
++num_accumulated_; ++num_accumulated_;
if (num_accumulated_ >= options_.scans_per_accumulation()) { if (num_accumulated_ >= options_.num_accumulated_range_data()) {
num_accumulated_ = 0; num_accumulated_ = 0;
const sensor::RangeData filtered_range_data = { const sensor::RangeData filtered_range_data = {
accumulated_range_data_.origin, accumulated_range_data_.origin,

View File

@ -31,8 +31,8 @@ proto::LocalTrajectoryBuilderOptions CreateLocalTrajectoryBuilderOptions(
proto::LocalTrajectoryBuilderOptions options; proto::LocalTrajectoryBuilderOptions options;
options.set_min_range(parameter_dictionary->GetDouble("min_range")); options.set_min_range(parameter_dictionary->GetDouble("min_range"));
options.set_max_range(parameter_dictionary->GetDouble("max_range")); options.set_max_range(parameter_dictionary->GetDouble("max_range"));
options.set_scans_per_accumulation( options.set_num_accumulated_range_data(
parameter_dictionary->GetInt("scans_per_accumulation")); parameter_dictionary->GetInt("num_accumulated_range_data"));
options.set_voxel_filter_size( options.set_voxel_filter_size(
parameter_dictionary->GetDouble("voxel_filter_size")); parameter_dictionary->GetDouble("voxel_filter_size"));
*options.mutable_high_resolution_adaptive_voxel_filter_options() = *options.mutable_high_resolution_adaptive_voxel_filter_options() =

View File

@ -49,7 +49,7 @@ class LocalTrajectoryBuilderTest : public ::testing::Test {
return { return {
min_range = 0.5, min_range = 0.5,
max_range = 50., max_range = 50.,
scans_per_accumulation = 1, num_accumulated_range_data = 1,
voxel_filter_size = 0.05, voxel_filter_size = 0.05,
high_resolution_adaptive_voxel_filter = { high_resolution_adaptive_voxel_filter = {

View File

@ -286,8 +286,8 @@ void PoseGraph::ComputeConstraintsForNode(
} }
constraint_builder_.NotifyEndOfNode(); constraint_builder_.NotifyEndOfNode();
++num_nodes_since_last_loop_closure_; ++num_nodes_since_last_loop_closure_;
if (options_.optimize_every_n_scans() > 0 && if (options_.optimize_every_n_nodes() > 0 &&
num_nodes_since_last_loop_closure_ > options_.optimize_every_n_scans()) { num_nodes_since_last_loop_closure_ > options_.optimize_every_n_nodes()) {
CHECK(!run_loop_closure_); CHECK(!run_loop_closure_);
run_loop_closure_ = true; run_loop_closure_ = true;
// If there is a 'work_queue_' already, some other thread will take care. // If there is a 'work_queue_' already, some other thread will take care.

View File

@ -354,8 +354,8 @@ void OptimizationProblem::Solve(const std::vector<Constraint>& constraints,
new ceres::AutoDiffCostFunction<SpaCostFunction, 6, 4, 3, 4, 3>( new ceres::AutoDiffCostFunction<SpaCostFunction, 6, 4, 3, 4, 3>(
new SpaCostFunction(Constraint::Pose{ new SpaCostFunction(Constraint::Pose{
relative_pose, relative_pose,
options_.consecutive_scan_translation_penalty_factor(), options_.consecutive_node_translation_weight(),
options_.consecutive_scan_rotation_penalty_factor()})), options_.consecutive_node_rotation_weight()})),
nullptr /* loss function */, C_nodes.at(first_node_id).rotation(), nullptr /* loss function */, C_nodes.at(first_node_id).rotation(),
C_nodes.at(first_node_id).translation(), C_nodes.at(first_node_id).translation(),
C_nodes.at(second_node_id).rotation(), C_nodes.at(second_node_id).rotation(),

View File

@ -43,8 +43,8 @@ class OptimizationProblemTest : public ::testing::Test {
acceleration_weight = 1e-4, acceleration_weight = 1e-4,
rotation_weight = 1e-2, rotation_weight = 1e-2,
huber_scale = 1., huber_scale = 1.,
consecutive_scan_translation_penalty_factor = 1e-2, consecutive_node_translation_weight = 1e-2,
consecutive_scan_rotation_penalty_factor = 1e-2, consecutive_node_rotation_weight = 1e-2,
fixed_frame_pose_translation_weight = 1e1, fixed_frame_pose_translation_weight = 1e1,
fixed_frame_pose_rotation_weight = 1e2, fixed_frame_pose_rotation_weight = 1e2,
log_solver_summary = true, log_solver_summary = true,

View File

@ -28,9 +28,9 @@ message LocalTrajectoryBuilderOptions {
float min_range = 1; float min_range = 1;
float max_range = 2; float max_range = 2;
// Number of scans to accumulate into one unwarped, combined scan to use for // Number of range data to accumulate into one unwarped, combined range data
// scan matching. // to use for scan matching.
int32 scans_per_accumulation = 3; int32 num_accumulated_range_data = 3;
// Voxel filter that gets applied to the range data immediately after // Voxel filter that gets applied to the range data immediately after
// cropping. // cropping.

View File

@ -17,12 +17,12 @@ syntax = "proto3";
package cartographer.mapping_3d.proto; package cartographer.mapping_3d.proto;
message MotionFilterOptions { message MotionFilterOptions {
// Threshold above which a new scan is inserted based on time. // Threshold above which range data is inserted based on time.
double max_time_seconds = 1; double max_time_seconds = 1;
// Threshold above which a new scan is inserted based on linear motion. // Threshold above which range data is inserted based on linear motion.
double max_distance_meters = 2; double max_distance_meters = 2;
// Threshold above which a new scan is inserted based on rotational motion. // Threshold above which range data is inserted based on rotational motion.
double max_angle_radians = 3; double max_angle_radians = 3;
} }

View File

@ -31,9 +31,9 @@ message SubmapsOptions {
// local SLAM only. // local SLAM only.
double low_resolution = 5; double low_resolution = 5;
// Number of scans before adding a new submap. Each submap will get twice the // Number of range data before adding a new submap. Each submap will get twice
// number of scans inserted: First for initialization without being matched // the number of range data inserted: First for initialization without being
// against, then while being matched. // matched against, then while being matched.
int32 num_range_data = 2; int32 num_range_data = 2;
RangeDataInserterOptions range_data_inserter_options = 3; RangeDataInserterOptions range_data_inserter_options = 3;

View File

@ -194,7 +194,7 @@ class MapByTime {
EndOfTrajectory(trajectory_id)); EndOfTrajectory(trajectory_id));
} }
// Returns an iterator to the the first element in the container belonging to // Returns an iterator to the first element in the container belonging to
// trajectory 'trajectory_id' whose time is not considered to go before // trajectory 'trajectory_id' whose time is not considered to go before
// 'time', or EndOfTrajectory(trajectory_id) if all keys are considered to go // 'time', or EndOfTrajectory(trajectory_id) if all keys are considered to go
// before 'time'. 'trajectory_id' must refer to an existing trajectory. // before 'time'. 'trajectory_id' must refer to an existing trajectory.

View File

@ -13,7 +13,7 @@
-- limitations under the License. -- limitations under the License.
POSE_GRAPH = { POSE_GRAPH = {
optimize_every_n_scans = 90, optimize_every_n_nodes = 90,
constraint_builder = { constraint_builder = {
sampling_ratio = 0.3, sampling_ratio = 0.3,
max_constraint_distance = 15., max_constraint_distance = 15.,
@ -65,8 +65,8 @@ POSE_GRAPH = {
huber_scale = 1e1, huber_scale = 1e1,
acceleration_weight = 1e3, acceleration_weight = 1e3,
rotation_weight = 3e5, rotation_weight = 3e5,
consecutive_scan_translation_penalty_factor = 1e5, consecutive_node_translation_weight = 1e5,
consecutive_scan_rotation_penalty_factor = 1e5, consecutive_node_rotation_weight = 1e5,
fixed_frame_pose_translation_weight = 1e1, fixed_frame_pose_translation_weight = 1e1,
fixed_frame_pose_rotation_weight = 1e2, fixed_frame_pose_rotation_weight = 1e2,
log_solver_summary = false, log_solver_summary = false,

View File

@ -19,7 +19,7 @@ TRAJECTORY_BUILDER_2D = {
min_z = -0.8, min_z = -0.8,
max_z = 2., max_z = 2.,
missing_data_ray_length = 5., missing_data_ray_length = 5.,
scans_per_accumulation = 1, num_accumulated_range_data = 1,
voxel_filter_size = 0.025, voxel_filter_size = 0.025,
adaptive_voxel_filter = { adaptive_voxel_filter = {

View File

@ -17,7 +17,7 @@ MAX_3D_RANGE = 60.
TRAJECTORY_BUILDER_3D = { TRAJECTORY_BUILDER_3D = {
min_range = 1., min_range = 1.,
max_range = MAX_3D_RANGE, max_range = MAX_3D_RANGE,
scans_per_accumulation = 1, num_accumulated_range_data = 1,
voxel_filter_size = 0.15, voxel_filter_size = 0.15,
high_resolution_adaptive_voxel_filter = { high_resolution_adaptive_voxel_filter = {

View File

@ -86,11 +86,11 @@ double acceleration_weight
double rotation_weight double rotation_weight
Scaling parameter for the IMU rotation term. Scaling parameter for the IMU rotation term.
double consecutive_scan_translation_penalty_factor double consecutive_node_translation_weight
Penalty factors for translation changes to the relative pose between consecutive scans. Scaling parameter for translation between consecutive nodes.
double consecutive_scan_rotation_penalty_factor double consecutive_node_rotation_weight
Penalty factors for rotation changes to the relative pose between consecutive scans. Scaling parameter for rotation between consecutive nodes.
double fixed_frame_pose_translation_weight double fixed_frame_pose_translation_weight
Scaling parameter for the FixedFramePose translation. Scaling parameter for the FixedFramePose translation.
@ -124,7 +124,7 @@ cartographer.mapping.proto.PoseGraphOptions pose_graph_options
cartographer.mapping.proto.PoseGraphOptions cartographer.mapping.proto.PoseGraphOptions
=========================================== ===========================================
int32 optimize_every_n_scans int32 optimize_every_n_nodes
Online loop closure: If positive, will run the loop closure while the map Online loop closure: If positive, will run the loop closure while the map
is built. is built.
@ -147,7 +147,7 @@ int32 max_num_final_iterations
optimization. optimization.
double global_sampling_ratio double global_sampling_ratio
Rate at which we sample a single trajectory's scans for global Rate at which we sample a single trajectory's nodes for global
localization. localization.
bool log_residual_histograms bool log_residual_histograms
@ -190,9 +190,9 @@ float max_z
float missing_data_ray_length float missing_data_ray_length
Points beyond 'max_range' will be inserted with this length as empty space. Points beyond 'max_range' will be inserted with this length as empty space.
int32 scans_per_accumulation int32 num_accumulated_range_data
Number of scans to accumulate into one unwarped, combined scan to use for Number of range data to accumulate into one unwarped, combined range data
scan matching. to use for scan matching.
float voxel_filter_size float voxel_filter_size
Voxel filter that gets applied to the range data immediately after Voxel filter that gets applied to the range data immediately after
@ -256,9 +256,9 @@ double resolution
Resolution of the map in meters. Resolution of the map in meters.
int32 num_range_data int32 num_range_data
Number of scans before adding a new submap. Each submap will get twice the Number of range data before adding a new submap. Each submap will get twice
number of scans inserted: First for initialization without being matched the number of range data inserted: First for initialization without being
against, then while being matched. matched against, then while being matched.
cartographer.mapping_2d.proto.RangeDataInserterOptions range_data_inserter_options cartographer.mapping_2d.proto.RangeDataInserterOptions range_data_inserter_options
Not yet documented. Not yet documented.
@ -323,9 +323,9 @@ float min_range
float max_range float max_range
Not yet documented. Not yet documented.
int32 scans_per_accumulation int32 num_accumulated_range_data
Number of scans to accumulate into one unwarped, combined scan to use for Number of range data to accumulate into one unwarped, combined range data
scan matching. to use for scan matching.
float voxel_filter_size float voxel_filter_size
Voxel filter that gets applied to the range data immediately after Voxel filter that gets applied to the range data immediately after
@ -369,13 +369,13 @@ cartographer.mapping_3d.proto.MotionFilterOptions
================================================= =================================================
double max_time_seconds double max_time_seconds
Threshold above which a new scan is inserted based on time. Threshold above which range data is inserted based on time.
double max_distance_meters double max_distance_meters
Threshold above which a new scan is inserted based on linear motion. Threshold above which range data is inserted based on linear motion.
double max_angle_radians double max_angle_radians
Threshold above which a new scan is inserted based on rotational motion. Threshold above which range data is inserted based on rotational motion.
cartographer.mapping_3d.proto.RangeDataInserterOptions cartographer.mapping_3d.proto.RangeDataInserterOptions
@ -410,9 +410,9 @@ double low_resolution
local SLAM only. local SLAM only.
int32 num_range_data int32 num_range_data
Number of scans before adding a new submap. Each submap will get twice the Number of range data before adding a new submap. Each submap will get twice
number of scans inserted: First for initialization without being matched the number of range data inserted: First for initialization without being
against, then while being matched. matched against, then while being matched.
cartographer.mapping_3d.proto.RangeDataInserterOptions range_data_inserter_options cartographer.mapping_3d.proto.RangeDataInserterOptions range_data_inserter_options
Not yet documented. Not yet documented.