Test PoseGraph3D (#990)

cartographer/mapping/internal/3d/pose_graph_3d.h

@@ -133,6 +133,11 @@ class PoseGraph3D : public PoseGraph {
   transform::Rigid3d GetInterpolatedGlobalTrajectoryPose(
       int trajectory_id, const common::Time time) const REQUIRES(mutex_);
 
+ protected:
+  // Waits until we caught up (i.e. nothing is waiting to be scheduled), and
+  // all computations have finished.
+  void WaitForAllComputations() EXCLUDES(mutex_);
+
  private:
   // The current state of the submap in the background threads. When this
   // transitions to kFinished, all nodes are tried to match against this submap.
@@ -180,10 +185,6 @@ class PoseGraph3D : public PoseGraph {
   // been computed, that will also do all work that queue up in 'work_queue_'.
   void HandleWorkQueue() REQUIRES(mutex_);
 
-  // Waits until we caught up (i.e. nothing is waiting to be scheduled), and
-  // all computations have finished.
-  void WaitForAllComputations() EXCLUDES(mutex_);
-
   // Runs the optimization. Callers have to make sure, that there is only one
   // optimization being run at a time.
   void RunOptimization() EXCLUDES(mutex_);

cartographer/mapping/internal/3d/pose_graph_3d_test.cc

@@ -0,0 +1,102 @@
+/*
+ * 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_3d.h"
+
+#include "cartographer/mapping/internal/test_helpers.h"
+#include "cartographer/mapping/proto/serialization.pb.h"
+#include "cartographer/transform/rigid_transform.h"
+#include "cartographer/transform/rigid_transform_test_helpers.h"
+#include "cartographer/transform/transform.h"
+#include "gmock/gmock.h"
+#include "google/protobuf/util/message_differencer.h"
+
+namespace cartographer {
+namespace mapping {
+namespace {
+
+class PoseGraph3DForTesting : public PoseGraph3D {
+ public:
+  PoseGraph3DForTesting(const proto::PoseGraphOptions& options,
+                        common::ThreadPool* thread_pool)
+      : PoseGraph3D(options, thread_pool) {}
+
+  void WaitForAllComputations() { PoseGraph3D::WaitForAllComputations(); }
+};
+
+class PoseGraph3DTest : public ::testing::Test {
+ protected:
+  PoseGraph3DTest()
+      : thread_pool_(common::make_unique<common::ThreadPool>(1)) {}
+
+  void SetUp() override {
+    const std::string kPoseGraphLua = R"text(
+      include "pose_graph.lua"
+      return POSE_GRAPH)text";
+    auto pose_graph_parameters = test::ResolveLuaParameters(kPoseGraphLua);
+    pose_graph_options_ = CreatePoseGraphOptions(pose_graph_parameters.get());
+    pose_graph_ = common::make_unique<PoseGraph3DForTesting>(
+        pose_graph_options_, thread_pool_.get());
+  }
+
+  proto::PoseGraphOptions pose_graph_options_;
+  std::unique_ptr<common::ThreadPool> thread_pool_;
+  std::unique_ptr<PoseGraph3DForTesting> pose_graph_;
+};
+
+TEST_F(PoseGraph3DTest, Empty) {
+  pose_graph_->WaitForAllComputations();
+  EXPECT_TRUE(pose_graph_->GetAllSubmapData().empty());
+  EXPECT_TRUE(pose_graph_->constraints().empty());
+  EXPECT_TRUE(pose_graph_->GetConnectedTrajectories().empty());
+  EXPECT_TRUE(pose_graph_->GetAllSubmapPoses().empty());
+  EXPECT_TRUE(pose_graph_->GetTrajectoryNodes().empty());
+  EXPECT_TRUE(pose_graph_->GetTrajectoryNodePoses().empty());
+  EXPECT_TRUE(pose_graph_->GetTrajectoryData().empty());
+  proto::PoseGraph empty_proto;
+  EXPECT_TRUE(google::protobuf::util::MessageDifferencer::Equals(
+      pose_graph_->ToProto(), empty_proto));
+}
+
+TEST_F(PoseGraph3DTest, BasicSerialization) {
+  proto::PoseGraph proto;
+  auto fake_node = test::CreateFakeNode();
+  test::AddToProtoGraph(fake_node, &proto);
+  pose_graph_->AddNodeFromProto(transform::Rigid3d::Identity(), fake_node);
+  auto fake_submap = test::CreateFakeSubmap3D();
+  test::AddToProtoGraph(fake_submap, &proto);
+  pose_graph_->AddSubmapFromProto(transform::Rigid3d::Identity(), fake_submap);
+  test::AddToProtoGraph(test::CreateFakeConstraint(fake_node, fake_submap),
+                        &proto);
+  pose_graph_->AddSerializedConstraints(FromProto(proto.constraint()));
+
+  pose_graph_->WaitForAllComputations();
+  proto::PoseGraph actual_proto = pose_graph_->ToProto();
+  EXPECT_TRUE(google::protobuf::util::MessageDifferencer::Equals(
+      proto.constraint(0), actual_proto.constraint(0)));
+  EXPECT_TRUE(google::protobuf::util::MessageDifferencer::Equals(
+      proto.trajectory(0).node(0), actual_proto.trajectory(0).node(0)));
+  EXPECT_TRUE(google::protobuf::util::MessageDifferencer::Equals(
+      proto.trajectory(0).submap(0), actual_proto.trajectory(0).submap(0)));
+  EXPECT_TRUE(google::protobuf::util::MessageDifferencer::Equals(
+      proto.trajectory(0), actual_proto.trajectory(0)));
+  EXPECT_TRUE(
+      google::protobuf::util::MessageDifferencer::Equals(proto, actual_proto));
+}
+
+}  // namespace
+}  // namespace mapping
+}  // namespace cartographer

cartographer/mapping/internal/test_helpers.cc

@@ -21,6 +21,7 @@
 #include "cartographer/common/lua_parameter_dictionary_test_helpers.h"
 #include "cartographer/common/make_unique.h"
 #include "cartographer/sensor/timed_point_cloud_data.h"
+#include "cartographer/transform/transform.h"
 
 namespace cartographer {
 namespace mapping {
@@ -67,6 +68,86 @@ GenerateFakeRangeMeasurements(double travel_distance, double duration,
   return measurements;
 }
 
+proto::Submap CreateFakeSubmap3D(int trajectory_id, int submap_index) {
+  proto::Submap proto;
+  proto.mutable_submap_id()->set_trajectory_id(trajectory_id);
+  proto.mutable_submap_id()->set_submap_index(submap_index);
+  proto.mutable_submap_3d()->set_num_range_data(1);
+  *proto.mutable_submap_3d()->mutable_local_pose() =
+      transform::ToProto(transform::Rigid3d::Identity());
+  return proto;
+}
+
+proto::Node CreateFakeNode(int trajectory_id, int node_index) {
+  proto::Node proto;
+  proto.mutable_node_id()->set_trajectory_id(trajectory_id);
+  proto.mutable_node_id()->set_node_index(node_index);
+  proto.mutable_node_data()->set_timestamp(42);
+  *proto.mutable_node_data()->mutable_local_pose() =
+      transform::ToProto(transform::Rigid3d::Identity());
+  return proto;
+}
+
+proto::PoseGraph::Constraint CreateFakeConstraint(const proto::Node& node,
+                                                  const proto::Submap& submap) {
+  proto::PoseGraph::Constraint proto;
+  proto.mutable_submap_id()->set_submap_index(
+      submap.submap_id().submap_index());
+  proto.mutable_submap_id()->set_trajectory_id(
+      submap.submap_id().trajectory_id());
+  proto.mutable_node_id()->set_node_index(node.node_id().node_index());
+  proto.mutable_node_id()->set_trajectory_id(node.node_id().trajectory_id());
+  transform::Rigid3d pose(
+      Eigen::Vector3d(2., 3., 4.),
+      Eigen::AngleAxisd(M_PI / 8., Eigen::Vector3d::UnitX()));
+  *proto.mutable_relative_pose() = transform::ToProto(pose);
+  proto.set_translation_weight(0.2f);
+  proto.set_rotation_weight(0.1f);
+  proto.set_tag(proto::PoseGraph::Constraint::INTER_SUBMAP);
+  return proto;
+}
+
+proto::Trajectory* CreateTrajectoryIfNeeded(int trajectory_id,
+                                            proto::PoseGraph* pose_graph) {
+  for (int i = 0; i < pose_graph->trajectory_size(); ++i) {
+    proto::Trajectory* trajectory = pose_graph->mutable_trajectory(i);
+    if (trajectory->trajectory_id() == trajectory_id) {
+      return trajectory;
+    }
+  }
+  proto::Trajectory* trajectory = pose_graph->add_trajectory();
+  trajectory->set_trajectory_id(trajectory_id);
+  return trajectory;
+}
+
+void AddToProtoGraph(const proto::Node& node_data,
+                     proto::PoseGraph* pose_graph) {
+  auto* trajectory =
+      CreateTrajectoryIfNeeded(node_data.node_id().trajectory_id(), pose_graph);
+  auto* node = trajectory->add_node();
+  node->set_timestamp(node_data.node_data().timestamp());
+  node->set_node_index(node_data.node_id().node_index());
+  *node->mutable_pose() = node_data.node_data().local_pose();
+}
+
+void AddToProtoGraph(const proto::Submap& submap_data,
+                     proto::PoseGraph* pose_graph) {
+  auto* trajectory = CreateTrajectoryIfNeeded(
+      submap_data.submap_id().trajectory_id(), pose_graph);
+  auto* submap = trajectory->add_submap();
+  submap->set_submap_index(submap_data.submap_id().submap_index());
+  if (submap_data.has_submap_2d()) {
+    *submap->mutable_pose() = submap_data.submap_2d().local_pose();
+  } else {
+    *submap->mutable_pose() = submap_data.submap_3d().local_pose();
+  }
+}
+
+void AddToProtoGraph(const proto::PoseGraph::Constraint& constraint,
+                     proto::PoseGraph* pose_graph) {
+  *pose_graph->add_constraint() = constraint;
+}
+
 }  // namespace test
 }  // namespace mapping
 }  // namespace cartographer

cartographer/mapping/internal/test_helpers.h

@@ -20,6 +20,7 @@
 #include <memory>
 
 #include "cartographer/common/lua_parameter_dictionary.h"
+#include "cartographer/mapping/proto/serialization.pb.h"
 #include "cartographer/sensor/timed_point_cloud_data.h"
 
 namespace cartographer {
@@ -33,6 +34,25 @@ std::vector<cartographer::sensor::TimedPointCloudData>
 GenerateFakeRangeMeasurements(double travel_distance, double duration,
                               double time_step);
 
+proto::Submap CreateFakeSubmap3D(int trajectory_id = 1, int submap_index = 1);
+
+proto::Node CreateFakeNode(int trajectory_id = 1, int node_index = 1);
+
+proto::PoseGraph::Constraint CreateFakeConstraint(const proto::Node& node,
+                                                  const proto::Submap& submap);
+
+proto::Trajectory* CreateTrajectoryIfNeeded(int trajectory_id,
+                                            proto::PoseGraph* pose_graph);
+
+void AddToProtoGraph(const proto::Node& node_data,
+                     proto::PoseGraph* pose_graph);
+
+void AddToProtoGraph(const proto::Submap& submap_data,
+                     proto::PoseGraph* pose_graph);
+
+void AddToProtoGraph(const proto::PoseGraph::Constraint& constraint,
+                     proto::PoseGraph* pose_graph);
+
 }  // namespace test
 }  // namespace mapping
 }  // namespace cartographer

cartographer/mapping/pose_graph_test.cc

@@ -0,0 +1,42 @@
+/*
+ * 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/pose_graph.h"
+
+#include "cartographer/mapping/internal/test_helpers.h"
+#include "cartographer/mapping/trajectory_builder_interface.h"
+#include "cartographer/transform/transform.h"
+#include "google/protobuf/util/message_differencer.h"
+#include "gtest/gtest.h"
+
+namespace cartographer {
+namespace mapping {
+namespace {
+
+TEST(PoseGraph, SerializeConstraint) {
+  proto::PoseGraph::Constraint expected_constraint = test::CreateFakeConstraint(
+      test::CreateFakeNode(1, 2), test::CreateFakeSubmap3D(2, 3));
+  ::google::protobuf::RepeatedPtrField<proto::PoseGraph::Constraint>
+      constraint_protos;
+  *constraint_protos.Add() = expected_constraint;
+  PoseGraph::Constraint constraint = FromProto(constraint_protos).front();
+  EXPECT_TRUE(google::protobuf::util::MessageDifferencer::Equals(
+      expected_constraint, ToProto(constraint)));
+}
+
+}  // namespace
+}  // namespace mapping
+}  // namespace cartographer