Add TSDF RangeDataInserter (#1236)
Adds TSDF RangeDataInserter, tests and integrates the configuration files.master
parent
3bf9ba0a69
commit
7dfe404278
|
@ -75,6 +75,12 @@ class MapLimits {
|
||||||
common::RoundToInt((max_.x() - point.x()) / resolution_ - 0.5));
|
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'.
|
// Returns true if the ProbabilityGrid contains 'cell_index'.
|
||||||
bool Contains(const Eigen::Array2i& cell_index) const {
|
bool Contains(const Eigen::Array2i& cell_index) const {
|
||||||
return (Eigen::Array2i(0, 0) <= cell_index).all() &&
|
return (Eigen::Array2i(0, 0) <= cell_index).all() &&
|
||||||
|
|
|
@ -35,6 +35,7 @@ constexpr int kSubpixelScale = 1000;
|
||||||
void GrowAsNeeded(const sensor::RangeData& range_data,
|
void GrowAsNeeded(const sensor::RangeData& range_data,
|
||||||
ProbabilityGrid* const probability_grid) {
|
ProbabilityGrid* const probability_grid) {
|
||||||
Eigen::AlignedBox2f bounding_box(range_data.origin.head<2>());
|
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;
|
constexpr float kPadding = 1e-6f;
|
||||||
for (const Eigen::Vector3f& hit : range_data.returns) {
|
for (const Eigen::Vector3f& hit : range_data.returns) {
|
||||||
bounding_box.extend(hit.head<2>());
|
bounding_box.extend(hit.head<2>());
|
||||||
|
|
|
@ -50,6 +50,19 @@ TEST(Submap2DTest, TheRightNumberOfRangeDataAreInserted) {
|
||||||
"hit_probability = 0.53, "
|
"hit_probability = 0.53, "
|
||||||
"miss_probability = 0.495, "
|
"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())};
|
ActiveSubmaps2D submaps{CreateSubmapsOptions2D(parameter_dictionary.get())};
|
||||||
|
|
|
@ -16,10 +16,6 @@
|
||||||
|
|
||||||
#include "cartographer/mapping/2d/tsdf_2d.h"
|
#include "cartographer/mapping/2d/tsdf_2d.h"
|
||||||
|
|
||||||
#include <limits>
|
|
||||||
|
|
||||||
#include "cartographer/common/make_unique.h"
|
|
||||||
|
|
||||||
namespace cartographer {
|
namespace cartographer {
|
||||||
namespace mapping {
|
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,
|
void TSDF2D::SetCell(const Eigen::Array2i& cell_index, float tsd,
|
||||||
float weight) {
|
float weight) {
|
||||||
const int flat_index = ToFlatIndex(cell_index);
|
const int flat_index = ToFlatIndex(cell_index);
|
||||||
uint16* tsdf_cell = &(*mutable_correspondence_cost_cells())[flat_index];
|
uint16* tsdf_cell = &(*mutable_correspondence_cost_cells())[flat_index];
|
||||||
uint16* weight_cell = &weight_cells_[flat_index];
|
|
||||||
if (*tsdf_cell >= value_converter_->getUpdateMarker()) {
|
if (*tsdf_cell >= value_converter_->getUpdateMarker()) {
|
||||||
return;
|
return;
|
||||||
}
|
}
|
||||||
|
@ -55,8 +56,8 @@ void TSDF2D::SetCell(const Eigen::Array2i& cell_index, float tsd,
|
||||||
mutable_known_cells_box()->extend(cell_index.matrix());
|
mutable_known_cells_box()->extend(cell_index.matrix());
|
||||||
*tsdf_cell =
|
*tsdf_cell =
|
||||||
value_converter_->TSDToValue(tsd) + value_converter_->getUpdateMarker();
|
value_converter_->TSDToValue(tsd) + value_converter_->getUpdateMarker();
|
||||||
|
uint16* weight_cell = &weight_cells_[flat_index];
|
||||||
*weight_cell = value_converter_->WeightToValue(weight);
|
*weight_cell = value_converter_->WeightToValue(weight);
|
||||||
return;
|
|
||||||
}
|
}
|
||||||
|
|
||||||
float TSDF2D::GetTSD(const Eigen::Array2i& cell_index) const {
|
float TSDF2D::GetTSD(const Eigen::Array2i& cell_index) const {
|
||||||
|
|
|
@ -46,6 +46,7 @@ class TSDF2D : public Grid2D {
|
||||||
virtual bool DrawToSubmapTexture(
|
virtual bool DrawToSubmapTexture(
|
||||||
proto::SubmapQuery::Response::SubmapTexture* const texture,
|
proto::SubmapQuery::Response::SubmapTexture* const texture,
|
||||||
transform::Rigid3d local_pose) const override;
|
transform::Rigid3d local_pose) const override;
|
||||||
|
bool CellIsUpdated(const Eigen::Array2i& cell_index) const;
|
||||||
|
|
||||||
private:
|
private:
|
||||||
std::unique_ptr<TSDValueConverter> value_converter_;
|
std::unique_ptr<TSDValueConverter> value_converter_;
|
||||||
|
|
|
@ -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
|
|
@ -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_
|
|
@ -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
|
|
@ -62,8 +62,21 @@ class PoseGraph2DTest : public ::testing::Test {
|
||||||
hit_probability = 0.53,
|
hit_probability = 0.53,
|
||||||
miss_probability = 0.495,
|
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>(
|
active_submaps_ = common::make_unique<ActiveSubmaps2D>(
|
||||||
mapping::CreateSubmapsOptions2D(parameter_dictionary.get()));
|
mapping::CreateSubmapsOptions2D(parameter_dictionary.get()));
|
||||||
}
|
}
|
||||||
|
|
|
@ -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;
|
||||||
|
}
|
|
@ -15,6 +15,7 @@
|
||||||
syntax = "proto3";
|
syntax = "proto3";
|
||||||
|
|
||||||
import "cartographer/mapping/proto/2d/probability_grid_range_data_inserter_options_2d.proto";
|
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;
|
package cartographer.mapping.proto;
|
||||||
|
|
||||||
|
@ -22,9 +23,11 @@ message RangeDataInserterOptions {
|
||||||
enum RangeDataInserterType {
|
enum RangeDataInserterType {
|
||||||
INVALID_INSERTER = 0;
|
INVALID_INSERTER = 0;
|
||||||
PROBABILITY_GRID_INSERTER_2D = 1;
|
PROBABILITY_GRID_INSERTER_2D = 1;
|
||||||
|
TSDF_INSERTER_2D = 2;
|
||||||
}
|
}
|
||||||
|
|
||||||
RangeDataInserterType range_data_inserter_type = 1;
|
RangeDataInserterType range_data_inserter_type = 1;
|
||||||
ProbabilityGridRangeDataInserterOptions2D
|
ProbabilityGridRangeDataInserterOptions2D
|
||||||
probability_grid_range_data_inserter_options_2d = 2;
|
probability_grid_range_data_inserter_options_2d = 2;
|
||||||
|
TSDFRangeDataInserterOptions2D tsdf_range_data_inserter_options_2d = 3;
|
||||||
}
|
}
|
||||||
|
|
|
@ -16,6 +16,7 @@
|
||||||
|
|
||||||
#include "cartographer/mapping/range_data_inserter_interface.h"
|
#include "cartographer/mapping/range_data_inserter_interface.h"
|
||||||
#include "cartographer/mapping/2d/probability_grid_range_data_inserter_2d.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 cartographer {
|
||||||
namespace mapping {
|
namespace mapping {
|
||||||
|
@ -37,6 +38,10 @@ proto::RangeDataInserterOptions CreateRangeDataInserterOptions(
|
||||||
parameter_dictionary
|
parameter_dictionary
|
||||||
->GetDictionary("probability_grid_range_data_inserter")
|
->GetDictionary("probability_grid_range_data_inserter")
|
||||||
.get());
|
.get());
|
||||||
|
*options.mutable_tsdf_range_data_inserter_options_2d() =
|
||||||
|
CreateTSDFRangeDataInserterOptions2D(
|
||||||
|
parameter_dictionary->GetDictionary("tsdf_range_data_inserter")
|
||||||
|
.get());
|
||||||
return options;
|
return options;
|
||||||
}
|
}
|
||||||
} // namespace mapping
|
} // namespace mapping
|
||||||
|
|
|
@ -74,6 +74,19 @@ TRAJECTORY_BUILDER_2D = {
|
||||||
hit_probability = 0.55,
|
hit_probability = 0.55,
|
||||||
miss_probability = 0.49,
|
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,
|
||||||
|
},
|
||||||
},
|
},
|
||||||
},
|
},
|
||||||
}
|
}
|
||||||
|
|
Loading…
Reference in New Issue