Minor fixes. (#303)

Extract mapping_2d::CreateLocalTrajectoryBuilderOptions() into its own file.
Loop initialization fix in the trajectory_connectivity_test.
Thread safety fixes in sparse_pose_graph.

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 {

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_a = 0; trajectory_a < kNumTrajectories; ++trajectory_a) {
-    for (int trajectory_b; trajectory_b < kNumTrajectories; ++trajectory_b) {
+    for (int trajectory_b = 0; trajectory_b < kNumTrajectories;
+         ++trajectory_b) {
       EXPECT_FALSE(trajectory_connectivity.TransitivelyConnected(trajectory_a,
                                                                  trajectory_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),

cartographer/mapping_2d/local_trajectory_builder.h

@@ -19,7 +19,6 @@
 
 #include <memory>
 
-#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 {

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

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_

cartographer/mapping_2d/proto/probability_grid.proto

@@ -29,4 +29,3 @@ message ProbabilityGrid {
   optional int32 min_x = 6;
   optional int32 min_y = 7;
 }
-

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, &notification](
           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([&notification]() { return notification; });

cartographer/mapping_2d/sparse_pose_graph.h

@@ -199,7 +199,7 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
   std::deque<mapping::TrajectoryNode::ConstantData> constant_node_data_;
   mapping::NestedVectorsById<mapping::TrajectoryNode, mapping::NodeId>
       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<std::vector<sparse_pose_graph::SubmapData>>

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, &notification](
           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([&notification]() { return notification; });

cartographer/mapping_3d/sparse_pose_graph.h

@@ -196,7 +196,7 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
   std::deque<mapping::TrajectoryNode::ConstantData> constant_node_data_;
   mapping::NestedVectorsById<mapping::TrajectoryNode, mapping::NodeId>
       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<std::vector<sparse_pose_graph::SubmapData>>

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"