diff --git a/cartographer/mapping/internal/3d/pose_graph_3d.h b/cartographer/mapping/internal/3d/pose_graph_3d.h index a3dac98..c9986e4 100644 --- a/cartographer/mapping/internal/3d/pose_graph_3d.h +++ b/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_); diff --git a/cartographer/mapping/internal/3d/pose_graph_3d_test.cc b/cartographer/mapping/internal/3d/pose_graph_3d_test.cc new file mode 100644 index 0000000..b529f1e --- /dev/null +++ b/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(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( + pose_graph_options_, thread_pool_.get()); + } + + proto::PoseGraphOptions pose_graph_options_; + std::unique_ptr thread_pool_; + std::unique_ptr 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 diff --git a/cartographer/mapping/internal/test_helpers.cc b/cartographer/mapping/internal/test_helpers.cc index c307131..ebbfee5 100644 --- a/cartographer/mapping/internal/test_helpers.cc +++ b/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 diff --git a/cartographer/mapping/internal/test_helpers.h b/cartographer/mapping/internal/test_helpers.h index 2e90851..f4ea723 100644 --- a/cartographer/mapping/internal/test_helpers.h +++ b/cartographer/mapping/internal/test_helpers.h @@ -20,6 +20,7 @@ #include #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 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 diff --git a/cartographer/mapping/pose_graph_test.cc b/cartographer/mapping/pose_graph_test.cc new file mode 100644 index 0000000..85da2a2 --- /dev/null +++ b/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 + 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