00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017 #include "cartographer/mapping/internal/3d/scan_matching/real_time_correlative_scan_matcher_3d.h"
00018
00019 #include <memory>
00020
00021 #include "Eigen/Core"
00022 #include "cartographer/common/lua_parameter_dictionary_test_helpers.h"
00023 #include "cartographer/mapping/3d/hybrid_grid.h"
00024 #include "cartographer/mapping/internal/scan_matching/real_time_correlative_scan_matcher.h"
00025 #include "cartographer/sensor/point_cloud.h"
00026 #include "cartographer/transform/rigid_transform.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 RealTimeCorrelativeScanMatcher3DTest : public ::testing::Test {
00037 protected:
00038 RealTimeCorrelativeScanMatcher3DTest()
00039 : hybrid_grid_(0.1f),
00040 expected_pose_(Eigen::Vector3d(-1., 0., 0.),
00041 Eigen::Quaterniond::Identity()) {
00042 for (const Eigen::Vector3f& point :
00043 {Eigen::Vector3f(-3.f, 2.f, 0.f), Eigen::Vector3f(-4.f, 2.f, 0.f),
00044 Eigen::Vector3f(-5.f, 2.f, 0.f), Eigen::Vector3f(-6.f, 2.f, 0.f),
00045 Eigen::Vector3f(-6.f, 3.f, 1.f), Eigen::Vector3f(-6.f, 4.f, 2.f),
00046 Eigen::Vector3f(-7.f, 3.f, 1.f)}) {
00047 point_cloud_.push_back({point});
00048 hybrid_grid_.SetProbability(
00049 hybrid_grid_.GetCellIndex(expected_pose_.cast<float>() * point), 1.);
00050 }
00051
00052 auto parameter_dictionary = common::MakeDictionary(R"text(
00053 return {
00054 linear_search_window = 0.3,
00055 angular_search_window = math.rad(1.),
00056 translation_delta_cost_weight = 1e-1,
00057 rotation_delta_cost_weight = 1.,
00058 })text");
00059 real_time_correlative_scan_matcher_.reset(
00060 new RealTimeCorrelativeScanMatcher3D(
00061 CreateRealTimeCorrelativeScanMatcherOptions(
00062 parameter_dictionary.get())));
00063 }
00064
00065 void TestFromInitialPose(const transform::Rigid3d& initial_pose) {
00066 transform::Rigid3d pose;
00067
00068 const float score = real_time_correlative_scan_matcher_->Match(
00069 initial_pose, point_cloud_, hybrid_grid_, &pose);
00070 LOG(INFO) << "Score: " << score;
00071 EXPECT_THAT(pose, transform::IsNearly(expected_pose_, 1e-3));
00072 }
00073
00074 HybridGrid hybrid_grid_;
00075 transform::Rigid3d expected_pose_;
00076 sensor::PointCloud point_cloud_;
00077 std::unique_ptr<RealTimeCorrelativeScanMatcher3D>
00078 real_time_correlative_scan_matcher_;
00079 };
00080
00081 TEST_F(RealTimeCorrelativeScanMatcher3DTest, PerfectEstimate) {
00082 TestFromInitialPose(
00083 transform::Rigid3d::Translation(Eigen::Vector3d(-1., 0., 0.)));
00084 }
00085
00086 TEST_F(RealTimeCorrelativeScanMatcher3DTest, AlongX) {
00087 TestFromInitialPose(
00088 transform::Rigid3d::Translation(Eigen::Vector3d(-0.8, 0., 0.)));
00089 }
00090
00091 TEST_F(RealTimeCorrelativeScanMatcher3DTest, AlongZ) {
00092 TestFromInitialPose(
00093 transform::Rigid3d::Translation(Eigen::Vector3d(-1., 0., -0.2)));
00094 }
00095
00096 TEST_F(RealTimeCorrelativeScanMatcher3DTest, AlongXYZ) {
00097 TestFromInitialPose(
00098 transform::Rigid3d::Translation(Eigen::Vector3d(-0.9, -0.2, 0.2)));
00099 }
00100
00101 TEST_F(RealTimeCorrelativeScanMatcher3DTest, RotationAroundX) {
00102 TestFromInitialPose(transform::Rigid3d(
00103 Eigen::Vector3d(-1., 0., 0.),
00104 Eigen::AngleAxisd(0.8 / 180. * M_PI, Eigen::Vector3d(1., 0., 0.))));
00105 }
00106
00107 TEST_F(RealTimeCorrelativeScanMatcher3DTest, RotationAroundY) {
00108 TestFromInitialPose(transform::Rigid3d(
00109 Eigen::Vector3d(-1., 0., 0.),
00110 Eigen::AngleAxisd(0.8 / 180. * M_PI, Eigen::Vector3d(0., 1., 0.))));
00111 }
00112
00113 TEST_F(RealTimeCorrelativeScanMatcher3DTest, RotationAroundYZ) {
00114 TestFromInitialPose(transform::Rigid3d(
00115 Eigen::Vector3d(-1., 0., 0.),
00116 Eigen::AngleAxisd(0.8 / 180. * M_PI, Eigen::Vector3d(0., 1., 1.))));
00117 }
00118
00119 }
00120 }
00121 }
00122 }