00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017 #include "cartographer/mapping/internal/2d/normal_estimation_2d.h"
00018
00019 #include "cartographer/common/lua_parameter_dictionary.h"
00020 #include "cartographer/common/lua_parameter_dictionary_test_helpers.h"
00021 #include "cartographer/common/math.h"
00022 #include "gmock/gmock.h"
00023 #include "gtest/gtest.h"
00024
00025 namespace cartographer {
00026 namespace mapping {
00027 namespace {
00028
00029 using ::testing::TestWithParam;
00030 using ::testing::Values;
00031
00032 TEST(NormalEstimation2DTest, SinglePoint) {
00033 const auto parameter_dictionary = common::MakeDictionary(
00034 "return { "
00035 "num_normal_samples = 2, "
00036 "sample_radius = 10.0, "
00037 "}");
00038 const proto::NormalEstimationOptions2D options =
00039 CreateNormalEstimationOptions2D(parameter_dictionary.get());
00040 auto range_data = sensor::RangeData();
00041 const size_t num_angles = 100;
00042 range_data.origin.x() = 0.f;
00043 range_data.origin.y() = 0.f;
00044 for (size_t angle_idx = 0; angle_idx < num_angles; ++angle_idx) {
00045 const double angle = static_cast<double>(angle_idx) /
00046 static_cast<double>(num_angles) * 2. * M_PI -
00047 M_PI;
00048 range_data.returns.clear();
00049 range_data.returns.push_back(
00050 {Eigen::Vector3d{std::cos(angle), std::sin(angle), 0.}.cast<float>()});
00051 std::vector<float> normals;
00052 normals = EstimateNormals(range_data, options);
00053 EXPECT_NEAR(common::NormalizeAngleDifference(angle - normals[0] - M_PI),
00054 0.0, 2.0 * M_PI / num_angles + 1e-4);
00055 }
00056 }
00057
00058 TEST(NormalEstimation2DTest, StraightLineGeometry) {
00059 const auto parameter_dictionary = common::MakeDictionary(
00060 "return { "
00061 "num_normal_samples = 2, "
00062 "sample_radius = 10.0, "
00063 "}");
00064 const proto::NormalEstimationOptions2D options =
00065 CreateNormalEstimationOptions2D(parameter_dictionary.get());
00066 auto range_data = sensor::RangeData();
00067 range_data.returns.push_back({Eigen::Vector3f{-1.f, 1.f, 0.f}});
00068 range_data.returns.push_back({Eigen::Vector3f{0.f, 1.f, 0.f}});
00069 range_data.returns.push_back({Eigen::Vector3f{1.f, 1.f, 0.f}});
00070 range_data.origin.x() = 0.f;
00071 range_data.origin.y() = 0.f;
00072 std::vector<float> normals;
00073 normals = EstimateNormals(range_data, options);
00074 for (const float normal : normals) {
00075 EXPECT_NEAR(normal, -M_PI_2, 1e-4);
00076 }
00077 normals.clear();
00078 range_data.returns.clear();
00079 range_data.returns.push_back({Eigen::Vector3f{1.f, 1.f, 0.f}});
00080 range_data.returns.push_back({Eigen::Vector3f{1.f, 0.f, 0.f}});
00081 range_data.returns.push_back({Eigen::Vector3f{1.f, -1.f, 0.f}});
00082 normals = EstimateNormals(range_data, options);
00083 for (const float normal : normals) {
00084 EXPECT_NEAR(std::abs(normal), M_PI, 1e-4);
00085 }
00086
00087 normals.clear();
00088 range_data.returns.clear();
00089 range_data.returns.push_back({Eigen::Vector3f{1.f, -1.f, 0.f}});
00090 range_data.returns.push_back({Eigen::Vector3f{0.f, -1.f, 0.f}});
00091 range_data.returns.push_back({Eigen::Vector3f{-1.f, -1.f, 0.f}});
00092 normals = EstimateNormals(range_data, options);
00093 for (const float normal : normals) {
00094 EXPECT_NEAR(normal, M_PI_2, 1e-4);
00095 }
00096
00097 normals.clear();
00098 range_data.returns.clear();
00099 range_data.returns.push_back({Eigen::Vector3f{-1.f, -1.f, 0.f}});
00100 range_data.returns.push_back({Eigen::Vector3f{-1.f, 0.f, 0.f}});
00101 range_data.returns.push_back({Eigen::Vector3f{-1.f, 1.f, 0.f}});
00102 normals = EstimateNormals(range_data, options);
00103 for (const float normal : normals) {
00104 EXPECT_NEAR(normal, 0, 1e-4);
00105 }
00106 }
00107
00108 class CircularGeometry2DTest : public TestWithParam<int> {};
00109
00110 TEST_P(CircularGeometry2DTest, NumSamplesPerNormal) {
00111 const auto parameter_dictionary = common::MakeDictionary(
00112 "return { "
00113 "num_normal_samples = " +
00114 std::to_string(GetParam()) +
00115 ", "
00116 "sample_radius = 10.0, "
00117 "}");
00118 const proto::NormalEstimationOptions2D options =
00119 CreateNormalEstimationOptions2D(parameter_dictionary.get());
00120 auto range_data = sensor::RangeData();
00121 const size_t num_angles = 100;
00122 for (size_t angle_idx = 0; angle_idx < num_angles; ++angle_idx) {
00123 const double angle = static_cast<double>(angle_idx) /
00124 static_cast<double>(num_angles) * 2. * M_PI -
00125 M_PI;
00126 range_data.returns.push_back(
00127 {Eigen::Vector3d{std::cos(angle), std::sin(angle), 0.}.cast<float>()});
00128 }
00129 range_data.origin.x() = 0.f;
00130 range_data.origin.y() = 0.f;
00131 std::vector<float> normals;
00132 normals = EstimateNormals(range_data, options);
00133 for (size_t angle_idx = 0; angle_idx < num_angles; ++angle_idx) {
00134 const double angle = static_cast<double>(angle_idx) /
00135 static_cast<double>(num_angles) * 2. * M_PI;
00136 EXPECT_NEAR(common::NormalizeAngleDifference(normals[angle_idx] - angle),
00137 0.0, 2.0 * M_PI / num_angles * GetParam() / 2.0 + 1e-4);
00138 }
00139 }
00140
00141 INSTANTIATE_TEST_CASE_P(InstantiationName, CircularGeometry2DTest,
00142 ::testing::Values(1, 2, 4, 5, 8));
00143
00144 }
00145 }
00146 }