diff --git a/cartographer/mapping/trajectory_builder.cc b/cartographer/mapping/trajectory_builder.cc index 2616b2f..0eee6ed 100644 --- a/cartographer/mapping/trajectory_builder.cc +++ b/cartographer/mapping/trajectory_builder.cc @@ -16,7 +16,7 @@ #include "cartographer/mapping/trajectory_builder.h" -#include "cartographer/mapping_2d/local_trajectory_builder.h" +#include "cartographer/mapping_2d/local_trajectory_builder_options.h" #include "cartographer/mapping_3d/local_trajectory_builder_options.h" namespace cartographer { diff --git a/cartographer/mapping/trajectory_connectivity_test.cc b/cartographer/mapping/trajectory_connectivity_test.cc index fe4dd62..4d6c6b0 100644 --- a/cartographer/mapping/trajectory_connectivity_test.cc +++ b/cartographer/mapping/trajectory_connectivity_test.cc @@ -32,8 +32,9 @@ TEST(TrajectoryConnectivityTest, TransitivelyConnected) { TrajectoryConnectivity trajectory_connectivity; // Make sure nothing's connected until we connect some things. - for (int trajectory_a; trajectory_a < kNumTrajectories; ++trajectory_a) { - for (int trajectory_b; trajectory_b < kNumTrajectories; ++trajectory_b) { + for (int trajectory_a = 0; trajectory_a < kNumTrajectories; ++trajectory_a) { + for (int trajectory_b = 0; trajectory_b < kNumTrajectories; + ++trajectory_b) { EXPECT_FALSE(trajectory_connectivity.TransitivelyConnected(trajectory_a, trajectory_b)); } diff --git a/cartographer/mapping_2d/local_trajectory_builder.cc b/cartographer/mapping_2d/local_trajectory_builder.cc index 4054489..a128b95 100644 --- a/cartographer/mapping_2d/local_trajectory_builder.cc +++ b/cartographer/mapping_2d/local_trajectory_builder.cc @@ -24,44 +24,6 @@ namespace cartographer { namespace mapping_2d { -proto::LocalTrajectoryBuilderOptions CreateLocalTrajectoryBuilderOptions( - common::LuaParameterDictionary* const parameter_dictionary) { - proto::LocalTrajectoryBuilderOptions options; - options.set_min_range(parameter_dictionary->GetDouble("min_range")); - options.set_max_range(parameter_dictionary->GetDouble("max_range")); - options.set_min_z(parameter_dictionary->GetDouble("min_z")); - options.set_max_z(parameter_dictionary->GetDouble("max_z")); - options.set_missing_data_ray_length( - parameter_dictionary->GetDouble("missing_data_ray_length")); - options.set_voxel_filter_size( - parameter_dictionary->GetDouble("voxel_filter_size")); - options.set_use_online_correlative_scan_matching( - parameter_dictionary->GetBool("use_online_correlative_scan_matching")); - *options.mutable_adaptive_voxel_filter_options() = - sensor::CreateAdaptiveVoxelFilterOptions( - parameter_dictionary->GetDictionary("adaptive_voxel_filter").get()); - *options.mutable_real_time_correlative_scan_matcher_options() = - scan_matching::CreateRealTimeCorrelativeScanMatcherOptions( - parameter_dictionary - ->GetDictionary("real_time_correlative_scan_matcher") - .get()); - *options.mutable_ceres_scan_matcher_options() = - scan_matching::CreateCeresScanMatcherOptions( - parameter_dictionary->GetDictionary("ceres_scan_matcher").get()); - *options.mutable_motion_filter_options() = - mapping_3d::CreateMotionFilterOptions( - parameter_dictionary->GetDictionary("motion_filter").get()); - options.set_imu_gravity_time_constant( - parameter_dictionary->GetDouble("imu_gravity_time_constant")); - options.set_num_odometry_states( - parameter_dictionary->GetNonNegativeInt("num_odometry_states")); - CHECK_GT(options.num_odometry_states(), 0); - *options.mutable_submaps_options() = CreateSubmapsOptions( - parameter_dictionary->GetDictionary("submaps").get()); - options.set_use_imu_data(parameter_dictionary->GetBool("use_imu_data")); - return options; -} - LocalTrajectoryBuilder::LocalTrajectoryBuilder( const proto::LocalTrajectoryBuilderOptions& options) : options_(options), diff --git a/cartographer/mapping_2d/local_trajectory_builder.h b/cartographer/mapping_2d/local_trajectory_builder.h index cb29143..fa0cb96 100644 --- a/cartographer/mapping_2d/local_trajectory_builder.h +++ b/cartographer/mapping_2d/local_trajectory_builder.h @@ -19,7 +19,6 @@ #include -#include "cartographer/common/lua_parameter_dictionary.h" #include "cartographer/common/time.h" #include "cartographer/mapping/global_trajectory_builder_interface.h" #include "cartographer/mapping/imu_tracker.h" @@ -36,9 +35,6 @@ namespace cartographer { namespace mapping_2d { -proto::LocalTrajectoryBuilderOptions CreateLocalTrajectoryBuilderOptions( - common::LuaParameterDictionary* parameter_dictionary); - // Wires up the local SLAM stack (i.e. UKF, scan matching, etc.) without loop // closure. class LocalTrajectoryBuilder { diff --git a/cartographer/mapping_2d/local_trajectory_builder_options.cc b/cartographer/mapping_2d/local_trajectory_builder_options.cc new file mode 100644 index 0000000..906c206 --- /dev/null +++ b/cartographer/mapping_2d/local_trajectory_builder_options.cc @@ -0,0 +1,67 @@ +/* + * Copyright 2016 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include "cartographer/mapping_2d/local_trajectory_builder_options.h" + +#include "cartographer/mapping_2d/scan_matching/ceres_scan_matcher.h" +#include "cartographer/mapping_2d/scan_matching/real_time_correlative_scan_matcher.h" +#include "cartographer/mapping_2d/submaps.h" +#include "cartographer/mapping_3d/motion_filter.h" +#include "cartographer/sensor/voxel_filter.h" + +namespace cartographer { +namespace mapping_2d { + +proto::LocalTrajectoryBuilderOptions CreateLocalTrajectoryBuilderOptions( + common::LuaParameterDictionary* const parameter_dictionary) { + proto::LocalTrajectoryBuilderOptions options; + options.set_min_range(parameter_dictionary->GetDouble("min_range")); + options.set_max_range(parameter_dictionary->GetDouble("max_range")); + options.set_min_z(parameter_dictionary->GetDouble("min_z")); + options.set_max_z(parameter_dictionary->GetDouble("max_z")); + options.set_missing_data_ray_length( + parameter_dictionary->GetDouble("missing_data_ray_length")); + options.set_voxel_filter_size( + parameter_dictionary->GetDouble("voxel_filter_size")); + options.set_use_online_correlative_scan_matching( + parameter_dictionary->GetBool("use_online_correlative_scan_matching")); + *options.mutable_adaptive_voxel_filter_options() = + sensor::CreateAdaptiveVoxelFilterOptions( + parameter_dictionary->GetDictionary("adaptive_voxel_filter").get()); + *options.mutable_real_time_correlative_scan_matcher_options() = + scan_matching::CreateRealTimeCorrelativeScanMatcherOptions( + parameter_dictionary + ->GetDictionary("real_time_correlative_scan_matcher") + .get()); + *options.mutable_ceres_scan_matcher_options() = + scan_matching::CreateCeresScanMatcherOptions( + parameter_dictionary->GetDictionary("ceres_scan_matcher").get()); + *options.mutable_motion_filter_options() = + mapping_3d::CreateMotionFilterOptions( + parameter_dictionary->GetDictionary("motion_filter").get()); + options.set_imu_gravity_time_constant( + parameter_dictionary->GetDouble("imu_gravity_time_constant")); + options.set_num_odometry_states( + parameter_dictionary->GetNonNegativeInt("num_odometry_states")); + CHECK_GT(options.num_odometry_states(), 0); + *options.mutable_submaps_options() = CreateSubmapsOptions( + parameter_dictionary->GetDictionary("submaps").get()); + options.set_use_imu_data(parameter_dictionary->GetBool("use_imu_data")); + return options; +} + +} // namespace mapping_2d +} // namespace cartographer diff --git a/cartographer/mapping_2d/local_trajectory_builder_options.h b/cartographer/mapping_2d/local_trajectory_builder_options.h new file mode 100644 index 0000000..7f6eb87 --- /dev/null +++ b/cartographer/mapping_2d/local_trajectory_builder_options.h @@ -0,0 +1,32 @@ +/* + * Copyright 2016 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef CARTOGRAPHER_MAPPING_2D_LOCAL_TRAJECTORY_BUILDER_OPTIONS_H_ +#define CARTOGRAPHER_MAPPING_2D_LOCAL_TRAJECTORY_BUILDER_OPTIONS_H_ + +#include "cartographer/common/lua_parameter_dictionary.h" +#include "cartographer/mapping_2d/proto/local_trajectory_builder_options.pb.h" + +namespace cartographer { +namespace mapping_2d { + +proto::LocalTrajectoryBuilderOptions CreateLocalTrajectoryBuilderOptions( + common::LuaParameterDictionary* parameter_dictionary); + +} // namespace mapping_2d +} // namespace cartographer + +#endif // CARTOGRAPHER_MAPPING_2D_LOCAL_TRAJECTORY_BUILDER_OPTIONS_H_ diff --git a/cartographer/mapping_2d/proto/probability_grid.proto b/cartographer/mapping_2d/proto/probability_grid.proto index d4f0e5e..0883818 100644 --- a/cartographer/mapping_2d/proto/probability_grid.proto +++ b/cartographer/mapping_2d/proto/probability_grid.proto @@ -29,4 +29,3 @@ message ProbabilityGrid { optional int32 min_x = 6; optional int32 min_y = 7; } - diff --git a/cartographer/mapping_2d/sparse_pose_graph.cc b/cartographer/mapping_2d/sparse_pose_graph.cc index 56b3664..635ddf3 100644 --- a/cartographer/mapping_2d/sparse_pose_graph.cc +++ b/cartographer/mapping_2d/sparse_pose_graph.cc @@ -290,7 +290,10 @@ void SparsePoseGraph::ComputeConstraintsForScan( void SparsePoseGraph::HandleScanQueue() { constraint_builder_.WhenDone( [this](const sparse_pose_graph::ConstraintBuilder::Result& result) { - constraints_.insert(constraints_.end(), result.begin(), result.end()); + { + common::MutexLocker locker(&mutex_); + constraints_.insert(constraints_.end(), result.begin(), result.end()); + } RunOptimization(); common::MutexLocker locker(&mutex_); @@ -334,8 +337,8 @@ void SparsePoseGraph::WaitForAllComputations() { constraint_builder_.WhenDone( [this, ¬ification]( const sparse_pose_graph::ConstraintBuilder::Result& result) { - constraints_.insert(constraints_.end(), result.begin(), result.end()); common::MutexLocker locker(&mutex_); + constraints_.insert(constraints_.end(), result.begin(), result.end()); notification = true; }); locker.Await([¬ification]() { return notification; }); diff --git a/cartographer/mapping_2d/sparse_pose_graph.h b/cartographer/mapping_2d/sparse_pose_graph.h index 2c32f6d..dbdddc0 100644 --- a/cartographer/mapping_2d/sparse_pose_graph.h +++ b/cartographer/mapping_2d/sparse_pose_graph.h @@ -199,7 +199,7 @@ class SparsePoseGraph : public mapping::SparsePoseGraph { std::deque constant_node_data_; mapping::NestedVectorsById trajectory_nodes_ GUARDED_BY(mutex_); - int num_trajectory_nodes_ = 0 GUARDED_BY(mutex_); + int num_trajectory_nodes_ GUARDED_BY(mutex_) = 0; // Current submap transforms used for displaying data. std::vector> diff --git a/cartographer/mapping_3d/sparse_pose_graph.cc b/cartographer/mapping_3d/sparse_pose_graph.cc index 286ae02..268a437 100644 --- a/cartographer/mapping_3d/sparse_pose_graph.cc +++ b/cartographer/mapping_3d/sparse_pose_graph.cc @@ -303,7 +303,10 @@ void SparsePoseGraph::ComputeConstraintsForScan( void SparsePoseGraph::HandleScanQueue() { constraint_builder_.WhenDone( [this](const sparse_pose_graph::ConstraintBuilder::Result& result) { - constraints_.insert(constraints_.end(), result.begin(), result.end()); + { + common::MutexLocker locker(&mutex_); + constraints_.insert(constraints_.end(), result.begin(), result.end()); + } RunOptimization(); common::MutexLocker locker(&mutex_); @@ -347,8 +350,8 @@ void SparsePoseGraph::WaitForAllComputations() { constraint_builder_.WhenDone( [this, ¬ification]( const sparse_pose_graph::ConstraintBuilder::Result& result) { - constraints_.insert(constraints_.end(), result.begin(), result.end()); common::MutexLocker locker(&mutex_); + constraints_.insert(constraints_.end(), result.begin(), result.end()); notification = true; }); locker.Await([¬ification]() { return notification; }); diff --git a/cartographer/mapping_3d/sparse_pose_graph.h b/cartographer/mapping_3d/sparse_pose_graph.h index a50e7fa..e67e264 100644 --- a/cartographer/mapping_3d/sparse_pose_graph.h +++ b/cartographer/mapping_3d/sparse_pose_graph.h @@ -196,7 +196,7 @@ class SparsePoseGraph : public mapping::SparsePoseGraph { std::deque constant_node_data_; mapping::NestedVectorsById trajectory_nodes_ GUARDED_BY(mutex_); - int num_trajectory_nodes_ = 0 GUARDED_BY(mutex_); + int num_trajectory_nodes_ GUARDED_BY(mutex_) = 0; // Current submap transforms used for displaying data. std::vector> diff --git a/cartographer/mapping_3d/sparse_pose_graph/spa_cost_function.h b/cartographer/mapping_3d/sparse_pose_graph/spa_cost_function.h index 14b9fb3..ae08806 100644 --- a/cartographer/mapping_3d/sparse_pose_graph/spa_cost_function.h +++ b/cartographer/mapping_3d/sparse_pose_graph/spa_cost_function.h @@ -24,6 +24,7 @@ #include "cartographer/common/math.h" #include "cartographer/mapping/sparse_pose_graph.h" #include "cartographer/transform/rigid_transform.h" +#include "cartographer/transform/transform.h" #include "ceres/ceres.h" #include "ceres/jet.h"