Test ConstraintBuilder2D/3D (#1100)

cartographer/mapping/internal/2d/pose_graph/constraint_builder_2d.cc

@@ -55,7 +55,7 @@ transform::Rigid2d ComputeSubmapPose(const Submap2D& submap) {
 
 ConstraintBuilder2D::ConstraintBuilder2D(
     const pose_graph::proto::ConstraintBuilderOptions& options,
-    common::ThreadPool* const thread_pool)
+    common::ThreadPoolInterface* const thread_pool)
     : options_(options),
       thread_pool_(thread_pool),
       sampler_(options.sampling_ratio()),

cartographer/mapping/internal/2d/pose_graph/constraint_builder_2d.h

@@ -61,7 +61,7 @@ class ConstraintBuilder2D {
   using Result = std::vector<Constraint>;
 
   ConstraintBuilder2D(const proto::ConstraintBuilderOptions& options,
-                      common::ThreadPool* thread_pool);
+                      common::ThreadPoolInterface* thread_pool);
   ~ConstraintBuilder2D();
 
   ConstraintBuilder2D(const ConstraintBuilder2D&) = delete;
@@ -139,7 +139,7 @@ class ConstraintBuilder2D {
   void FinishComputation(int computation_index) EXCLUDES(mutex_);
 
   const pose_graph::proto::ConstraintBuilderOptions options_;
-  common::ThreadPool* thread_pool_;
+  common::ThreadPoolInterface* thread_pool_;
   common::Mutex mutex_;
 
   // 'callback' set by WhenDone().

cartographer/mapping/internal/2d/pose_graph/constraint_builder_2d_test.cc

@@ -0,0 +1,108 @@
+/*
+ * Copyright 2018 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/internal/2d/pose_graph/constraint_builder_2d.h"
+
+#include <functional>
+
+#include "cartographer/common/internal/testing/thread_pool_for_testing.h"
+#include "cartographer/mapping/2d/submap_2d.h"
+#include "cartographer/mapping/internal/pose_graph/constraint_builder.h"
+#include "cartographer/mapping/internal/testing/test_helpers.h"
+#include "gmock/gmock.h"
+#include "gtest/gtest.h"
+
+namespace cartographer {
+namespace mapping {
+namespace pose_graph {
+namespace {
+
+class MockCallback {
+ public:
+  MOCK_METHOD1(Run, void(const ConstraintBuilder2D::Result&));
+};
+
+class ConstraintBuilder2DTest : public ::testing::Test {
+ protected:
+  void SetUp() override {
+    auto constraint_builder_parameters = test::ResolveLuaParameters(R"text(
+            include "pose_graph.lua"
+            POSE_GRAPH.constraint_builder.sampling_ratio = 1
+            POSE_GRAPH.constraint_builder.min_score = 0
+            POSE_GRAPH.constraint_builder.global_localization_min_score = 0
+            return POSE_GRAPH.constraint_builder)text");
+    constraint_builder_ = common::make_unique<ConstraintBuilder2D>(
+        CreateConstraintBuilderOptions(constraint_builder_parameters.get()),
+        &thread_pool_);
+  }
+
+  std::unique_ptr<ConstraintBuilder2D> constraint_builder_;
+  MockCallback mock_;
+  common::testing::ThreadPoolForTesting thread_pool_;
+};
+
+TEST_F(ConstraintBuilder2DTest, CallsBack) {
+  EXPECT_EQ(constraint_builder_->GetNumFinishedNodes(), 0);
+  EXPECT_CALL(mock_, Run(testing::IsEmpty()));
+  constraint_builder_->NotifyEndOfNode();
+  constraint_builder_->WhenDone(
+      std::bind(&MockCallback::Run, &mock_, std::placeholders::_1));
+  thread_pool_.WaitUntilIdle();
+  EXPECT_EQ(constraint_builder_->GetNumFinishedNodes(), 1);
+}
+
+TEST_F(ConstraintBuilder2DTest, FindsConstraints) {
+  TrajectoryNode::Data node_data;
+  node_data.filtered_gravity_aligned_point_cloud.push_back(
+      Eigen::Vector3f(0.1, 0.2, 0.3));
+  node_data.gravity_alignment = Eigen::Quaterniond::Identity();
+  node_data.local_pose = transform::Rigid3d::Identity();
+  SubmapId submap_id{0, 1};
+  Submap2D submap(MapLimits(1., Eigen::Vector2d(2., 3.), CellLimits(100, 110)),
+                  Eigen::Vector2f(4.f, 5.f));
+  int expected_nodes = 0;
+  for (int i = 0; i < 2; ++i) {
+    EXPECT_EQ(constraint_builder_->GetNumFinishedNodes(), expected_nodes);
+    for (int j = 0; j < 2; ++j) {
+      constraint_builder_->MaybeAddConstraint(submap_id, &submap, NodeId{},
+                                              &node_data,
+                                              transform::Rigid2d::Identity());
+    }
+    constraint_builder_->MaybeAddGlobalConstraint(submap_id, &submap, NodeId{},
+                                                  &node_data);
+    constraint_builder_->NotifyEndOfNode();
+    thread_pool_.WaitUntilIdle();
+    EXPECT_EQ(constraint_builder_->GetNumFinishedNodes(), ++expected_nodes);
+    constraint_builder_->NotifyEndOfNode();
+    thread_pool_.WaitUntilIdle();
+    EXPECT_EQ(constraint_builder_->GetNumFinishedNodes(), ++expected_nodes);
+    EXPECT_CALL(mock_,
+                Run(testing::AllOf(
+                    testing::SizeIs(3),
+                    testing::Each(testing::Field(
+                        &PoseGraphInterface::Constraint::tag,
+                        PoseGraphInterface::Constraint::INTER_SUBMAP)))));
+    constraint_builder_->WhenDone(
+        std::bind(&MockCallback::Run, &mock_, std::placeholders::_1));
+    thread_pool_.WaitUntilIdle();
+    constraint_builder_->DeleteScanMatcher(submap_id);
+  }
+}
+
+}  // namespace
+}  // namespace pose_graph
+}  // namespace mapping
+}  // namespace cartographer

cartographer/mapping/internal/3d/pose_graph/constraint_builder_3d.cc

@@ -57,7 +57,7 @@ static auto* kGlobalConstraintLowResolutionScoresMetric =
 
 ConstraintBuilder3D::ConstraintBuilder3D(
     const proto::ConstraintBuilderOptions& options,
-    common::ThreadPool* const thread_pool)
+    common::ThreadPoolInterface* const thread_pool)
     : options_(options),
       thread_pool_(thread_pool),
       sampler_(options.sampling_ratio()),

cartographer/mapping/internal/3d/pose_graph/constraint_builder_3d.h

@@ -60,7 +60,7 @@ class ConstraintBuilder3D {
   using Result = std::vector<Constraint>;
 
   ConstraintBuilder3D(const proto::ConstraintBuilderOptions& options,
-                      common::ThreadPool* thread_pool);
+                      common::ThreadPoolInterface* thread_pool);
   ~ConstraintBuilder3D();
 
   ConstraintBuilder3D(const ConstraintBuilder3D&) = delete;
@@ -153,7 +153,7 @@ class ConstraintBuilder3D {
   void FinishComputation(int computation_index) EXCLUDES(mutex_);
 
   const proto::ConstraintBuilderOptions options_;
-  common::ThreadPool* thread_pool_;
+  common::ThreadPoolInterface* thread_pool_;
   common::Mutex mutex_;
 
   // 'callback' set by WhenDone().

cartographer/mapping/internal/3d/pose_graph/constraint_builder_3d_test.cc

@@ -0,0 +1,118 @@
+/*
+ * Copyright 2018 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/internal/3d/pose_graph/constraint_builder_3d.h"
+
+#include <functional>
+
+#include "cartographer/common/internal/testing/thread_pool_for_testing.h"
+#include "cartographer/mapping/3d/submap_3d.h"
+#include "cartographer/mapping/internal/pose_graph/constraint_builder.h"
+#include "cartographer/mapping/internal/testing/test_helpers.h"
+#include "cartographer/mapping/pose_graph.h"
+#include "cartographer/transform/rigid_transform.h"
+#include "gmock/gmock.h"
+#include "gtest/gtest.h"
+
+namespace cartographer {
+namespace mapping {
+namespace pose_graph {
+namespace {
+
+class MockCallback {
+ public:
+  MOCK_METHOD1(Run, void(const ConstraintBuilder3D::Result&));
+};
+
+class ConstraintBuilder3DTest : public ::testing::Test {
+ protected:
+  void SetUp() override {
+    auto constraint_builder_parameters = test::ResolveLuaParameters(R"text(
+    include "pose_graph.lua"
+    POSE_GRAPH.constraint_builder.sampling_ratio = 1
+    POSE_GRAPH.constraint_builder.min_score = 0
+    POSE_GRAPH.constraint_builder.global_localization_min_score = 0
+    POSE_GRAPH.constraint_builder.fast_correlative_scan_matcher_3d.min_low_resolution_score = 0
+    POSE_GRAPH.constraint_builder.fast_correlative_scan_matcher_3d.min_rotational_score = 0
+    return POSE_GRAPH.constraint_builder)text");
+    constraint_builder_ = common::make_unique<ConstraintBuilder3D>(
+        CreateConstraintBuilderOptions(constraint_builder_parameters.get()),
+        &thread_pool_);
+  }
+
+  std::unique_ptr<ConstraintBuilder3D> constraint_builder_;
+  MockCallback mock_;
+  common::testing::ThreadPoolForTesting thread_pool_;
+};
+
+TEST_F(ConstraintBuilder3DTest, CallsBack) {
+  EXPECT_EQ(constraint_builder_->GetNumFinishedNodes(), 0);
+  EXPECT_CALL(mock_, Run(testing::IsEmpty()));
+  constraint_builder_->NotifyEndOfNode();
+  constraint_builder_->WhenDone(
+      std::bind(&MockCallback::Run, &mock_, std::placeholders::_1));
+  thread_pool_.WaitUntilIdle();
+  EXPECT_EQ(constraint_builder_->GetNumFinishedNodes(), 1);
+}
+
+TEST_F(ConstraintBuilder3DTest, FindsConstraints) {
+  TrajectoryNode node;
+  auto node_data = std::make_shared<TrajectoryNode::Data>();
+  node_data->gravity_alignment = Eigen::Quaterniond::Identity();
+  node_data->high_resolution_point_cloud.push_back(
+      Eigen::Vector3f(0.1, 0.2, 0.3));
+  node_data->low_resolution_point_cloud.push_back(
+      Eigen::Vector3f(0.1, 0.2, 0.3));
+  node_data->rotational_scan_matcher_histogram = Eigen::VectorXf::Zero(3);
+  node_data->local_pose = transform::Rigid3d::Identity();
+  node.constant_data = node_data;
+  std::vector<TrajectoryNode> submap_nodes = {node};
+  SubmapId submap_id{0, 1};
+  Submap3D submap(0.1, 0.1, transform::Rigid3d::Identity());
+  int expected_nodes = 0;
+  for (int i = 0; i < 2; ++i) {
+    EXPECT_EQ(constraint_builder_->GetNumFinishedNodes(), expected_nodes);
+    for (int j = 0; j < 2; ++j) {
+      constraint_builder_->MaybeAddConstraint(
+          submap_id, &submap, NodeId{}, node.constant_data.get(), submap_nodes,
+          transform::Rigid3d::Identity(), transform::Rigid3d::Identity());
+    }
+    constraint_builder_->MaybeAddGlobalConstraint(
+        submap_id, &submap, NodeId{}, node.constant_data.get(), submap_nodes,
+        Eigen::Quaterniond::Identity(), Eigen::Quaterniond::Identity());
+    constraint_builder_->NotifyEndOfNode();
+    thread_pool_.WaitUntilIdle();
+    EXPECT_EQ(constraint_builder_->GetNumFinishedNodes(), ++expected_nodes);
+    constraint_builder_->NotifyEndOfNode();
+    thread_pool_.WaitUntilIdle();
+    EXPECT_EQ(constraint_builder_->GetNumFinishedNodes(), ++expected_nodes);
+    EXPECT_CALL(mock_,
+                Run(testing::AllOf(
+                    testing::SizeIs(3),
+                    testing::Each(testing::Field(
+                        &PoseGraphInterface::Constraint::tag,
+                        PoseGraphInterface::Constraint::INTER_SUBMAP)))));
+    constraint_builder_->WhenDone(
+        std::bind(&MockCallback::Run, &mock_, std::placeholders::_1));
+    thread_pool_.WaitUntilIdle();
+    constraint_builder_->DeleteScanMatcher(submap_id);
+  }
+}
+
+}  // namespace
+}  // namespace pose_graph
+}  // namespace mapping
+}  // namespace cartographer