Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Erandtke/210928 map salting #22

Open
wants to merge 13 commits into
base: knight
Choose a base branch
from
4 changes: 3 additions & 1 deletion cartographer/mapping/2d/grid_2d.cc
Original file line number Diff line number Diff line change
Expand Up @@ -100,7 +100,9 @@ void Grid2D::FinishUpdate() {
while (!update_indices_.empty()) {
DCHECK_GE(correspondence_cost_cells_[update_indices_.back()],
kUpdateMarker);
correspondence_cost_cells_[update_indices_.back()] -= kUpdateMarker;
bool isSalted = (correspondence_cost_cells_[update_indices_.back()]) == SentinelValue(Sentinel::Salted);
if(!isSalted)
correspondence_cost_cells_[update_indices_.back()] -= kUpdateMarker;
update_indices_.pop_back();
}
}
Expand Down
20 changes: 17 additions & 3 deletions cartographer/mapping/2d/grid_2d.h
Original file line number Diff line number Diff line change
Expand Up @@ -25,6 +25,7 @@
#include "cartographer/mapping/proto/2d/grid_2d.pb.h"
#include "cartographer/mapping/proto/2d/submaps_options_2d.pb.h"
#include "cartographer/mapping/proto/submap_visualization.pb.h"
#include "cartographer/mapping/sentinel.h"
#include "cartographer/mapping/value_conversion_tables.h"

