diff --git a/cartographer/mapping/internal/2d/normal_estimation_2d.cc b/cartographer/mapping/internal/2d/normal_estimation_2d.cc new file mode 100644 index 0000000..de5550b --- /dev/null +++ b/cartographer/mapping/internal/2d/normal_estimation_2d.cc @@ -0,0 +1,110 @@ +/* + * Copyright 2018 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include "cartographer/mapping/internal/2d/normal_estimation_2d.h" + +namespace cartographer { +namespace mapping { +namespace { + +float NormalTo2DAngle(const Eigen::Vector3f& v) { + return std::atan2(v[1], v[0]); +} + +// Estimate the normal of an estimation_point as the arithmetic mean of the the +// normals of the vectors from estimation_point to each point in the +// sample_window. +float EstimateNormal(const sensor::PointCloud& returns, + const size_t estimation_point_index, + const size_t sample_window_begin, + const size_t sample_window_end, + const Eigen::Vector3f& sensor_origin) { + const Eigen::Vector3f& estimation_point = returns[estimation_point_index]; + if (sample_window_end - sample_window_begin < 2) { + return NormalTo2DAngle(sensor_origin - estimation_point); + } + Eigen::Vector3f mean_normal = Eigen::Vector3f::Zero(); + const Eigen::Vector3f& estimation_point_to_observation = + sensor_origin - estimation_point; + for (size_t sample_point_index = sample_window_begin; + sample_point_index < sample_window_end; ++sample_point_index) { + if (sample_point_index == estimation_point_index) continue; + const Eigen::Vector3f& sample_point = returns[sample_point_index]; + const Eigen::Vector3f& tangent = estimation_point - sample_point; + Eigen::Vector3f sample_normal = {-tangent[1], tangent[0], 0.f}; + constexpr float kMinNormalLength = 1e-6f; + if (sample_normal.norm() < kMinNormalLength) { + continue; + } + // Ensure sample_normal points towards 'sensor_origin'. + if (sample_normal.dot(estimation_point_to_observation) < 0) { + sample_normal = -sample_normal; + } + sample_normal.normalize(); + mean_normal += sample_normal; + } + return NormalTo2DAngle(mean_normal); +} +} // namespace + +proto::NormalEstimationOptions2D CreateNormalEstimationOptions2D( + common::LuaParameterDictionary* parameter_dictionary) { + proto::NormalEstimationOptions2D options; + options.set_num_normal_samples( + parameter_dictionary->GetInt("num_normal_samples")); + options.set_sample_radius(parameter_dictionary->GetDouble("sample_radius")); + CHECK_GT(options.num_normal_samples(), 0); + CHECK_GT(options.sample_radius(), 0.0); + return options; +} + +// Estimates the normal for each 'return' in 'range_data'. +// Assumes the angles in the range data returns are sorted with respect to +// the orientation of the vector from 'origin' to 'return'. +std::vector EstimateNormals( + const sensor::RangeData& range_data, + const proto::NormalEstimationOptions2D& normal_estimation_options) { + std::vector normals; + normals.reserve(range_data.returns.size()); + const size_t max_num_samples = normal_estimation_options.num_normal_samples(); + const float sample_radius = normal_estimation_options.sample_radius(); + for (size_t current_point = 0; current_point < range_data.returns.size(); + ++current_point) { + const Eigen::Vector3f& hit = range_data.returns[current_point]; + size_t sample_window_begin = current_point; + for (; sample_window_begin > 0 && + current_point - sample_window_begin < max_num_samples / 2 && + (hit - range_data.returns[sample_window_begin - 1]).norm() < + sample_radius; + --sample_window_begin) { + } + size_t sample_window_end = current_point; + for (; + sample_window_end < range_data.returns.size() && + sample_window_end - current_point < ceil(max_num_samples / 2.0) + 1 && + (hit - range_data.returns[sample_window_end]).norm() < sample_radius; + ++sample_window_end) { + } + const float normal_estimate = + EstimateNormal(range_data.returns, current_point, sample_window_begin, + sample_window_end, range_data.origin); + normals.push_back(normal_estimate); + } + return normals; +} + +} // namespace mapping +} // namespace cartographer diff --git a/cartographer/mapping/internal/2d/normal_estimation_2d.h b/cartographer/mapping/internal/2d/normal_estimation_2d.h new file mode 100644 index 0000000..f3d937d --- /dev/null +++ b/cartographer/mapping/internal/2d/normal_estimation_2d.h @@ -0,0 +1,42 @@ +/* + * Copyright 2018 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef CARTOGRAPHER_MAPPING_INTERNAL_NORMAL_ESTIMATION_2D_H_ +#define CARTOGRAPHER_MAPPING_INTERNAL_NORMAL_ESTIMATION_2D_H_ + +#include +#include "cartographer/mapping/proto/2d/normal_estimation_options_2d.pb.h" +#include "cartographer/sensor/point_cloud.h" +#include "cartographer/sensor/range_data.h" +#include "cartographer/transform/transform.h" + +namespace cartographer { +namespace mapping { + +proto::NormalEstimationOptions2D CreateNormalEstimationOptions2D( + common::LuaParameterDictionary* parameter_dictionary); + +// Estimates the normal for each 'return' in 'range_data'. +// Assumes the angles in the range data returns are sorted with respect to +// the orientation of the vector from 'origin' to 'return'. +std::vector EstimateNormals( + const sensor::RangeData& range_data, + const proto::NormalEstimationOptions2D& normal_estimation_options); + +} // namespace mapping +} // namespace cartographer + +#endif // CARTOGRAPHER_MAPPING_INTERNAL_NORMAL_ESTIMATION_2D_H_ diff --git a/cartographer/mapping/internal/2d/normal_estimation_2d_test.cc b/cartographer/mapping/internal/2d/normal_estimation_2d_test.cc new file mode 100644 index 0000000..889d6a2 --- /dev/null +++ b/cartographer/mapping/internal/2d/normal_estimation_2d_test.cc @@ -0,0 +1,144 @@ +/* + * Copyright 2018 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include "cartographer/mapping/internal/2d/normal_estimation_2d.h" + +#include "cartographer/common/lua_parameter_dictionary.h" +#include "cartographer/common/lua_parameter_dictionary_test_helpers.h" +#include "cartographer/common/math.h" +#include "gmock/gmock.h" +#include "gtest/gtest.h" + +namespace cartographer { +namespace mapping { +namespace { + +using ::testing::TestWithParam; +using ::testing::Values; + +TEST(NormalEstimation2DTest, SinglePoint) { + const auto parameter_dictionary = common::MakeDictionary( + "return { " + "num_normal_samples = 2, " + "sample_radius = 10.0, " + "}"); + const proto::NormalEstimationOptions2D options = + CreateNormalEstimationOptions2D(parameter_dictionary.get()); + sensor::RangeData range_data; + const size_t num_angles = 100; + range_data.origin.x() = 0.f; + range_data.origin.y() = 0.f; + for (size_t angle_idx = 0; angle_idx < num_angles; ++angle_idx) { + const double angle = static_cast(angle_idx) / + static_cast(num_angles) * 2. * M_PI - + M_PI; + range_data.returns.clear(); + range_data.returns.emplace_back(std::cos(angle), std::sin(angle), 0.f); + std::vector normals; + normals = EstimateNormals(range_data, options); + EXPECT_NEAR(common::NormalizeAngleDifference(angle - normals[0] - M_PI), + 0.0, 2.0 * M_PI / num_angles + 1e-4); + } +} + +TEST(NormalEstimation2DTest, StraightLineGeometry) { + const auto parameter_dictionary = common::MakeDictionary( + "return { " + "num_normal_samples = 2, " + "sample_radius = 10.0, " + "}"); + const proto::NormalEstimationOptions2D options = + CreateNormalEstimationOptions2D(parameter_dictionary.get()); + sensor::RangeData range_data; + range_data.returns.emplace_back(-1.f, 1.f, 0.f); + range_data.returns.emplace_back(0.f, 1.f, 0.f); + range_data.returns.emplace_back(1.f, 1.f, 0.f); + range_data.origin.x() = 0.f; + range_data.origin.y() = 0.f; + std::vector normals; + normals = EstimateNormals(range_data, options); + for (const float normal : normals) { + EXPECT_NEAR(normal, -M_PI_2, 1e-4); + } + normals.clear(); + range_data.returns.clear(); + range_data.returns.emplace_back(1.f, 1.f, 0.f); + range_data.returns.emplace_back(1.f, 0.f, 0.f); + range_data.returns.emplace_back(1.f, -1.f, 0.f); + normals = EstimateNormals(range_data, options); + for (const float normal : normals) { + EXPECT_NEAR(std::abs(normal), M_PI, 1e-4); + } + + normals.clear(); + range_data.returns.clear(); + range_data.returns.emplace_back(1.f, -1.f, 0.f); + range_data.returns.emplace_back(0.f, -1.f, 0.f); + range_data.returns.emplace_back(-1.f, -1.f, 0.f); + normals = EstimateNormals(range_data, options); + for (const float normal : normals) { + EXPECT_NEAR(normal, M_PI_2, 1e-4); + } + + normals.clear(); + range_data.returns.clear(); + range_data.returns.emplace_back(-1.f, -1.f, 0.f); + range_data.returns.emplace_back(-1.f, 0.f, 0.f); + range_data.returns.emplace_back(-1.f, 1.f, 0.f); + normals = EstimateNormals(range_data, options); + for (const float normal : normals) { + EXPECT_NEAR(normal, 0, 1e-4); + } +} + +class CircularGeometry2DTest : public TestWithParam {}; + +TEST_P(CircularGeometry2DTest, NumSamplesPerNormal) { + const auto parameter_dictionary = common::MakeDictionary( + "return { " + "num_normal_samples = " + + std::to_string(GetParam()) + + ", " + "sample_radius = 10.0, " + "}"); + const proto::NormalEstimationOptions2D options = + CreateNormalEstimationOptions2D(parameter_dictionary.get()); + sensor::RangeData range_data; + const size_t num_angles = 100; + for (size_t angle_idx = 0; angle_idx < num_angles; ++angle_idx) { + const double angle = static_cast(angle_idx) / + static_cast(num_angles) * 2. * M_PI - + M_PI; + range_data.returns.emplace_back(std::cos(angle), std::sin(angle), 0.f); + } + range_data.origin.x() = 0.f; + range_data.origin.y() = 0.f; + std::vector normals; + normals = EstimateNormals(range_data, options); + for (size_t angle_idx = 0; angle_idx < num_angles; ++angle_idx) { + const double angle = static_cast(angle_idx) / + static_cast(num_angles) * 2. * M_PI; + EXPECT_NEAR(common::NormalizeAngleDifference(normals[angle_idx] - angle), + 0.0, 2.0 * M_PI / num_angles * GetParam() / 2.0 + 1e-4); + } +} + +INSTANTIATE_TEST_CASE_P(InstantiationName, CircularGeometry2DTest, + ::testing::Values(1, 2, 4, 5, 8)); + +} // namespace +} // namespace mapping +} // namespace cartographer diff --git a/cartographer/mapping/proto/2d/normal_estimation_options_2d.proto b/cartographer/mapping/proto/2d/normal_estimation_options_2d.proto new file mode 100644 index 0000000..dc88368 --- /dev/null +++ b/cartographer/mapping/proto/2d/normal_estimation_options_2d.proto @@ -0,0 +1,22 @@ +// Copyright 2018 The Cartographer Authors +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +syntax = "proto3"; + +package cartographer.mapping.proto; + +message NormalEstimationOptions2D { + int32 num_normal_samples = 1; + float sample_radius = 2; +}