cartographer: correlative_scan_matcher

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/internal/2d/scan_matching/correlative_scan_matcher_2d.h"
00018 #include "cartographer/sensor/point_cloud.h"
00019 #include "gtest/gtest.h"
00020 
00021 namespace cartographer {
00022 namespace mapping {
00023 namespace scan_matching {
00024 namespace {
00025 
00026 TEST(SearchParameters, Construction) {
00027   const SearchParameters search_parameters(4, 5, 0.03, 0.05);
00028   EXPECT_EQ(5, search_parameters.num_angular_perturbations);
00029   EXPECT_NEAR(0.03, search_parameters.angular_perturbation_step_size, 1e-9);
00030   EXPECT_NEAR(0.05, search_parameters.resolution, 1e-9);
00031   EXPECT_EQ(11, search_parameters.num_scans);
00032   EXPECT_EQ(11, search_parameters.linear_bounds.size());
00033   for (const SearchParameters::LinearBounds linear_bounds :
00034        search_parameters.linear_bounds) {
00035     EXPECT_EQ(-4, linear_bounds.min_x);
00036     EXPECT_EQ(4, linear_bounds.max_x);
00037     EXPECT_EQ(-4, linear_bounds.min_y);
00038     EXPECT_EQ(4, linear_bounds.max_y);
00039   }
00040 }
00041 
00042 TEST(Candidate, Construction) {
00043   const SearchParameters search_parameters(4, 5, 0.03, 0.05);
00044   const Candidate2D candidate(3, 4, -5, search_parameters);
00045   EXPECT_EQ(3, candidate.scan_index);
00046   EXPECT_EQ(4, candidate.x_index_offset);
00047   EXPECT_EQ(-5, candidate.y_index_offset);
00048   EXPECT_NEAR(0.25, candidate.x, 1e-9);
00049   EXPECT_NEAR(-0.2, candidate.y, 1e-9);
00050   EXPECT_NEAR(-0.06, candidate.orientation, 1e-9);
00051   EXPECT_NEAR(0., candidate.score, 1e-9);
00052 
00053   Candidate2D bigger_candidate(3, 4, 5, search_parameters);
00054   bigger_candidate.score = 1.;
00055   EXPECT_LT(candidate, bigger_candidate);
00056 }
00057 
00058 TEST(GenerateRotatedScans, GenerateRotatedScans) {
00059   sensor::PointCloud point_cloud;
00060   point_cloud.push_back({Eigen::Vector3f{-1.f, 1.f, 0.f}});
00061   const std::vector<sensor::PointCloud> scans =
00062       GenerateRotatedScans(point_cloud, SearchParameters(0, 1, M_PI / 2., 0.));
00063   EXPECT_EQ(3, scans.size());
00064   EXPECT_NEAR(1., scans[0][0].position.x(), 1e-6);
00065   EXPECT_NEAR(1., scans[0][0].position.y(), 1e-6);
00066   EXPECT_NEAR(-1., scans[1][0].position.x(), 1e-6);
00067   EXPECT_NEAR(1., scans[1][0].position.y(), 1e-6);
00068   EXPECT_NEAR(-1., scans[2][0].position.x(), 1e-6);
00069   EXPECT_NEAR(-1., scans[2][0].position.y(), 1e-6);
00070 }
00071 
00072 TEST(DiscretizeScans, DiscretizeScans) {
00073   sensor::PointCloud point_cloud;
00074   point_cloud.push_back({Eigen::Vector3f{0.025f, 0.175f, 0.f}});
00075   point_cloud.push_back({Eigen::Vector3f{-0.025f, 0.175f, 0.f}});
00076   point_cloud.push_back({Eigen::Vector3f{-0.075f, 0.175f, 0.f}});
00077   point_cloud.push_back({Eigen::Vector3f{-0.125f, 0.175f, 0.f}});
00078   point_cloud.push_back({Eigen::Vector3f{-0.125f, 0.125f, 0.f}});
00079   point_cloud.push_back({Eigen::Vector3f{-0.125f, 0.075f, 0.f}});
00080   point_cloud.push_back({Eigen::Vector3f{-0.125f, 0.025f, 0.f}});
00081   const MapLimits map_limits(0.05, Eigen::Vector2d(0.05, 0.25),
00082                              CellLimits(6, 6));
00083   const std::vector<sensor::PointCloud> scans =
00084       GenerateRotatedScans(point_cloud, SearchParameters(0, 0, 0., 0.));
00085   const std::vector<DiscreteScan2D> discrete_scans =
00086       DiscretizeScans(map_limits, scans, Eigen::Translation2f::Identity());
00087   EXPECT_EQ(1, discrete_scans.size());
00088   EXPECT_EQ(7, discrete_scans[0].size());
00089   EXPECT_TRUE((Eigen::Array2i(1, 0) == discrete_scans[0][0]).all());
00090   EXPECT_TRUE((Eigen::Array2i(1, 1) == discrete_scans[0][1]).all());
00091   EXPECT_TRUE((Eigen::Array2i(1, 2) == discrete_scans[0][2]).all());
00092   EXPECT_TRUE((Eigen::Array2i(1, 3) == discrete_scans[0][3]).all());
00093   EXPECT_TRUE((Eigen::Array2i(2, 3) == discrete_scans[0][4]).all());
00094   EXPECT_TRUE((Eigen::Array2i(3, 3) == discrete_scans[0][5]).all());
00095   EXPECT_TRUE((Eigen::Array2i(4, 3) == discrete_scans[0][6]).all());
00096 }
00097 
00098 }  // namespace
00099 }  // namespace scan_matching
00100 }  // namespace mapping
00101 }  // namespace cartographer