12345qiupeng
/
cartographer
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Add Normal Estimation (#1213) 

Adds Normal Estimation needed for TSDF RangeDataInserter.
master
Kevin Daun 2018-06-26 14:22:16 +02:00 committed by Wally B. Feed
parent 2bd987ffb4
commit b4594bcdbd
4 changed files with 318 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

110
cartographer/mapping/internal/2d/normal_estimation_2d.cc Normal file
View File

@ -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<float> EstimateNormals(
const sensor::RangeData& range_data,
const proto::NormalEstimationOptions2D& normal_estimation_options) {
std::vector<float> 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

42
cartographer/mapping/internal/2d/normal_estimation_2d.h Normal file
View File

@ -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 <vector>
#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<float> EstimateNormals(
const sensor::RangeData& range_data,
const proto::NormalEstimationOptions2D& normal_estimation_options);
} // namespace mapping
} // namespace cartographer
#endif // CARTOGRAPHER_MAPPING_INTERNAL_NORMAL_ESTIMATION_2D_H_

144
cartographer/mapping/internal/2d/normal_estimation_2d_test.cc Normal file
View File

@ -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<double>(angle_idx) /
static_cast<double>(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<float> 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<float> 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<int> {};
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<double>(angle_idx) /
static_cast<double>(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<float> normals;
normals = EstimateNormals(range_data, options);
for (size_t angle_idx = 0; angle_idx < num_angles; ++angle_idx) {
const double angle = static_cast<double>(angle_idx) /
static_cast<double>(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

22
cartographer/mapping/proto/2d/normal_estimation_options_2d.proto Normal file
View File

@ -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;
}