00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017 #include "cartographer/mapping/internal/3d/scan_matching/fast_correlative_scan_matcher_3d.h"
00018
00019 #include <algorithm>
00020 #include <cmath>
00021 #include <random>
00022 #include <string>
00023
00024 #include "absl/memory/memory.h"
00025 #include "cartographer/common/lua_parameter_dictionary_test_helpers.h"
00026 #include "cartographer/mapping/3d/range_data_inserter_3d.h"
00027 #include "cartographer/transform/rigid_transform_test_helpers.h"
00028 #include "cartographer/transform/transform.h"
00029 #include "gtest/gtest.h"
00030
00031 namespace cartographer {
00032 namespace mapping {
00033 namespace scan_matching {
00034 namespace {
00035
00036 class FastCorrelativeScanMatcher3DTest : public ::testing::Test {
00037 protected:
00038 FastCorrelativeScanMatcher3DTest()
00039 : range_data_inserter_(CreateRangeDataInserterTestOptions3D()),
00040 options_(CreateFastCorrelativeScanMatcher3DTestOptions3D(6)) {}
00041
00042 void SetUp() override {
00043 point_cloud_ = {
00044 {Eigen::Vector3f(4.f, 0.f, 0.f)}, {Eigen::Vector3f(4.5f, 0.f, 0.f)},
00045 {Eigen::Vector3f(5.f, 0.f, 0.f)}, {Eigen::Vector3f(5.5f, 0.f, 0.f)},
00046 {Eigen::Vector3f(0.f, 4.f, 0.f)}, {Eigen::Vector3f(0.f, 4.5f, 0.f)},
00047 {Eigen::Vector3f(0.f, 5.f, 0.f)}, {Eigen::Vector3f(0.f, 5.5f, 0.f)},
00048 {Eigen::Vector3f(0.f, 0.f, 4.f)}, {Eigen::Vector3f(0.f, 0.f, 4.5f)},
00049 {Eigen::Vector3f(0.f, 0.f, 5.f)}, {Eigen::Vector3f(0.f, 0.f, 5.5f)}};
00050 }
00051
00052 transform::Rigid3f GetRandomPose() {
00053 const float x = 0.7f * distribution_(prng_);
00054 const float y = 0.7f * distribution_(prng_);
00055 const float z = 0.7f * distribution_(prng_);
00056 const float theta = 0.2f * distribution_(prng_);
00057 return transform::Rigid3f::Translation(Eigen::Vector3f(x, y, z)) *
00058 transform::Rigid3f::Rotation(
00059 Eigen::AngleAxisf(theta, Eigen::Vector3f::UnitZ()));
00060 }
00061
00062 static proto::FastCorrelativeScanMatcherOptions3D
00063 CreateFastCorrelativeScanMatcher3DTestOptions3D(
00064 const int branch_and_bound_depth) {
00065 auto parameter_dictionary = common::MakeDictionary(
00066 "return {"
00067 "branch_and_bound_depth = " +
00068 std::to_string(branch_and_bound_depth) +
00069 ", "
00070 "full_resolution_depth = " +
00071 std::to_string(branch_and_bound_depth) +
00072 ", "
00073 "min_rotational_score = 0.1, "
00074
00075
00076 "min_low_resolution_score = 0.15, "
00077 "linear_xy_search_window = 0.8, "
00078 "linear_z_search_window = 0.8, "
00079 "angular_search_window = 0.3, "
00080 "}");
00081 return CreateFastCorrelativeScanMatcherOptions3D(
00082 parameter_dictionary.get());
00083 }
00084
00085 static mapping::proto::RangeDataInserterOptions3D
00086 CreateRangeDataInserterTestOptions3D() {
00087 auto parameter_dictionary = common::MakeDictionary(
00088 "return { "
00089 "hit_probability = 0.7, "
00090 "miss_probability = 0.4, "
00091 "num_free_space_voxels = 5, "
00092 "}");
00093 return CreateRangeDataInserterOptions3D(parameter_dictionary.get());
00094 }
00095
00096 std::unique_ptr<FastCorrelativeScanMatcher3D> GetFastCorrelativeScanMatcher(
00097 const proto::FastCorrelativeScanMatcherOptions3D& options,
00098 const transform::Rigid3f& pose) {
00099 hybrid_grid_ = absl::make_unique<HybridGrid>(0.05f);
00100 range_data_inserter_.Insert(
00101 sensor::RangeData{pose.translation(),
00102 sensor::TransformPointCloud(point_cloud_, pose),
00103 {}},
00104 hybrid_grid_.get());
00105 hybrid_grid_->FinishUpdate();
00106
00107 return absl::make_unique<FastCorrelativeScanMatcher3D>(
00108 *hybrid_grid_, hybrid_grid_.get(), &GetRotationalScanMatcherHistogram(),
00109 options);
00110 }
00111
00112 TrajectoryNode::Data CreateConstantData(
00113 const sensor::PointCloud& low_resolution_point_cloud) {
00114 return TrajectoryNode::Data{common::FromUniversal(0),
00115 Eigen::Quaterniond::Identity(),
00116 {},
00117 point_cloud_,
00118 low_resolution_point_cloud,
00119 GetRotationalScanMatcherHistogram()};
00120 }
00121
00122 const Eigen::VectorXf& GetRotationalScanMatcherHistogram() {
00123 return rotational_scan_matcher_histogram_;
00124 }
00125
00126 std::mt19937 prng_ = std::mt19937(42);
00127 std::uniform_real_distribution<float> distribution_ =
00128 std::uniform_real_distribution<float>(-1.f, 1.f);
00129 RangeDataInserter3D range_data_inserter_;
00130 const proto::FastCorrelativeScanMatcherOptions3D options_;
00131 sensor::PointCloud point_cloud_;
00132 std::unique_ptr<HybridGrid> hybrid_grid_;
00133 const Eigen::VectorXf rotational_scan_matcher_histogram_ =
00134 Eigen::VectorXf::Zero(10);
00135 };
00136
00137 constexpr float kMinScore = 0.1f;
00138
00139 TEST_F(FastCorrelativeScanMatcher3DTest, CorrectPoseForMatch) {
00140 for (int i = 0; i != 20; ++i) {
00141 const auto expected_pose = GetRandomPose();
00142
00143 std::unique_ptr<FastCorrelativeScanMatcher3D> fast_correlative_scan_matcher(
00144 GetFastCorrelativeScanMatcher(options_, expected_pose));
00145
00146 const std::unique_ptr<FastCorrelativeScanMatcher3D::Result> result =
00147 fast_correlative_scan_matcher->Match(
00148 transform::Rigid3d::Identity(), transform::Rigid3d::Identity(),
00149 CreateConstantData(point_cloud_), kMinScore);
00150 EXPECT_THAT(result, testing::NotNull());
00151 EXPECT_LT(kMinScore, result->score);
00152 EXPECT_LT(0.09f, result->rotational_score);
00153 EXPECT_LT(0.14f, result->low_resolution_score);
00154 EXPECT_THAT(expected_pose,
00155 transform::IsNearly(result->pose_estimate.cast<float>(), 0.05f))
00156 << "Actual: " << transform::ToProto(result->pose_estimate).DebugString()
00157 << "\nExpected: " << transform::ToProto(expected_pose).DebugString();
00158
00159 const std::unique_ptr<FastCorrelativeScanMatcher3D::Result>
00160 low_resolution_result = fast_correlative_scan_matcher->Match(
00161 transform::Rigid3d::Identity(), transform::Rigid3d::Identity(),
00162 CreateConstantData({{Eigen::Vector3f(42.f, 42.f, 42.f)}}),
00163 kMinScore);
00164 EXPECT_THAT(low_resolution_result, testing::IsNull())
00165 << low_resolution_result->low_resolution_score;
00166 }
00167 }
00168
00169 TEST_F(FastCorrelativeScanMatcher3DTest, CorrectPoseForMatchFullSubmap) {
00170 const auto expected_pose = GetRandomPose();
00171
00172 std::unique_ptr<FastCorrelativeScanMatcher3D> fast_correlative_scan_matcher(
00173 GetFastCorrelativeScanMatcher(options_, expected_pose));
00174
00175 const std::unique_ptr<FastCorrelativeScanMatcher3D::Result> result =
00176 fast_correlative_scan_matcher->MatchFullSubmap(
00177 Eigen::Quaterniond::Identity(), Eigen::Quaterniond::Identity(),
00178 CreateConstantData(point_cloud_), kMinScore);
00179 EXPECT_THAT(result, testing::NotNull());
00180 EXPECT_LT(kMinScore, result->score);
00181 EXPECT_LT(0.09f, result->rotational_score);
00182 EXPECT_LT(0.14f, result->low_resolution_score);
00183 EXPECT_THAT(expected_pose,
00184 transform::IsNearly(result->pose_estimate.cast<float>(), 0.05f))
00185 << "Actual: " << transform::ToProto(result->pose_estimate).DebugString()
00186 << "\nExpected: " << transform::ToProto(expected_pose).DebugString();
00187
00188 const std::unique_ptr<FastCorrelativeScanMatcher3D::Result>
00189 low_resolution_result = fast_correlative_scan_matcher->MatchFullSubmap(
00190 Eigen::Quaterniond::Identity(), Eigen::Quaterniond::Identity(),
00191 CreateConstantData({{Eigen::Vector3f(42.f, 42.f, 42.f)}}), kMinScore);
00192 EXPECT_THAT(low_resolution_result, testing::IsNull())
00193 << low_resolution_result->low_resolution_score;
00194 }
00195
00196 }
00197 }
00198 }
00199 }