cartographer: rotational_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/3d/scan_matching/rotational_scan_matcher.h"
00018 
00019 #include <cmath>
00020 
00021 #include "gtest/gtest.h"
00022 
00023 namespace cartographer {
00024 namespace mapping {
00025 namespace scan_matching {
00026 namespace {
00027 
00028 TEST(RotationalScanMatcher3DTest, OnlySameHistogramIsScoreOne) {
00029   Eigen::VectorXf histogram(7);
00030   histogram << 1.f, 43.f, 0.5f, 0.3123f, 23.f, 42.f, 0.f;
00031   RotationalScanMatcher matcher(&histogram);
00032   const auto scores = matcher.Match(histogram, 0.f, {0.f, 1.f});
00033   ASSERT_EQ(2, scores.size());
00034   EXPECT_NEAR(1.f, scores[0], 1e-6);
00035   EXPECT_GT(1.f, scores[1]);
00036 }
00037 
00038 TEST(RotationalScanMatcher3DTest, InterpolatesAsExpected) {
00039   constexpr int kNumBuckets = 10;
00040   constexpr float kAnglePerBucket = M_PI / kNumBuckets;
00041   constexpr float kNoInitialRotation = 0.f;
00042   const Eigen::VectorXf histogram_no_initial_rotation =
00043       Eigen::VectorXf::Unit(kNumBuckets, 3);
00044   RotationalScanMatcher matcher(&histogram_no_initial_rotation);
00045   for (float t = 0.f; t < 1.f; t += 0.1f) {
00046     // 't' is the fraction of overlap and we have to divide by the norm of the
00047     // histogram to get the expected score.
00048     const float expected_score = t / std::hypot(t, 1 - t);
00049     // We rotate the 't'-th fraction of a bucket into the matcher's histogram.
00050     auto scores = matcher.Match(Eigen::VectorXf::Unit(kNumBuckets, 2),
00051                                 kNoInitialRotation, {t * kAnglePerBucket});
00052     ASSERT_EQ(1, scores.size());
00053     EXPECT_NEAR(expected_score, scores[0], 1e-6);
00054     // Also verify rotating out of a bucket.
00055     scores = matcher.Match(Eigen::VectorXf::Unit(kNumBuckets, 2),
00056                            kNoInitialRotation, {(2 - t) * kAnglePerBucket});
00057     ASSERT_EQ(1, scores.size());
00058     EXPECT_NEAR(expected_score, scores[0], 1e-6);
00059     // And into and out of a bucket with negative angle.
00060     scores =
00061         matcher.Match(Eigen::VectorXf::Unit(kNumBuckets, 4), kNoInitialRotation,
00062                       {-t * kAnglePerBucket, (t - 2) * kAnglePerBucket});
00063     ASSERT_EQ(2, scores.size());
00064     EXPECT_NEAR(expected_score, scores[0], 1e-6);
00065     EXPECT_NEAR(expected_score, scores[1], 1e-6);
00066   }
00067 }
00068 
00069 }  // namespace
00070 }  // namespace scan_matching
00071 }  // namespace mapping
00072 }  // namespace cartographer