cartographer: submap_3d_test.cc Source File

Go to the documentation of this file.
00001 /*
00002  * Copyright 2016 The Cartographer Authors
00003  *
00004  * Licensed under the Apache License, Version 2.0 (the "License");
00005  * you may not use this file except in compliance with the License.
00006  * You may obtain a copy of the License at
00007  *
00008  *      http://www.apache.org/licenses/LICENSE-2.0
00009  *
00010  * Unless required by applicable law or agreed to in writing, software
00011  * distributed under the License is distributed on an "AS IS" BASIS,
00012  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
00013  * See the License for the specific language governing permissions and
00014  * limitations under the License.
00015  */
00016 
00017 #include "cartographer/mapping/3d/submap_3d.h"
00018 
00019 #include "cartographer/transform/transform.h"
00020 #include "gmock/gmock.h"
00021 
00022 namespace cartographer {
00023 namespace mapping {
00024 namespace {
00025 
00026 TEST(SubmapsTest, ToFromProto) {
00027   Eigen::VectorXf histogram(2);
00028   histogram << 1.0f, 2.0f;
00029   const Submap3D expected(
00030       0.05, 0.25,
00031       transform::Rigid3d(Eigen::Vector3d(1., 2., 0.),
00032                          Eigen::Quaterniond(0., 0., 0., 1.)),
00033       histogram);
00034   const proto::Submap proto =
00035       expected.ToProto(true /* include_probability_grid_data */);
00036   EXPECT_FALSE(proto.has_submap_2d());
00037   EXPECT_TRUE(proto.has_submap_3d());
00038   const auto actual = Submap3D(proto.submap_3d());
00039   EXPECT_TRUE(expected.local_pose().translation().isApprox(
00040       actual.local_pose().translation(), 1e-6));
00041   EXPECT_TRUE(expected.local_pose().rotation().isApprox(
00042       actual.local_pose().rotation(), 1e-6));
00043   EXPECT_EQ(expected.num_range_data(), actual.num_range_data());
00044   EXPECT_EQ(expected.insertion_finished(), actual.insertion_finished());
00045   EXPECT_NEAR(expected.high_resolution_hybrid_grid().resolution(), 0.05, 1e-6);
00046   EXPECT_NEAR(expected.low_resolution_hybrid_grid().resolution(), 0.25, 1e-6);
00047   EXPECT_TRUE(expected.rotational_scan_matcher_histogram().isApprox(
00048       actual.rotational_scan_matcher_histogram(), 1e-6));
00049 }
00050 
00051 }  // namespace
00052 }  // namespace mapping
00053 }  // namespace cartographer