Add TSDF RangeDataInserter (#1236)

Adds TSDF RangeDataInserter, tests and integrates the configuration files.
master
Kevin Daun 2018-07-09 13:46:46 +02:00 committed by Wally B. Feed
parent 3bf9ba0a69
commit 7dfe404278
13 changed files with 752 additions and 7 deletions

View File

@ -75,6 +75,12 @@ class MapLimits {
common::RoundToInt((max_.x() - point.x()) / resolution_ - 0.5));
}
// Returns the center of the cell at 'cell_index'.
Eigen::Vector2f GetCellCenter(const Eigen::Array2i cell_index) const {
return {max_.x() - resolution() * (cell_index[1] + 0.5),
max_.y() - resolution() * (cell_index[0] + 0.5)};
}
// Returns true if the ProbabilityGrid contains 'cell_index'.
bool Contains(const Eigen::Array2i& cell_index) const {
return (Eigen::Array2i(0, 0) <= cell_index).all() &&

View File

@ -35,6 +35,7 @@ constexpr int kSubpixelScale = 1000;
void GrowAsNeeded(const sensor::RangeData& range_data,
ProbabilityGrid* const probability_grid) {
Eigen::AlignedBox2f bounding_box(range_data.origin.head<2>());
// Padding around bounding box to avoid numerical issues at cell boundaries.
constexpr float kPadding = 1e-6f;
for (const Eigen::Vector3f& hit : range_data.returns) {
bounding_box.extend(hit.head<2>());

View File

@ -50,6 +50,19 @@ TEST(Submap2DTest, TheRightNumberOfRangeDataAreInserted) {
"hit_probability = 0.53, "
"miss_probability = 0.495, "
"},"
"tsdf_range_data_inserter = { "
"truncation_distance = 2.0,"
"maximum_weight = 10.,"
"update_free_space = false,"
"normal_estimation_options = {"
"num_normal_samples = 2,"
"sample_radius = 10.,"
"},"
"project_sdf_distance_to_scan_normal = false,"
"update_weight_range_exponent = 0,"
"update_weight_angle_scan_normal_to_ray_kernel_bandwith = 0,"
"update_weight_distance_cell_to_hit_kernel_bandwith = 0,"
"},"
"},"
"}");
ActiveSubmaps2D submaps{CreateSubmapsOptions2D(parameter_dictionary.get())};

View File

@ -16,10 +16,6 @@
#include "cartographer/mapping/2d/tsdf_2d.h"
#include <limits>
#include "cartographer/common/make_unique.h"
namespace cartographer {
namespace mapping {
@ -43,11 +39,16 @@ TSDF2D::TSDF2D(const proto::Grid2D& proto) : Grid2D(proto) {
}
}
bool TSDF2D::CellIsUpdated(const Eigen::Array2i& cell_index) const {
const int flat_index = ToFlatIndex(cell_index);
uint16 tsdf_cell = correspondence_cost_cells()[flat_index];
return tsdf_cell >= value_converter_->getUpdateMarker();
}
void TSDF2D::SetCell(const Eigen::Array2i& cell_index, float tsd,
float weight) {
const int flat_index = ToFlatIndex(cell_index);
uint16* tsdf_cell = &(*mutable_correspondence_cost_cells())[flat_index];
uint16* weight_cell = &weight_cells_[flat_index];
if (*tsdf_cell >= value_converter_->getUpdateMarker()) {
return;
}
@ -55,8 +56,8 @@ void TSDF2D::SetCell(const Eigen::Array2i& cell_index, float tsd,
mutable_known_cells_box()->extend(cell_index.matrix());
*tsdf_cell =
value_converter_->TSDToValue(tsd) + value_converter_->getUpdateMarker();
uint16* weight_cell = &weight_cells_[flat_index];
*weight_cell = value_converter_->WeightToValue(weight);
return;
}
float TSDF2D::GetTSD(const Eigen::Array2i& cell_index) const {

View File

@ -46,6 +46,7 @@ class TSDF2D : public Grid2D {
virtual bool DrawToSubmapTexture(
proto::SubmapQuery::Response::SubmapTexture* const texture,
transform::Rigid3d local_pose) const override;
bool CellIsUpdated(const Eigen::Array2i& cell_index) const;
private:
std::unique_ptr<TSDValueConverter> value_converter_;

View File

@ -0,0 +1,234 @@
/*
* 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/2d/tsdf_range_data_inserter_2d.h"
#include "cartographer/mapping/internal/2d/normal_estimation_2d.h"
#include "cartographer/mapping/internal/2d/ray_to_pixel_mask.h"
namespace cartographer {
namespace mapping {
namespace {
// Factor for subpixel accuracy of start and end point for ray casts.
constexpr int kSubpixelScale = 1000;
// Minimum distance between range observation and origin. Otherwise, range
// observations are discarded.
constexpr float kMinRangeMeters = 1e-6f;
constexpr float kSqrtTwoPi = std::sqrt(2.0 * M_PI);
void GrowAsNeeded(const sensor::RangeData& range_data,
const float truncation_distance, TSDF2D* const tsdf) {
Eigen::AlignedBox2f bounding_box(range_data.origin.head<2>());
for (const Eigen::Vector3f& hit : range_data.returns) {
const Eigen::Vector3f direction = (hit - range_data.origin).normalized();
const Eigen::Vector3f end_position = hit + truncation_distance * direction;
bounding_box.extend(end_position.head<2>());
}
// Padding around bounding box to avoid numerical issues at cell boundaries.
constexpr float kPadding = 1e-6f;
tsdf->GrowLimits(bounding_box.min() - kPadding * Eigen::Vector2f::Ones());
tsdf->GrowLimits(bounding_box.max() + kPadding * Eigen::Vector2f::Ones());
}
float GaussianKernel(const float x, const float sigma) {
return 1.0 / (kSqrtTwoPi * sigma) * std::exp(-0.5 * x * x / (sigma * sigma));
}
std::pair<Eigen::Array2i, Eigen::Array2i> SuperscaleRay(
const Eigen::Vector2f& begin, const Eigen::Vector2f& end,
TSDF2D* const tsdf) {
const MapLimits& limits = tsdf->limits();
const double superscaled_resolution = limits.resolution() / kSubpixelScale;
const MapLimits superscaled_limits(
superscaled_resolution, limits.max(),
CellLimits(limits.cell_limits().num_x_cells * kSubpixelScale,
limits.cell_limits().num_y_cells * kSubpixelScale));
const Eigen::Array2i superscaled_begin =
superscaled_limits.GetCellIndex(begin);
const Eigen::Array2i superscaled_end = superscaled_limits.GetCellIndex(end);
return std::make_pair(superscaled_begin, superscaled_end);
}
struct RangeDataSorter {
RangeDataSorter(Eigen::Vector3f origin) { origin_ = origin.head<2>(); }
bool operator()(Eigen::Vector3f lhs, Eigen::Vector3f rhs) {
const Eigen::Vector2f delta_lhs = (lhs.head<2>() - origin_).normalized();
const Eigen::Vector2f delta_rhs = (lhs.head<2>() - origin_).normalized();
if ((delta_lhs[1] < 0.f) != (delta_rhs[1] < 0.f)) {
return delta_lhs[1] < 0.f;
} else if (delta_lhs[1] < 0.f) {
return delta_lhs[0] < delta_rhs[0];
} else {
return delta_lhs[0] > delta_rhs[0];
}
}
private:
Eigen::Vector2f origin_;
};
float ComputeRangeWeightFactor(float range, int exponent) {
float weight = 0.f;
if (std::abs(range) > kMinRangeMeters) {
weight = 1.f / (std::pow(range, exponent));
}
return weight;
}
} // namespace
proto::TSDFRangeDataInserterOptions2D CreateTSDFRangeDataInserterOptions2D(
common::LuaParameterDictionary* parameter_dictionary) {
proto::TSDFRangeDataInserterOptions2D options;
options.set_truncation_distance(
parameter_dictionary->GetDouble("truncation_distance"));
options.set_maximum_weight(parameter_dictionary->GetDouble("maximum_weight"));
options.set_update_free_space(
parameter_dictionary->GetBool("update_free_space"));
*options
.mutable_normal_estimation_options() = CreateNormalEstimationOptions2D(
parameter_dictionary->GetDictionary("normal_estimation_options").get());
options.set_project_sdf_distance_to_scan_normal(
parameter_dictionary->GetBool("project_sdf_distance_to_scan_normal"));
options.set_update_weight_range_exponent(
parameter_dictionary->GetInt("update_weight_range_exponent"));
options.set_update_weight_angle_scan_normal_to_ray_kernel_bandwith(
parameter_dictionary->GetDouble(
"update_weight_angle_scan_normal_to_ray_kernel_bandwith"));
options.set_update_weight_distance_cell_to_hit_kernel_bandwith(
parameter_dictionary->GetDouble(
"update_weight_distance_cell_to_hit_kernel_bandwith"));
return options;
}
TSDFRangeDataInserter2D::TSDFRangeDataInserter2D(
const proto::TSDFRangeDataInserterOptions2D& options)
: options_(options) {}
// Casts a ray from origin towards hit for each hit in range data.
// If 'options.update_free_space' is 'true', all cells along the ray
// until 'truncation_distance' behind hit are updated. Otherwise, only the cells
// within 'truncation_distance' around hit are updated.
void TSDFRangeDataInserter2D::Insert(const sensor::RangeData& range_data,
GridInterface* grid) const {
const float truncation_distance =
static_cast<float>(options_.truncation_distance());
TSDF2D* tsdf = static_cast<TSDF2D*>(grid);
GrowAsNeeded(range_data, truncation_distance, tsdf);
// Compute normals if needed.
bool scale_update_weight_angle_scan_normal_to_ray =
options_.update_weight_angle_scan_normal_to_ray_kernel_bandwith() != 0.f;
sensor::RangeData sorted_range_data = range_data;
std::vector<float> normals;
if (options_.project_sdf_distance_to_scan_normal() ||
scale_update_weight_angle_scan_normal_to_ray) {
std::sort(sorted_range_data.returns.begin(),
sorted_range_data.returns.end(),
RangeDataSorter(sorted_range_data.origin));
normals = EstimateNormals(sorted_range_data,
options_.normal_estimation_options());
}
const Eigen::Vector2f origin = sorted_range_data.origin.head<2>();
for (size_t hit_index = 0; hit_index < sorted_range_data.returns.size();
++hit_index) {
const Eigen::Vector2f hit = sorted_range_data.returns[hit_index].head<2>();
const float normal = normals.empty()
? std::numeric_limits<float>::quiet_NaN()
: normals[hit_index];
InsertHit(options_, hit, origin, normal, tsdf);
}
tsdf->FinishUpdate();
}
void TSDFRangeDataInserter2D::InsertHit(
const proto::TSDFRangeDataInserterOptions2D& options,
const Eigen::Vector2f& hit, const Eigen::Vector2f& origin, float normal,
TSDF2D* tsdf) const {
const Eigen::Vector2f ray = hit - origin;
const float range = ray.norm();
const float truncation_distance =
static_cast<float>(options_.truncation_distance());
if (range < truncation_distance) return;
const float truncation_ratio = truncation_distance / range;
const Eigen::Vector2f ray_begin =
options_.update_free_space() ? origin
: origin + (1.0f - truncation_ratio) * ray;
const Eigen::Vector2f ray_end = origin + (1.0f + truncation_ratio) * ray;
std::pair<Eigen::Array2i, Eigen::Array2i> superscaled_ray =
SuperscaleRay(ray_begin, ray_end, tsdf);
std::vector<Eigen::Array2i> ray_mask = RayToPixelMask(
superscaled_ray.first, superscaled_ray.second, kSubpixelScale);
// Precompute weight factors.
float weight_factor_angle_ray_normal = 1.f;
if (options_.update_weight_angle_scan_normal_to_ray_kernel_bandwith() !=
0.f) {
const Eigen::Vector2f negative_ray = -ray;
float angle_ray_normal =
common::NormalizeAngleDifference(normal - common::atan2(negative_ray));
weight_factor_angle_ray_normal = GaussianKernel(
angle_ray_normal,
options_.update_weight_angle_scan_normal_to_ray_kernel_bandwith());
}
float weight_factor_range = 1.f;
if (options_.update_weight_range_exponent() != 0) {
weight_factor_range = ComputeRangeWeightFactor(
range, options_.update_weight_range_exponent());
}
// Update Cells.
for (const Eigen::Array2i& cell_index : ray_mask) {
if (tsdf->CellIsUpdated(cell_index)) continue;
Eigen::Vector2f cell_center = tsdf->limits().GetCellCenter(cell_index);
float distance_cell_to_origin = (cell_center - origin).norm();
float update_tsd = range - distance_cell_to_origin;
if (options_.project_sdf_distance_to_scan_normal()) {
float normal_orientation = normal;
update_tsd = (cell_center - hit)
.dot(Eigen::Vector2f{std::cos(normal_orientation),
std::sin(normal_orientation)});
}
update_tsd =
common::Clamp(update_tsd, -truncation_distance, truncation_distance);
float update_weight = weight_factor_range * weight_factor_angle_ray_normal;
if (options_.update_weight_distance_cell_to_hit_kernel_bandwith() != 0.f) {
update_weight *= GaussianKernel(
update_tsd,
options_.update_weight_distance_cell_to_hit_kernel_bandwith());
}
UpdateCell(cell_index, update_tsd, update_weight, tsdf);
}
}
void TSDFRangeDataInserter2D::UpdateCell(const Eigen::Array2i& cell,
float update_sdf, float update_weight,
TSDF2D* tsdf) const {
if (update_weight == 0.f) return;
const std::pair<float, float> tsd_and_weight = tsdf->GetTSDAndWeight(cell);
float updated_weight = tsd_and_weight.second + update_weight;
float updated_sdf = (tsd_and_weight.first * tsd_and_weight.second +
update_sdf * update_weight) /
updated_weight;
updated_weight =
std::min(updated_weight, static_cast<float>(options_.maximum_weight()));
tsdf->SetCell(cell, updated_sdf, updated_weight);
}
} // namespace mapping
} // namespace cartographer

View File

@ -0,0 +1,60 @@
/*
* 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_2D_TSDF_RANGE_DATA_INSERTER_2D_H_
#define CARTOGRAPHER_MAPPING_2D_TSDF_RANGE_DATA_INSERTER_2D_H_
#include "cartographer/common/lua_parameter_dictionary.h"
#include "cartographer/mapping/2d/tsdf_2d.h"
#include "cartographer/mapping/proto/2d/tsdf_range_data_inserter_options_2d.pb.h"
#include "cartographer/mapping/range_data_inserter_interface.h"
#include "cartographer/sensor/point_cloud.h"
#include "cartographer/sensor/range_data.h"
namespace cartographer {
namespace mapping {
proto::TSDFRangeDataInserterOptions2D CreateTSDFRangeDataInserterOptions2D(
common::LuaParameterDictionary* parameter_dictionary);
class TSDFRangeDataInserter2D : public RangeDataInserterInterface {
public:
explicit TSDFRangeDataInserter2D(
const proto::TSDFRangeDataInserterOptions2D& options);
TSDFRangeDataInserter2D(const TSDFRangeDataInserter2D&) = delete;
TSDFRangeDataInserter2D& operator=(const TSDFRangeDataInserter2D&) = delete;
// Casts a ray from origin towards hit for each hit in range data.
// If 'options.update_free_space' is 'true', all cells along the ray
// until 'truncation_distance' behind hit are updated. Otherwise, only the
// cells within 'truncation_distance' around hit are updated.
virtual void Insert(const sensor::RangeData& range_data,
GridInterface* grid) const override;
private:
void InsertHit(const proto::TSDFRangeDataInserterOptions2D& options,
const Eigen::Vector2f& hit, const Eigen::Vector2f& origin,
float normal, TSDF2D* tsdf) const;
void UpdateCell(const Eigen::Array2i& cell, float update_sdf,
float update_weight, TSDF2D* tsdf) const;
const proto::TSDFRangeDataInserterOptions2D options_;
};
} // namespace mapping
} // namespace cartographer
#endif // CARTOGRAPHER_MAPPING_2D_TSDF_RANGE_DATA_INSERTER_2D_H_

View File

@ -0,0 +1,348 @@
/*
* 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/2d/tsdf_range_data_inserter_2d.h"
#include "cartographer/common/lua_parameter_dictionary.h"
#include "cartographer/common/lua_parameter_dictionary_test_helpers.h"
#include "gmock/gmock.h"
namespace cartographer {
namespace mapping {
namespace {
class RangeDataInserterTest2DTSDF : public ::testing::Test {
protected:
RangeDataInserterTest2DTSDF()
: tsdf_(MapLimits(1., Eigen::Vector2d(0., 7.), CellLimits(8, 1)), 2.0,
10.0) {
auto parameter_dictionary = common::MakeDictionary(
"return { "
"truncation_distance = 2.0,"
"maximum_weight = 10.,"
"update_free_space = false,"
"normal_estimation_options = {"
"num_normal_samples = 2,"
"sample_radius = 10.,"
"},"
"project_sdf_distance_to_scan_normal = false,"
"update_weight_range_exponent = 0,"
"update_weight_angle_scan_normal_to_ray_kernel_bandwith = 0,"
"update_weight_distance_cell_to_hit_kernel_bandwith = 0,"
"}");
options_ = CreateTSDFRangeDataInserterOptions2D(parameter_dictionary.get());
range_data_inserter_ =
common::make_unique<TSDFRangeDataInserter2D>(options_);
}
void InsertPoint() {
sensor::RangeData range_data;
range_data.returns.emplace_back(-0.5f, 3.5f, 0.f);
range_data.origin.x() = -0.5f;
range_data.origin.y() = -0.5f;
range_data_inserter_->Insert(range_data, &tsdf_);
tsdf_.FinishUpdate();
}
proto::TSDFRangeDataInserterOptions2D options_;
TSDF2D tsdf_;
std::unique_ptr<TSDFRangeDataInserter2D> range_data_inserter_;
};
class MockCellProperties {
public:
MockCellProperties(const Eigen::Array2i& cell_index, const TSDF2D& tsdf)
: is_known_(tsdf.IsKnown(cell_index)),
tsd_(tsdf.GetTSD(cell_index)),
weight_(tsdf.GetWeight(cell_index)){};
bool is_known_;
float tsd_;
float weight_;
};
::std::ostream& operator<<(::std::ostream& os, const MockCellProperties& bar) {
return os << std::to_string(bar.is_known_) + "\t" + std::to_string(bar.tsd_) +
"\t" + std::to_string(bar.weight_);
}
MATCHER_P3(EqualCellProperties, expected_is_known, expected_tsd,
expected_weight,
std::string("Expected ") + std::to_string(expected_is_known) + "\t" +
std::to_string(expected_tsd) + "\t" +
std::to_string(expected_weight)) {
bool result = expected_is_known == arg.is_known_;
result = result && (std::abs(expected_tsd - arg.tsd_ < 1e-4));
result = result && (std::abs(expected_weight - arg.weight_ < 1e-2));
return result;
}
TEST_F(RangeDataInserterTest2DTSDF, InsertPoint) {
InsertPoint();
const float truncation_distance =
static_cast<float>(options_.truncation_distance());
const float maximum_weight = static_cast<float>(options_.maximum_weight());
for (float y = 1.5; y < 6.; ++y) {
// Cell intersects with ray.
float x = -0.5f;
Eigen::Array2i cell_index =
tsdf_.limits().GetCellIndex(Eigen::Vector2f(x, y));
float expected_tsdf =
std::max(std::min(3.5f - y, truncation_distance), -truncation_distance);
float expected_weight = 1.f;
EXPECT_THAT(MockCellProperties(cell_index, tsdf_),
EqualCellProperties(true, expected_tsdf, expected_weight));
// Cells don't intersect with ray.
x = 0.5f;
cell_index = tsdf_.limits().GetCellIndex(Eigen::Vector2f(x, y));
expected_tsdf = -truncation_distance;
expected_weight = 0.;
EXPECT_THAT(MockCellProperties(cell_index, tsdf_),
EqualCellProperties(false, expected_tsdf, expected_weight));
x = 1.5f;
cell_index = tsdf_.limits().GetCellIndex(Eigen::Vector2f(x, y));
EXPECT_THAT(MockCellProperties(cell_index, tsdf_),
EqualCellProperties(false, expected_tsdf, expected_weight));
}
// Cells don't intersect with ray.
Eigen::Array2i cell_index =
tsdf_.limits().GetCellIndex(Eigen::Vector2f(0.5, 6.5));
EXPECT_THAT(MockCellProperties(cell_index, tsdf_),
EqualCellProperties(false, -truncation_distance, 0.));
cell_index = tsdf_.limits().GetCellIndex(Eigen::Vector2f(-0.5, -1.5));
EXPECT_THAT(MockCellProperties(cell_index, tsdf_),
EqualCellProperties(false, -truncation_distance, 0.));
for (int i = 0; i < 1000; ++i) {
InsertPoint();
}
for (float y = 1.5; y < 6.; ++y) {
// Cell intersects with ray.
float x = -0.5f;
Eigen::Array2i cell_index =
tsdf_.limits().GetCellIndex(Eigen::Vector2f(x, y));
float expected_tsdf =
std::max(std::min(3.5f - y, truncation_distance), -truncation_distance);
EXPECT_THAT(MockCellProperties(cell_index, tsdf_),
EqualCellProperties(true, expected_tsdf, maximum_weight));
}
}
TEST_F(RangeDataInserterTest2DTSDF, InsertPointWithFreeSpaceUpdate) {
options_.set_update_free_space(true);
range_data_inserter_ = common::make_unique<TSDFRangeDataInserter2D>(options_);
InsertPoint();
const float truncation_distance =
static_cast<float>(options_.truncation_distance());
const float maximum_weight = static_cast<float>(options_.maximum_weight());
for (float y = -0.5; y < 6.; ++y) {
// Cells intersect with ray.
float x = -0.5f;
Eigen::Array2i cell_index =
tsdf_.limits().GetCellIndex(Eigen::Vector2f(x, y));
float expected_tsdf =
std::max(std::min(3.5f - y, truncation_distance), -truncation_distance);
float expected_weight = 1.f;
EXPECT_THAT(MockCellProperties(cell_index, tsdf_),
EqualCellProperties(true, expected_tsdf, expected_weight));
// Cells don't intersect with ray.
x = 0.5f;
cell_index = tsdf_.limits().GetCellIndex(Eigen::Vector2f(x, y));
expected_tsdf = -truncation_distance;
expected_weight = 0.;
EXPECT_THAT(MockCellProperties(cell_index, tsdf_),
EqualCellProperties(false, expected_tsdf, expected_weight));
x = 1.5f;
cell_index = tsdf_.limits().GetCellIndex(Eigen::Vector2f(x, y));
EXPECT_THAT(MockCellProperties(cell_index, tsdf_),
EqualCellProperties(false, expected_tsdf, expected_weight));
}
// Cells don't intersect with the ray.
Eigen::Array2i cell_index =
tsdf_.limits().GetCellIndex(Eigen::Vector2f(-0.5, 6.5));
EXPECT_THAT(MockCellProperties(cell_index, tsdf_),
EqualCellProperties(false, -truncation_distance, 0.));
cell_index = tsdf_.limits().GetCellIndex(Eigen::Vector2f(-0.5, -1.5));
EXPECT_THAT(MockCellProperties(cell_index, tsdf_),
EqualCellProperties(false, -truncation_distance, 0.));
for (int i = 0; i < 1000; ++i) {
InsertPoint();
}
for (float y = -0.5; y < 6.; ++y) {
// Cell intersects with ray.
float x = -0.5f;
Eigen::Array2i cell_index =
tsdf_.limits().GetCellIndex(Eigen::Vector2f(x, y));
float expected_tsdf =
std::max(std::min(3.5f - y, truncation_distance), -truncation_distance);
EXPECT_THAT(MockCellProperties(cell_index, tsdf_),
EqualCellProperties(true, expected_tsdf, maximum_weight));
}
}
TEST_F(RangeDataInserterTest2DTSDF, InsertPointLinearWeight) {
options_.set_update_weight_range_exponent(1);
range_data_inserter_ = common::make_unique<TSDFRangeDataInserter2D>(options_);
InsertPoint();
const float truncation_distance =
static_cast<float>(options_.truncation_distance());
for (float y = 1.5; y < 6.; ++y) {
// Cell intersects with ray.
float x = -0.5f;
Eigen::Array2i cell_index =
tsdf_.limits().GetCellIndex(Eigen::Vector2f(x, y));
float expected_tsdf =
std::max(std::min(3.5f - y, truncation_distance), -truncation_distance);
float expected_weight = 1.f / 4.f;
EXPECT_THAT(MockCellProperties(cell_index, tsdf_),
EqualCellProperties(true, expected_tsdf, expected_weight));
}
}
TEST_F(RangeDataInserterTest2DTSDF, InsertPointQuadraticWeight) {
options_.set_update_weight_range_exponent(2);
range_data_inserter_ = common::make_unique<TSDFRangeDataInserter2D>(options_);
InsertPoint();
const float truncation_distance =
static_cast<float>(options_.truncation_distance());
for (float y = 1.5; y < 6.; ++y) {
// Cell intersects with ray.
float x = -0.5f;
Eigen::Array2i cell_index =
tsdf_.limits().GetCellIndex(Eigen::Vector2f(x, y));
float expected_tsdf =
std::max(std::min(3.5f - y, truncation_distance), -truncation_distance);
float expected_weight = 1.f / std::pow(4.f, 2);
EXPECT_THAT(MockCellProperties(cell_index, tsdf_),
EqualCellProperties(true, expected_tsdf, expected_weight));
}
}
TEST_F(RangeDataInserterTest2DTSDF,
InsertSmallAnglePointWithoutNormalProjection) {
sensor::RangeData range_data;
range_data.returns.emplace_back(-0.5f, 3.5f, 0.f);
range_data.returns.emplace_back(5.5f, 3.5f, 0.f);
range_data.returns.emplace_back(10.5f, 3.5f, 0.f);
range_data.origin.x() = -0.5f;
range_data.origin.y() = -0.5f;
range_data_inserter_->Insert(range_data, &tsdf_);
tsdf_.FinishUpdate();
float x = 4.5f;
float y = 2.5f;
Eigen::Array2i cell_index =
tsdf_.limits().GetCellIndex(Eigen::Vector2f(x, y));
Eigen::Vector2f ray =
Eigen::Vector2f(-0.5f, -0.5f) - Eigen::Vector2f(5.5f, 3.5f);
float ray_length = ray.norm();
Eigen::Vector2f origin_to_cell =
Eigen::Vector2f(x, y) - Eigen::Vector2f(-0.5f, -0.5f);
float distance_origin_to_cell = origin_to_cell.norm();
float expected_tsdf = ray_length - distance_origin_to_cell;
float expected_weight = 1.f;
EXPECT_THAT(MockCellProperties(cell_index, tsdf_),
EqualCellProperties(true, expected_tsdf, expected_weight));
}
TEST_F(RangeDataInserterTest2DTSDF, InsertSmallAnglePointWitNormalProjection) {
options_.set_project_sdf_distance_to_scan_normal(true);
range_data_inserter_ = common::make_unique<TSDFRangeDataInserter2D>(options_);
sensor::RangeData range_data;
range_data.returns.emplace_back(-0.5f, 3.5f, 0.f);
range_data.returns.emplace_back(5.5f, 3.5f, 0.f);
range_data.origin.x() = -0.5f;
range_data.origin.y() = -0.5f;
range_data_inserter_->Insert(range_data, &tsdf_);
tsdf_.FinishUpdate();
float x = 4.5f;
float y = 2.5f;
Eigen::Array2i cell_index =
tsdf_.limits().GetCellIndex(Eigen::Vector2f(x, y));
float expected_tsdf = 1.f;
float expected_weight = 1.f;
EXPECT_THAT(MockCellProperties(cell_index, tsdf_),
EqualCellProperties(true, expected_tsdf, expected_weight));
x = 6.5f;
y = 4.5f;
cell_index = tsdf_.limits().GetCellIndex(Eigen::Vector2f(x, y));
expected_tsdf = -1.f;
expected_weight = 1.f;
EXPECT_THAT(MockCellProperties(cell_index, tsdf_),
EqualCellProperties(true, expected_tsdf, expected_weight));
}
TEST_F(RangeDataInserterTest2DTSDF,
InsertPointsWithAngleScanNormalToRayWeight) {
float bandwith = 10.f;
options_.set_update_weight_angle_scan_normal_to_ray_kernel_bandwith(bandwith);
range_data_inserter_ = common::make_unique<TSDFRangeDataInserter2D>(options_);
sensor::RangeData range_data;
range_data.returns.emplace_back(-0.5f, 3.5f, 0.f);
range_data.returns.emplace_back(5.5f, 3.5f, 0.f);
range_data.origin.x() = -0.5f;
range_data.origin.y() = -0.5f;
range_data_inserter_->Insert(range_data, &tsdf_);
tsdf_.FinishUpdate();
float x = -0.5f;
float y = 3.5f;
// Ray is perpendicular to surface.
Eigen::Array2i cell_index =
tsdf_.limits().GetCellIndex(Eigen::Vector2f(x, y));
float expected_weight = 1.f / (std::sqrt(2 * M_PI) * bandwith);
EXPECT_NEAR(expected_weight, tsdf_.GetWeight(cell_index), 1e-3);
x = 6.5f;
y = 4.5f;
cell_index = tsdf_.limits().GetCellIndex(Eigen::Vector2f(x, y));
EXPECT_NEAR(expected_weight, tsdf_.GetWeight(cell_index), 1e-3);
// Ray is inclined relative to surface.
cell_index = tsdf_.limits().GetCellIndex(Eigen::Vector2f(x, y));
float angle = std::atan(7.f / 5.f);
expected_weight = 1.f / (std::sqrt(2 * M_PI) * bandwith) *
std::exp(angle * angle / (2 * std::pow(bandwith, 2)));
EXPECT_NEAR(expected_weight, tsdf_.GetWeight(cell_index), 1e-3);
x = 6.5f;
y = 4.5f;
cell_index = tsdf_.limits().GetCellIndex(Eigen::Vector2f(x, y));
EXPECT_NEAR(expected_weight, tsdf_.GetWeight(cell_index), 1e-3);
}
TEST_F(RangeDataInserterTest2DTSDF, InsertPointsWithDistanceCellToHit) {
float bandwith = 10.f;
options_.set_update_weight_distance_cell_to_hit_kernel_bandwith(bandwith);
range_data_inserter_ = common::make_unique<TSDFRangeDataInserter2D>(options_);
InsertPoint();
const float truncation_distance =
static_cast<float>(options_.truncation_distance());
for (float y = 1.5; y < 6.; ++y) {
float x = -0.5f;
Eigen::Array2i cell_index =
tsdf_.limits().GetCellIndex(Eigen::Vector2f(x, y));
float expected_tsdf =
std::max(std::min(3.5f - y, truncation_distance), -truncation_distance);
float expected_weight =
1.f / (std::sqrt(2 * M_PI) * bandwith) *
std::exp(std::pow(expected_tsdf, 2) / (2 * std::pow(bandwith, 2)));
EXPECT_THAT(MockCellProperties(cell_index, tsdf_),
EqualCellProperties(true, expected_tsdf, expected_weight));
}
}
} // namespace
} // namespace mapping
} // namespace cartographer

View File

@ -62,8 +62,21 @@ class PoseGraph2DTest : public ::testing::Test {
hit_probability = 0.53,
miss_probability = 0.495,
},
tsdf_range_data_inserter = {
truncation_distance = 0.3,
maximum_weight = 10.,
update_free_space = false,
normal_estimation_options = {
num_normal_samples = 4,
sample_radius = 0.5,
},
project_sdf_distance_to_scan_normal = false,
update_weight_range_exponent = 0,
update_weight_angle_scan_normal_to_ray_kernel_bandwith = 0,
update_weight_distance_cell_to_hit_kernel_bandwith = 0,
},
})text");
},
})text");
active_submaps_ = common::make_unique<ActiveSubmaps2D>(
mapping::CreateSubmapsOptions2D(parameter_dictionary.get()));
}

View File

@ -0,0 +1,47 @@
// 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;
import "cartographer/mapping/proto/2d/normal_estimation_options_2d.proto";
message TSDFRangeDataInserterOptions2D {
// Distance to the surface within the signed distance function is evaluated.
double truncation_distance = 1;
// Maximum weight that can be stored in a cell.
double maximum_weight = 2;
// Enables updating cells between the sensor origin and the range observation
// as free space.
bool update_free_space = 3;
NormalEstimationOptions2D normal_estimation_options = 4;
// Project the distance between the updated cell und the range observation to
// the estimated scan normal.
bool project_sdf_distance_to_scan_normal = 5;
// Update weight is scaled with 1/distance(origin,hit)^range_exponent.
int32 update_weight_range_exponent = 6;
// Kernel bandwith of the weight factor based on the angle between.
// scan normal and ray
double update_weight_angle_scan_normal_to_ray_kernel_bandwith = 7;
// Kernel bandwith of the weight factor based on the distance between
// cell and scan observation.
double update_weight_distance_cell_to_hit_kernel_bandwith = 8;
}

View File

@ -15,6 +15,7 @@
syntax = "proto3";
import "cartographer/mapping/proto/2d/probability_grid_range_data_inserter_options_2d.proto";
import "cartographer/mapping/proto/2d/tsdf_range_data_inserter_options_2d.proto";
package cartographer.mapping.proto;
@ -22,9 +23,11 @@ message RangeDataInserterOptions {
enum RangeDataInserterType {
INVALID_INSERTER = 0;
PROBABILITY_GRID_INSERTER_2D = 1;
TSDF_INSERTER_2D = 2;
}
RangeDataInserterType range_data_inserter_type = 1;
ProbabilityGridRangeDataInserterOptions2D
probability_grid_range_data_inserter_options_2d = 2;
TSDFRangeDataInserterOptions2D tsdf_range_data_inserter_options_2d = 3;
}

View File

@ -16,6 +16,7 @@
#include "cartographer/mapping/range_data_inserter_interface.h"
#include "cartographer/mapping/2d/probability_grid_range_data_inserter_2d.h"
#include "cartographer/mapping/2d/tsdf_range_data_inserter_2d.h"
namespace cartographer {
namespace mapping {
@ -37,6 +38,10 @@ proto::RangeDataInserterOptions CreateRangeDataInserterOptions(
parameter_dictionary
->GetDictionary("probability_grid_range_data_inserter")
.get());
*options.mutable_tsdf_range_data_inserter_options_2d() =
CreateTSDFRangeDataInserterOptions2D(
parameter_dictionary->GetDictionary("tsdf_range_data_inserter")
.get());
return options;
}
} // namespace mapping

View File

@ -74,6 +74,19 @@ TRAJECTORY_BUILDER_2D = {
hit_probability = 0.55,
miss_probability = 0.49,
},
tsdf_range_data_inserter = {
truncation_distance = 0.3,
maximum_weight = 10.,
update_free_space = false,
normal_estimation_options = {
num_normal_samples = 4,
sample_radius = 0.5,
},
project_sdf_distance_to_scan_normal = false,
update_weight_range_exponent = 0,
update_weight_angle_scan_normal_to_ray_kernel_bandwith = 0,
update_weight_distance_cell_to_hit_kernel_bandwith = 0,
},
},
},
}