namespace cartographer {
Expand All @@ -51,7 +52,7 @@ class Grid2D : public GridInterface {

// Returns the correspondence cost of the cell with 'cell_index'.
float GetCorrespondenceCost(const Eigen::Array2i& cell_index) const {
if (!limits().Contains(cell_index)) return max_correspondence_cost_;
if (!limits().Contains(cell_index) or IsSalted(cell_index)) return max_correspondence_cost_;
return (*value_to_correspondence_cost_table_)
[correspondence_cost_cells()[ToFlatIndex(cell_index)]];
}
Expand All @@ -68,7 +69,20 @@ class Grid2D : public GridInterface {
bool IsKnown(const Eigen::Array2i& cell_index) const {
return limits_.Contains(cell_index) &&
correspondence_cost_cells_[ToFlatIndex(cell_index)] !=
kUnknownCorrespondenceValue;
SentinelValue(Sentinel::Unknown);
}

// Returns true if the cell is salted
bool IsSalted(const Eigen::Array2i& cell_index) const {
return limits_.Contains(cell_index) &&
correspondence_cost_cells_[ToFlatIndex(cell_index)] ==
SentinelValue(Sentinel::Salted);
}

// Salts cell
void SaltCell(const Eigen::Array2i& cell_index) {
correspondence_cost_cells_[ToFlatIndex(cell_index)] =
SentinelValue(Sentinel::Salted);
}

// Fills in 'offset' and 'limits' to define a subregion of that contains all
Expand Down Expand Up @@ -111,7 +125,7 @@ class Grid2D : public GridInterface {

// Converts a 'cell_index' into an index into 'cells_'.
int ToFlatIndex(const Eigen::Array2i& cell_index) const {
CHECK(limits_.Contains(cell_index)) << cell_index;
CHECK(limits_.Contains(cell_index)) << cell_index << " " << limits_.cell_limits().num_x_cells << " " << limits_.cell_limits().num_y_cells;
return limits_.cell_limits().num_x_cells * cell_index.y() + cell_index.x();
}

Expand Down
5 changes: 3 additions & 2 deletions cartographer/mapping/2d/tsd_value_converter.cc
Original file line number Diff line number Diff line change
Expand Up @@ -15,6 +15,7 @@
*/

#include "cartographer/mapping/2d/tsd_value_converter.h"
#include "cartographer/mapping/sentinel.h"

namespace cartographer {
namespace mapping {
Expand All @@ -24,8 +25,8 @@ TSDValueConverter::TSDValueConverter(float max_tsd, float max_weight,
: max_tsd_(max_tsd),
min_tsd_(-max_tsd),
max_weight_(max_weight),
tsd_resolution_(32766.f / (max_tsd_ - min_tsd_)),
weight_resolution_(32766.f / (max_weight_ - min_weight_)),
tsd_resolution_(NumInt16Values<float>() / (max_tsd_ - min_tsd_)),
weight_resolution_(NumInt16Values<float>() / (max_weight_ - min_weight_)),
value_to_tsd_(
conversion_tables->GetConversionTable(min_tsd_, min_tsd_, max_tsd_)),
value_to_weight_(conversion_tables->GetConversionTable(
Expand Down
13 changes: 7 additions & 6 deletions cartographer/mapping/2d/tsd_value_converter.h
Original file line number Diff line number Diff line change
Expand Up @@ -22,6 +22,7 @@

#include "cartographer/common/math.h"
#include "cartographer/common/port.h"
#include "cartographer/mapping/sentinel.h"
#include "cartographer/mapping/value_conversion_tables.h"
#include "glog/logging.h"

Expand All @@ -38,19 +39,19 @@ class TSDValueConverter {
// Converts a tsd to a uint16 in the [1, 32767] range.
inline uint16 TSDToValue(const float tsd) const {
const int value =
common::RoundToInt((ClampTSD(tsd) - min_tsd_) * tsd_resolution_) + 1;
DCHECK_GE(value, 1);
DCHECK_LE(value, 32767);
common::RoundToInt((ClampTSD(tsd) - min_tsd_) * tsd_resolution_) + SentinelCount<int>();
DCHECK_GE(value, SentinelCount<int>());
DCHECK_LE(value, MaxInt16Value<int>());
return value;
}

// Converts a weight to a uint16 in the [1, 32767] range.
inline uint16 WeightToValue(const float weight) const {
const int value = common::RoundToInt((ClampWeight(weight) - min_weight_) *
weight_resolution_) +
1;
DCHECK_GE(value, 1);
DCHECK_LE(value, 32767);
SentinelCount<int>();
DCHECK_GE(value, SentinelCount<int>());
DCHECK_LE(value, MaxInt16Value<int>());
return value;
}

Expand Down
17 changes: 9 additions & 8 deletions cartographer/mapping/2d/tsd_value_converter_test.cc
Original file line number Diff line number Diff line change
Expand Up @@ -15,6 +15,7 @@
*/

#include "cartographer/mapping/2d/tsd_value_converter.h"
#include "cartographer/mapping/sentinel.h"

#include "gtest/gtest.h"

Expand Down Expand Up @@ -45,30 +46,30 @@ TEST_F(TSDValueConverterTest, DefaultValues) {
}

TEST_F(TSDValueConverterTest, ValueToTSDConversions) {
for (uint16 i = 1; i < 32768; ++i) {
for (uint16 i = SentinelCount<uint16>(); i <= MaxInt16Value<uint16>(); ++i) {
EXPECT_EQ(
tsd_value_converter_.TSDToValue(tsd_value_converter_.ValueToTSD(i)), i);
}
}

TEST_F(TSDValueConverterTest, ValueToTSDConversionsWithUpdateMarker) {
for (uint16 i = 1; i < 32768; ++i) {
for (uint16 i = SentinelCount<uint16>(); i <= MaxInt16Value<uint16>(); ++i) {
EXPECT_EQ(tsd_value_converter_.TSDToValue(tsd_value_converter_.ValueToTSD(
i + tsd_value_converter_.getUpdateMarker())),
i);
}
}

TEST_F(TSDValueConverterTest, ValueToWeightConversions) {
for (uint16 i = 1; i < 32768; ++i) {
for (uint16 i = SentinelCount<uint16>(); i <= MaxInt16Value<uint16>(); ++i) {
EXPECT_EQ(tsd_value_converter_.WeightToValue(
tsd_value_converter_.ValueToWeight(i)),
i);
}
}

TEST_F(TSDValueConverterTest, ValueToWeightConversionsWithUpdateMarker) {
for (uint16 i = 1; i < 32768; ++i) {
for (uint16 i = SentinelCount<uint16>(); i <= MaxInt16Value<uint16>(); ++i) {
EXPECT_EQ(
tsd_value_converter_.WeightToValue(tsd_value_converter_.ValueToWeight(
i + tsd_value_converter_.getUpdateMarker())),
Expand All @@ -78,7 +79,7 @@ TEST_F(TSDValueConverterTest, ValueToWeightConversionsWithUpdateMarker) {

TEST_F(TSDValueConverterTest, TSDToValueConversions) {
uint16 num_samples = 1000;
float tolerance = truncation_distance_ * 2.f / 32767.f;
float tolerance = truncation_distance_ * 2.f / MaxInt16Value<float>();
for (uint16 i = 0; i < num_samples; ++i) {
float sdf_sample =
-truncation_distance_ + i * 2.f * truncation_distance_ / num_samples;
Expand All @@ -90,7 +91,7 @@ TEST_F(TSDValueConverterTest, TSDToValueConversions) {

TEST_F(TSDValueConverterTest, WeightToValueConversions) {
uint16 num_samples = 1000;
float tolerance = max_weight_ / 32767.f;
float tolerance = max_weight_ / MaxInt16Value<float>();
for (uint16 i = 0; i < num_samples; ++i) {
float weight_sample = i * max_weight_ / num_samples;
EXPECT_NEAR(tsd_value_converter_.ValueToWeight(
Expand All @@ -100,7 +101,7 @@ TEST_F(TSDValueConverterTest, WeightToValueConversions) {
}

TEST_F(TSDValueConverterTest, WeightToValueOutOfRangeConversions) {
float tolerance = max_weight_ / 32767.f;
float tolerance = max_weight_ / MaxInt16Value<float>();
EXPECT_NEAR(tsd_value_converter_.ValueToWeight(
tsd_value_converter_.WeightToValue(2.f * max_weight_)),
max_weight_, tolerance);
Expand All @@ -110,7 +111,7 @@ TEST_F(TSDValueConverterTest, WeightToValueOutOfRangeConversions) {
}

TEST_F(TSDValueConverterTest, TSDToValueOutOfRangeConversions) {
float tolerance = truncation_distance_ * 2.f / 32767.f;
float tolerance = truncation_distance_ * 2.f / MaxInt16Value<float>();
EXPECT_NEAR(tsd_value_converter_.ValueToTSD(
tsd_value_converter_.TSDToValue(2.f * truncation_distance_)),
truncation_distance_, tolerance);
Expand Down
3 changes: 2 additions & 1 deletion cartographer/mapping/2d/tsdf_2d.cc
Original file line number Diff line number Diff line change
Expand Up @@ -56,7 +56,8 @@ 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];
if (*tsdf_cell >= value_converter_->getUpdateMarker()) {
if (*tsdf_cell >= value_converter_->getUpdateMarker()
or *tsdf_cell == SentinelValue(Sentinel::Salted)) {
return;
}
mutable_update_indices()->push_back(flat_index);
Expand Down
1 change: 1 addition & 0 deletions cartographer/mapping/2d/tsdf_2d.h
Original file line number Diff line number Diff line change
Expand Up @@ -35,6 +35,7 @@ class TSDF2D : public Grid2D {
explicit TSDF2D(const proto::Grid2D& proto,
ValueConversionTables* conversion_tables);


void SetCell(const Eigen::Array2i& cell_index, const float tsd,
const float weight);
GridType GetGridType() const override;
Expand Down
1 change: 1 addition & 0 deletions cartographer/mapping/2d/tsdf_2d_test.cc
Original file line number Diff line number Diff line change
Expand Up @@ -118,6 +118,7 @@ TEST(TSDF2DTest, WriteRead) {
std::uniform_real_distribution<float> tsd_distribution(-truncation_distance,
truncation_distance);
std::uniform_real_distribution<float> weight_distribution(0.f, max_weight);

for (size_t i = 0; i < 1; ++i) {
const float tsd = tsd_distribution(rng);
const float weight = weight_distribution(rng);
Expand Down
54 changes: 49 additions & 5 deletions cartographer/mapping/2d/tsdf_range_data_inserter_2d.cc
Original file line number Diff line number Diff line change
Expand Up @@ -18,6 +18,7 @@

#include "cartographer/mapping/internal/2d/normal_estimation_2d.h"
#include "cartographer/mapping/internal/2d/ray_to_pixel_mask.h"
#include "cartographer/mapping/internal/2d/hit_to_pixel_mask.h"

namespace cartographer {
namespace mapping {
Expand All @@ -31,7 +32,9 @@ constexpr float kMinRangeMeters = 1e-6f;
const float kSqrtTwoPi = std::sqrt(2.0 * M_PI);

void GrowAsNeeded(const sensor::RangeData& range_data,
const float truncation_distance, TSDF2D* const tsdf) {
const float truncation_distance,
const float /*salt_radius*/,
TSDF2D* const tsdf) {
Eigen::AlignedBox2f bounding_box(range_data.origin.head<2>());
for (const sensor::RangefinderPoint& hit : range_data.returns) {
const Eigen::Vector3f direction =
Expand All @@ -42,8 +45,8 @@ void GrowAsNeeded(const sensor::RangeData& range_data,
}
// 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());
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) {
Expand All @@ -66,6 +69,17 @@ std::pair<Eigen::Array2i, Eigen::Array2i> SuperscaleRay(
return std::make_pair(superscaled_begin, superscaled_end);
}

std::pair<Eigen::Array2i,int> CircleMask(
const Eigen::Vector2f& point,
const float radius,
TSDF2D* const tsdf) {

auto &limits = tsdf->limits();
Eigen::Array2i point_index = limits.GetCellIndex(point);
auto integer_radius = radius / limits.resolution();
return std::make_pair(point_index,integer_radius);
}

struct RangeDataSorter {
RangeDataSorter(Eigen::Vector3f origin) { origin_ = origin.head<2>(); }
bool operator()(const sensor::RangefinderPoint& lhs,
Expand Down Expand Up @@ -133,7 +147,8 @@ void TSDFRangeDataInserter2D::Insert(const sensor::RangeData& range_data,
const float truncation_distance =
static_cast<float>(options_.truncation_distance());
TSDF2D* tsdf = static_cast<TSDF2D*>(grid);
GrowAsNeeded(range_data, truncation_distance, tsdf);
float salt_radius = 0.5; // meters
GrowAsNeeded(range_data, truncation_distance, salt_radius, tsdf);

// Compute normals if needed.
bool scale_update_weight_angle_scan_normal_to_ray =
Expand All @@ -154,10 +169,19 @@ void TSDFRangeDataInserter2D::Insert(const sensor::RangeData& range_data,
++hit_index) {
const Eigen::Vector2f hit =
sorted_range_data.returns[hit_index].position.head<2>();
const bool salted =
sorted_range_data.returns[hit_index].salted;
const float normal = normals.empty()
? std::numeric_limits<float>::quiet_NaN()
: normals[hit_index];
InsertHit(options_, hit, origin, normal, tsdf);
if(salted)
{
InsertSalt(options_,hit,salt_radius,tsdf);
}
else
{
InsertHit(options_, hit, origin, normal, tsdf);
}
}
tsdf->FinishUpdate();
}
Expand Down Expand Up @@ -192,6 +216,7 @@ void TSDFRangeDataInserter2D::InsertHit(
angle_ray_normal,
options_.update_weight_angle_scan_normal_to_ray_kernel_bandwidth());
}

float weight_factor_range = 1.f;
if (options_.update_weight_range_exponent() != 0) {
weight_factor_range = ComputeRangeWeightFactor(
Expand All @@ -201,6 +226,7 @@ void TSDFRangeDataInserter2D::InsertHit(
// Update Cells.
for (const Eigen::Array2i& cell_index : ray_mask) {
if (tsdf->CellIsUpdated(cell_index)) continue;
if (tsdf->IsSalted(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;
Expand All @@ -218,10 +244,28 @@ void TSDFRangeDataInserter2D::InsertHit(
update_tsd,
options_.update_weight_distance_cell_to_hit_kernel_bandwidth());
}

UpdateCell(cell_index, update_tsd, update_weight, tsdf);
}
}

void TSDFRangeDataInserter2D::InsertSalt(
const proto::TSDFRangeDataInserterOptions2D& options,
const Eigen::Vector2f& hit,
const float salt_radius,
TSDF2D* tsdf) const {

auto [mask_center,mask_radius] = CircleMask(hit, salt_radius, tsdf);
std::vector<Eigen::Array2i> hit_mask = HitToPixelMask(
mask_center, mask_radius, tsdf->limits());

// Update Cells.
for (const Eigen::Array2i& cell_index : hit_mask) {
if (tsdf->IsSalted(cell_index)) continue;
tsdf->SaltCell(cell_index);
}
}

void TSDFRangeDataInserter2D::UpdateCell(const Eigen::Array2i& cell,
float update_sdf, float update_weight,
TSDF2D* tsdf) const {
Expand Down
8 changes: 7 additions & 1 deletion cartographer/mapping/2d/tsdf_range_data_inserter_2d.h
Original file line number Diff line number Diff line change
Expand Up @@ -49,6 +49,12 @@ class TSDFRangeDataInserter2D : public RangeDataInserterInterface {
void InsertHit(const proto::TSDFRangeDataInserterOptions2D& options,
const Eigen::Vector2f& hit, const Eigen::Vector2f& origin,
float normal, TSDF2D* tsdf) const;
void InsertSalt(
const proto::TSDFRangeDataInserterOptions2D& options,
const Eigen::Vector2f& hit,
float salt_radius,
TSDF2D* tsdf) const;

void UpdateCell(const Eigen::Array2i& cell, float update_sdf,
float update_weight, TSDF2D* tsdf) const;
const proto::TSDFRangeDataInserterOptions2D options_;
Expand All @@ -57,4 +63,4 @@ class TSDFRangeDataInserter2D : public RangeDataInserterInterface {
} // namespace mapping
} // namespace cartographer

#endif // CARTOGRAPHER_MAPPING_2D_TSDF_RANGE_DATA_INSERTER_2D_H_
#endif // CARTOGRAPHER_MAPPING_2D_TSDF_RANGE_DATA_INSERTER_2D_H_
23 changes: 22 additions & 1 deletion cartographer/mapping/2d/tsdf_range_data_inserter_2d_test.cc
Original file line number Diff line number Diff line change
Expand Up @@ -344,6 +344,27 @@ TEST_F(RangeDataInserterTest2DTSDF, InsertPointsWithDistanceCellToHit) {
}
}

TEST_F(RangeDataInserterTest2DTSDF, InsertSaltPoints) {
float bandwidth = 10.f;
options_.set_update_weight_distance_cell_to_hit_kernel_bandwidth(bandwidth);
range_data_inserter_ = absl::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) * bandwidth) *
std::exp(std::pow(expected_tsdf, 2) / (2 * std::pow(bandwidth, 2)));
EXPECT_THAT(MockCellProperties(cell_index, tsdf_),
EqualCellProperties(true, expected_tsdf, expected_weight));
}
}

} // namespace
} // namespace mapping
} // namespace cartographer
} // namespace cartographer
Loading