Add TSDF2D Grid (#1209)
- Adds TSDF2D Grid - Adds tests for TSDF2D Grid - Introduces Grid2D::GrowLimits(...) for multiple grids to reduce code duplication between TSDF2D and Grid2Dmaster
parent
b5279532ce
commit
15ecb88a92
|
@ -114,6 +114,13 @@ void Grid2D::ComputeCroppedLimits(Eigen::Array2i* const offset,
|
||||||
// these coordinates going forward. This method must be called immediately
|
// these coordinates going forward. This method must be called immediately
|
||||||
// after 'FinishUpdate', before any calls to 'ApplyLookupTable'.
|
// after 'FinishUpdate', before any calls to 'ApplyLookupTable'.
|
||||||
void Grid2D::GrowLimits(const Eigen::Vector2f& point) {
|
void Grid2D::GrowLimits(const Eigen::Vector2f& point) {
|
||||||
|
GrowLimits(point, {mutable_correspondence_cost_cells()},
|
||||||
|
{kUnknownCorrespondenceValue});
|
||||||
|
}
|
||||||
|
|
||||||
|
void Grid2D::GrowLimits(const Eigen::Vector2f& point,
|
||||||
|
const std::vector<std::vector<uint16>*>& grids,
|
||||||
|
const std::vector<uint16>& grids_unknown_cell_values) {
|
||||||
CHECK(update_indices_.empty());
|
CHECK(update_indices_.empty());
|
||||||
while (!limits_.Contains(limits_.GetCellIndex(point))) {
|
while (!limits_.Contains(limits_.GetCellIndex(point))) {
|
||||||
const int x_offset = limits_.cell_limits().num_x_cells / 2;
|
const int x_offset = limits_.cell_limits().num_x_cells / 2;
|
||||||
|
@ -128,15 +135,18 @@ void Grid2D::GrowLimits(const Eigen::Vector2f& point) {
|
||||||
const int offset = x_offset + stride * y_offset;
|
const int offset = x_offset + stride * y_offset;
|
||||||
const int new_size = new_limits.cell_limits().num_x_cells *
|
const int new_size = new_limits.cell_limits().num_x_cells *
|
||||||
new_limits.cell_limits().num_y_cells;
|
new_limits.cell_limits().num_y_cells;
|
||||||
std::vector<uint16> new_cells(new_size, kUnknownCorrespondenceValue);
|
|
||||||
|
for (size_t grid_index = 0; grid_index < grids.size(); ++grid_index) {
|
||||||
|
std::vector<uint16> new_cells(new_size,
|
||||||
|
grids_unknown_cell_values[grid_index]);
|
||||||
for (int i = 0; i < limits_.cell_limits().num_y_cells; ++i) {
|
for (int i = 0; i < limits_.cell_limits().num_y_cells; ++i) {
|
||||||
for (int j = 0; j < limits_.cell_limits().num_x_cells; ++j) {
|
for (int j = 0; j < limits_.cell_limits().num_x_cells; ++j) {
|
||||||
new_cells[offset + j + i * stride] =
|
new_cells[offset + j + i * stride] =
|
||||||
correspondence_cost_cells_[j +
|
(*grids[grid_index])[j + i * limits_.cell_limits().num_x_cells];
|
||||||
i * limits_.cell_limits().num_x_cells];
|
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
correspondence_cost_cells_ = new_cells;
|
*grids[grid_index] = new_cells;
|
||||||
|
}
|
||||||
limits_ = new_limits;
|
limits_ = new_limits;
|
||||||
if (!known_cells_box_.isEmpty()) {
|
if (!known_cells_box_.isEmpty()) {
|
||||||
known_cells_box_.translate(Eigen::Vector2i(x_offset, y_offset));
|
known_cells_box_.translate(Eigen::Vector2i(x_offset, y_offset));
|
||||||
|
@ -147,10 +157,8 @@ void Grid2D::GrowLimits(const Eigen::Vector2f& point) {
|
||||||
proto::Grid2D Grid2D::ToProto() const {
|
proto::Grid2D Grid2D::ToProto() const {
|
||||||
proto::Grid2D result;
|
proto::Grid2D result;
|
||||||
*result.mutable_limits() = mapping::ToProto(limits_);
|
*result.mutable_limits() = mapping::ToProto(limits_);
|
||||||
result.mutable_cells()->Reserve(correspondence_cost_cells_.size());
|
*result.mutable_cells() = {correspondence_cost_cells_.begin(),
|
||||||
for (const auto& cell : correspondence_cost_cells_) {
|
correspondence_cost_cells_.end()};
|
||||||
result.mutable_cells()->Add(cell);
|
|
||||||
}
|
|
||||||
CHECK(update_indices().empty()) << "Serializing a grid during an update is "
|
CHECK(update_indices().empty()) << "Serializing a grid during an update is "
|
||||||
"not supported. Finish the update first.";
|
"not supported. Finish the update first.";
|
||||||
if (!known_cells_box().isEmpty()) {
|
if (!known_cells_box().isEmpty()) {
|
||||||
|
|
|
@ -33,7 +33,7 @@ proto::GridOptions2D CreateGridOptions2D(
|
||||||
|
|
||||||
class Grid2D : public GridInterface {
|
class Grid2D : public GridInterface {
|
||||||
public:
|
public:
|
||||||
explicit Grid2D(const MapLimits& limits, float min_correspondence_cost,
|
Grid2D(const MapLimits& limits, float min_correspondence_cost,
|
||||||
float max_correspondence_cost);
|
float max_correspondence_cost);
|
||||||
explicit Grid2D(const proto::Grid2D& proto);
|
explicit Grid2D(const proto::Grid2D& proto);
|
||||||
|
|
||||||
|
@ -73,6 +73,10 @@ class Grid2D : public GridInterface {
|
||||||
transform::Rigid3d local_pose) const = 0;
|
transform::Rigid3d local_pose) const = 0;
|
||||||
|
|
||||||
protected:
|
protected:
|
||||||
|
void GrowLimits(const Eigen::Vector2f& point,
|
||||||
|
const std::vector<std::vector<uint16>*>& grids,
|
||||||
|
const std::vector<uint16>& grids_unknown_cell_values);
|
||||||
|
|
||||||
const std::vector<uint16>& correspondence_cost_cells() const {
|
const std::vector<uint16>& correspondence_cost_cells() const {
|
||||||
return correspondence_cost_cells_;
|
return correspondence_cost_cells_;
|
||||||
}
|
}
|
||||||
|
|
|
@ -0,0 +1,173 @@
|
||||||
|
/*
|
||||||
|
* 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_2d.h"
|
||||||
|
|
||||||
|
#include <limits>
|
||||||
|
|
||||||
|
#include "cartographer/common/make_unique.h"
|
||||||
|
|
||||||
|
namespace cartographer {
|
||||||
|
namespace mapping {
|
||||||
|
|
||||||
|
TSDF2D::TSDF2D(const MapLimits& limits, float truncation_distance,
|
||||||
|
float max_weight)
|
||||||
|
: Grid2D(limits, -truncation_distance, truncation_distance),
|
||||||
|
value_converter_(common::make_unique<TSDValueConverter>(
|
||||||
|
truncation_distance, max_weight)),
|
||||||
|
weight_cells_(
|
||||||
|
limits.cell_limits().num_x_cells * limits.cell_limits().num_y_cells,
|
||||||
|
value_converter_->getUnknownWeightValue()) {}
|
||||||
|
|
||||||
|
TSDF2D::TSDF2D(const proto::Grid2D& proto) : Grid2D(proto) {
|
||||||
|
CHECK(proto.has_tsdf_2d());
|
||||||
|
value_converter_ = common::make_unique<TSDValueConverter>(
|
||||||
|
proto.tsdf_2d().truncation_distance(), proto.tsdf_2d().max_weight());
|
||||||
|
weight_cells_.reserve(proto.tsdf_2d().weight_cells_size());
|
||||||
|
for (const auto& cell : proto.tsdf_2d().weight_cells()) {
|
||||||
|
CHECK_LE(cell, std::numeric_limits<uint16>::max());
|
||||||
|
weight_cells_.push_back(cell);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
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;
|
||||||
|
}
|
||||||
|
mutable_update_indices()->push_back(flat_index);
|
||||||
|
mutable_known_cells_box()->extend(cell_index.matrix());
|
||||||
|
*tsdf_cell =
|
||||||
|
value_converter_->TSDToValue(tsd) + value_converter_->getUpdateMarker();
|
||||||
|
*weight_cell = value_converter_->WeightToValue(weight);
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
float TSDF2D::GetTSD(const Eigen::Array2i& cell_index) const {
|
||||||
|
if (limits().Contains(cell_index)) {
|
||||||
|
return value_converter_->ValueToTSD(
|
||||||
|
correspondence_cost_cells()[ToFlatIndex(cell_index)]);
|
||||||
|
}
|
||||||
|
return value_converter_->getMinTSD();
|
||||||
|
}
|
||||||
|
|
||||||
|
float TSDF2D::GetWeight(const Eigen::Array2i& cell_index) const {
|
||||||
|
if (limits().Contains(cell_index)) {
|
||||||
|
return value_converter_->ValueToWeight(
|
||||||
|
weight_cells_[ToFlatIndex(cell_index)]);
|
||||||
|
}
|
||||||
|
return value_converter_->getMinWeight();
|
||||||
|
}
|
||||||
|
|
||||||
|
std::pair<float, float> TSDF2D::GetTSDAndWeight(
|
||||||
|
const Eigen::Array2i& cell_index) const {
|
||||||
|
if (limits().Contains(cell_index)) {
|
||||||
|
int flat_index = ToFlatIndex(cell_index);
|
||||||
|
return std::make_pair(
|
||||||
|
value_converter_->ValueToTSD(correspondence_cost_cells()[flat_index]),
|
||||||
|
value_converter_->ValueToWeight(weight_cells_[flat_index]));
|
||||||
|
}
|
||||||
|
return std::make_pair(value_converter_->getMinTSD(),
|
||||||
|
value_converter_->getMinWeight());
|
||||||
|
}
|
||||||
|
|
||||||
|
void TSDF2D::GrowLimits(const Eigen::Vector2f& point) {
|
||||||
|
Grid2D::GrowLimits(point,
|
||||||
|
{mutable_correspondence_cost_cells(), &weight_cells_},
|
||||||
|
{value_converter_->getUnknownTSDValue(),
|
||||||
|
value_converter_->getUnknownWeightValue()});
|
||||||
|
}
|
||||||
|
|
||||||
|
proto::Grid2D TSDF2D::ToProto() const {
|
||||||
|
proto::Grid2D result;
|
||||||
|
result = Grid2D::ToProto();
|
||||||
|
*result.mutable_tsdf_2d()->mutable_weight_cells() = {weight_cells_.begin(),
|
||||||
|
weight_cells_.end()};
|
||||||
|
result.mutable_tsdf_2d()->set_truncation_distance(
|
||||||
|
value_converter_->getMaxTSD());
|
||||||
|
result.mutable_tsdf_2d()->set_max_weight(value_converter_->getMaxWeight());
|
||||||
|
return result;
|
||||||
|
}
|
||||||
|
|
||||||
|
std::unique_ptr<Grid2D> TSDF2D::ComputeCroppedGrid() const {
|
||||||
|
Eigen::Array2i offset;
|
||||||
|
CellLimits cell_limits;
|
||||||
|
ComputeCroppedLimits(&offset, &cell_limits);
|
||||||
|
const double resolution = limits().resolution();
|
||||||
|
const Eigen::Vector2d max =
|
||||||
|
limits().max() - resolution * Eigen::Vector2d(offset.y(), offset.x());
|
||||||
|
std::unique_ptr<TSDF2D> cropped_grid = common::make_unique<TSDF2D>(
|
||||||
|
MapLimits(resolution, max, cell_limits), value_converter_->getMaxTSD(),
|
||||||
|
value_converter_->getMaxWeight());
|
||||||
|
for (const Eigen::Array2i& xy_index : XYIndexRangeIterator(cell_limits)) {
|
||||||
|
if (!IsKnown(xy_index + offset)) continue;
|
||||||
|
cropped_grid->SetCell(xy_index, GetTSD(xy_index + offset),
|
||||||
|
GetWeight(xy_index + offset));
|
||||||
|
}
|
||||||
|
return std::move(cropped_grid);
|
||||||
|
}
|
||||||
|
|
||||||
|
bool TSDF2D::DrawToSubmapTexture(
|
||||||
|
proto::SubmapQuery::Response::SubmapTexture* const texture,
|
||||||
|
transform::Rigid3d local_pose) const {
|
||||||
|
Eigen::Array2i offset;
|
||||||
|
CellLimits cell_limits;
|
||||||
|
ComputeCroppedLimits(&offset, &cell_limits);
|
||||||
|
|
||||||
|
std::string cells;
|
||||||
|
for (const Eigen::Array2i& xy_index : XYIndexRangeIterator(cell_limits)) {
|
||||||
|
if (!IsKnown(xy_index + offset)) {
|
||||||
|
cells.push_back(0); // value
|
||||||
|
cells.push_back(0); // alpha
|
||||||
|
}
|
||||||
|
// We would like to add 'delta' but this is not possible using a value and
|
||||||
|
// alpha. We use premultiplied alpha, so when 'delta' is positive we can
|
||||||
|
// add it by setting 'alpha' to zero. If it is negative, we set 'value' to
|
||||||
|
// zero, and use 'alpha' to subtract. This is only correct when the pixel
|
||||||
|
// is currently white, so walls will look too gray. This should be hard to
|
||||||
|
// detect visually for the user, though.
|
||||||
|
float normalized_tsdf = std::abs(GetTSD(xy_index + offset));
|
||||||
|
normalized_tsdf =
|
||||||
|
std::pow(normalized_tsdf / value_converter_->getMaxTSD(), 0.5f);
|
||||||
|
float normalized_weight =
|
||||||
|
GetWeight(xy_index + offset) / value_converter_->getMaxWeight();
|
||||||
|
const int delta = static_cast<int>(
|
||||||
|
std::round(normalized_weight * (normalized_tsdf * 255. - 128.)));
|
||||||
|
const uint8 alpha = delta > 0 ? 0 : -delta;
|
||||||
|
const uint8 value = delta > 0 ? delta : 0;
|
||||||
|
cells.push_back(value);
|
||||||
|
cells.push_back((value || alpha) ? alpha : 1);
|
||||||
|
}
|
||||||
|
|
||||||
|
common::FastGzipString(cells, texture->mutable_cells());
|
||||||
|
texture->set_width(cell_limits.num_x_cells);
|
||||||
|
texture->set_height(cell_limits.num_y_cells);
|
||||||
|
const double resolution = limits().resolution();
|
||||||
|
texture->set_resolution(resolution);
|
||||||
|
const double max_x = limits().max().x() - resolution * offset.y();
|
||||||
|
const double max_y = limits().max().y() - resolution * offset.x();
|
||||||
|
*texture->mutable_slice_pose() = transform::ToProto(
|
||||||
|
local_pose.inverse() *
|
||||||
|
transform::Rigid3d::Translation(Eigen::Vector3d(max_x, max_y, 0.)));
|
||||||
|
|
||||||
|
return true;
|
||||||
|
}
|
||||||
|
|
||||||
|
} // namespace mapping
|
||||||
|
} // namespace cartographer
|
|
@ -0,0 +1,58 @@
|
||||||
|
/*
|
||||||
|
* 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_2D_H_
|
||||||
|
#define CARTOGRAPHER_MAPPING_2D_TSDF_2D_H_
|
||||||
|
|
||||||
|
#include <vector>
|
||||||
|
|
||||||
|
#include "cartographer/common/port.h"
|
||||||
|
#include "cartographer/mapping/2d/grid_2d.h"
|
||||||
|
#include "cartographer/mapping/2d/map_limits.h"
|
||||||
|
#include "cartographer/mapping/2d/xy_index.h"
|
||||||
|
#include "cartographer/mapping/internal/tsd_value_converter.h"
|
||||||
|
namespace cartographer {
|
||||||
|
namespace mapping {
|
||||||
|
|
||||||
|
// Represents a 2D grid of truncated signed distances and weights.
|
||||||
|
class TSDF2D : public Grid2D {
|
||||||
|
public:
|
||||||
|
TSDF2D(const MapLimits& limits, float truncation_distance, float max_weight);
|
||||||
|
explicit TSDF2D(const proto::Grid2D& proto);
|
||||||
|
|
||||||
|
void SetCell(const Eigen::Array2i& cell_index, const float tsd,
|
||||||
|
const float weight);
|
||||||
|
float GetTSD(const Eigen::Array2i& cell_index) const;
|
||||||
|
float GetWeight(const Eigen::Array2i& cell_index) const;
|
||||||
|
std::pair<float, float> GetTSDAndWeight(
|
||||||
|
const Eigen::Array2i& cell_index) const;
|
||||||
|
|
||||||
|
virtual void GrowLimits(const Eigen::Vector2f& point) override;
|
||||||
|
proto::Grid2D ToProto() const;
|
||||||
|
virtual std::unique_ptr<Grid2D> ComputeCroppedGrid() const override;
|
||||||
|
virtual bool DrawToSubmapTexture(
|
||||||
|
proto::SubmapQuery::Response::SubmapTexture* const texture,
|
||||||
|
transform::Rigid3d local_pose) const override;
|
||||||
|
|
||||||
|
private:
|
||||||
|
std::unique_ptr<TSDValueConverter> value_converter_;
|
||||||
|
std::vector<uint16> weight_cells_; // Highest bit is update marker.
|
||||||
|
};
|
||||||
|
|
||||||
|
} // namespace mapping
|
||||||
|
} // namespace cartographer
|
||||||
|
|
||||||
|
#endif // CARTOGRAPHER_MAPPING_2D_TSDF_2D_H_
|
|
@ -0,0 +1,155 @@
|
||||||
|
/*
|
||||||
|
* 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_2d.h"
|
||||||
|
|
||||||
|
#include <random>
|
||||||
|
|
||||||
|
#include "gtest/gtest.h"
|
||||||
|
|
||||||
|
namespace cartographer {
|
||||||
|
namespace mapping {
|
||||||
|
namespace {
|
||||||
|
|
||||||
|
using Eigen::Array2i;
|
||||||
|
using Eigen::Vector2f;
|
||||||
|
|
||||||
|
TEST(TSDF2DTest, ProtoConstructor) {
|
||||||
|
proto::Grid2D proto;
|
||||||
|
const MapLimits limits(1., {2., 3.}, CellLimits(4., 5.));
|
||||||
|
*proto.mutable_limits() = ToProto(limits);
|
||||||
|
proto.mutable_tsdf_2d()->set_max_weight(10.0);
|
||||||
|
proto.mutable_tsdf_2d()->set_truncation_distance(1.0);
|
||||||
|
for (int i = 6; i < 12; ++i) {
|
||||||
|
proto.mutable_cells()->Add(static_cast<uint16>(i));
|
||||||
|
proto.mutable_tsdf_2d()->mutable_weight_cells()->Add(
|
||||||
|
static_cast<uint16>(i));
|
||||||
|
}
|
||||||
|
proto.mutable_known_cells_box()->set_max_x(19);
|
||||||
|
proto.mutable_known_cells_box()->set_max_y(20);
|
||||||
|
proto.mutable_known_cells_box()->set_min_x(21);
|
||||||
|
proto.mutable_known_cells_box()->set_min_y(22);
|
||||||
|
proto.set_max_correspondence_cost(1.0);
|
||||||
|
proto.set_min_correspondence_cost(-1.0);
|
||||||
|
|
||||||
|
TSDF2D grid(proto);
|
||||||
|
EXPECT_EQ(proto.limits().DebugString(), ToProto(grid.limits()).DebugString());
|
||||||
|
}
|
||||||
|
|
||||||
|
TEST(TSDF2DTest, ToProto) {
|
||||||
|
TSDF2D tsdf(MapLimits(1., Eigen::Vector2d(1., 1.), CellLimits(2, 2)), 1.0f,
|
||||||
|
10.0f);
|
||||||
|
|
||||||
|
const auto proto = tsdf.ToProto();
|
||||||
|
EXPECT_EQ(ToProto(tsdf.limits()).DebugString(), proto.limits().DebugString());
|
||||||
|
}
|
||||||
|
|
||||||
|
TEST(TSDF2DTest, GetCellIndex) {
|
||||||
|
TSDF2D tsdf(MapLimits(2., Eigen::Vector2d(8., 14.), CellLimits(14, 8)), 1.f,
|
||||||
|
10.f);
|
||||||
|
|
||||||
|
const MapLimits& limits = tsdf.limits();
|
||||||
|
const CellLimits& cell_limits = limits.cell_limits();
|
||||||
|
ASSERT_EQ(14, cell_limits.num_x_cells);
|
||||||
|
ASSERT_EQ(8, cell_limits.num_y_cells);
|
||||||
|
EXPECT_TRUE(
|
||||||
|
(Array2i(0, 0) == limits.GetCellIndex(Vector2f(7.f, 13.f))).all());
|
||||||
|
EXPECT_TRUE(
|
||||||
|
(Array2i(13, 0) == limits.GetCellIndex(Vector2f(7.f, -13.f))).all());
|
||||||
|
EXPECT_TRUE(
|
||||||
|
(Array2i(0, 7) == limits.GetCellIndex(Vector2f(-7.f, 13.f))).all());
|
||||||
|
EXPECT_TRUE(
|
||||||
|
(Array2i(13, 7) == limits.GetCellIndex(Vector2f(-7.f, -13.f))).all());
|
||||||
|
|
||||||
|
// Check around the origin.
|
||||||
|
EXPECT_TRUE(
|
||||||
|
(Array2i(6, 3) == limits.GetCellIndex(Vector2f(0.5f, 0.5f))).all());
|
||||||
|
EXPECT_TRUE(
|
||||||
|
(Array2i(6, 3) == limits.GetCellIndex(Vector2f(1.5f, 1.5f))).all());
|
||||||
|
EXPECT_TRUE(
|
||||||
|
(Array2i(7, 3) == limits.GetCellIndex(Vector2f(0.5f, -0.5f))).all());
|
||||||
|
EXPECT_TRUE(
|
||||||
|
(Array2i(6, 4) == limits.GetCellIndex(Vector2f(-0.5f, 0.5f))).all());
|
||||||
|
EXPECT_TRUE(
|
||||||
|
(Array2i(7, 4) == limits.GetCellIndex(Vector2f(-0.5f, -0.5f))).all());
|
||||||
|
}
|
||||||
|
|
||||||
|
TEST(TSDF2DTest, WriteRead) {
|
||||||
|
const float truncation_distance = 1.f;
|
||||||
|
const float max_weight = 10.f;
|
||||||
|
TSDF2D tsdf(MapLimits(1., Eigen::Vector2d(1., 2.), CellLimits(2, 2)),
|
||||||
|
truncation_distance, max_weight);
|
||||||
|
|
||||||
|
const MapLimits& limits = tsdf.limits();
|
||||||
|
EXPECT_EQ(1., limits.max().x());
|
||||||
|
EXPECT_EQ(2., limits.max().y());
|
||||||
|
|
||||||
|
const CellLimits& cell_limits = limits.cell_limits();
|
||||||
|
ASSERT_EQ(2, cell_limits.num_x_cells);
|
||||||
|
ASSERT_EQ(2, cell_limits.num_y_cells);
|
||||||
|
|
||||||
|
std::mt19937 rng(42);
|
||||||
|
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);
|
||||||
|
tsdf.SetCell(limits.GetCellIndex(Vector2f(-0.5f, 0.5f)), tsd, weight);
|
||||||
|
EXPECT_NEAR(
|
||||||
|
tsdf.GetTSDAndWeight(limits.GetCellIndex(Vector2f(-0.5f, 0.5f))).first,
|
||||||
|
tsd, 2.f * truncation_distance / 32768.f);
|
||||||
|
EXPECT_NEAR(
|
||||||
|
tsdf.GetTSDAndWeight(limits.GetCellIndex(Vector2f(-0.5f, 0.5f))).second,
|
||||||
|
weight, max_weight / 32768.f);
|
||||||
|
EXPECT_NEAR(tsdf.GetTSD(limits.GetCellIndex(Vector2f(-0.5f, 0.5f))), tsd,
|
||||||
|
2.f * truncation_distance / 32768.f);
|
||||||
|
EXPECT_NEAR(tsdf.GetWeight(limits.GetCellIndex(Vector2f(-0.5f, 0.5f))),
|
||||||
|
weight, max_weight / 32768.f);
|
||||||
|
}
|
||||||
|
for (const Array2i& xy_index : {limits.GetCellIndex(Vector2f(-0.5f, 1.5f)),
|
||||||
|
limits.GetCellIndex(Vector2f(0.5f, 0.5f)),
|
||||||
|
limits.GetCellIndex(Vector2f(0.5f, 1.5f))}) {
|
||||||
|
EXPECT_TRUE(limits.Contains(xy_index));
|
||||||
|
EXPECT_FALSE(tsdf.IsKnown(xy_index));
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
TEST(TSDF2DTest, CorrectCropping) {
|
||||||
|
// Create a TSDF with random values.
|
||||||
|
const float truncation_distance = 1.f;
|
||||||
|
const float max_weight = 10.f;
|
||||||
|
std::mt19937 rng(42);
|
||||||
|
std::uniform_real_distribution<float> tsdf_distribution(-truncation_distance,
|
||||||
|
truncation_distance);
|
||||||
|
std::uniform_real_distribution<float> weight_distribution(0.f, max_weight);
|
||||||
|
TSDF2D tsdf(MapLimits(0.05, Eigen::Vector2d(10., 10.), CellLimits(400, 400)),
|
||||||
|
truncation_distance, max_weight);
|
||||||
|
for (const Array2i& xy_index :
|
||||||
|
XYIndexRangeIterator(Array2i(100, 100), Array2i(299, 299))) {
|
||||||
|
tsdf.SetCell(xy_index, tsdf_distribution(rng), weight_distribution(rng));
|
||||||
|
}
|
||||||
|
Array2i offset;
|
||||||
|
CellLimits limits;
|
||||||
|
tsdf.ComputeCroppedLimits(&offset, &limits);
|
||||||
|
EXPECT_TRUE((offset == Array2i(100, 100)).all());
|
||||||
|
EXPECT_EQ(limits.num_x_cells, 200);
|
||||||
|
EXPECT_EQ(limits.num_y_cells, 200);
|
||||||
|
}
|
||||||
|
|
||||||
|
} // namespace
|
||||||
|
} // namespace mapping
|
||||||
|
} // namespace cartographer
|
|
@ -18,6 +18,7 @@ package cartographer.mapping.proto;
|
||||||
|
|
||||||
import "cartographer/mapping/proto/2d/map_limits.proto";
|
import "cartographer/mapping/proto/2d/map_limits.proto";
|
||||||
import "cartographer/mapping/proto/2d/probability_grid.proto";
|
import "cartographer/mapping/proto/2d/probability_grid.proto";
|
||||||
|
import "cartographer/mapping/proto/2d/tsdf_2d.proto";
|
||||||
|
|
||||||
message Grid2D {
|
message Grid2D {
|
||||||
message CellBox {
|
message CellBox {
|
||||||
|
@ -34,6 +35,7 @@ message Grid2D {
|
||||||
CellBox known_cells_box = 3;
|
CellBox known_cells_box = 3;
|
||||||
oneof grid {
|
oneof grid {
|
||||||
ProbabilityGrid probability_grid_2d = 4;
|
ProbabilityGrid probability_grid_2d = 4;
|
||||||
|
TSDF2D tsdf_2d = 5;
|
||||||
}
|
}
|
||||||
float min_correspondence_cost = 6;
|
float min_correspondence_cost = 6;
|
||||||
float max_correspondence_cost = 7;
|
float max_correspondence_cost = 7;
|
||||||
|
|
|
@ -0,0 +1,23 @@
|
||||||
|
// Copyright 2018 The Cartographer Authors
|
||||||
|
//
|
||||||
|
// Licensed under the Apache License, Version 2.0 (the "License");
|
||||||
|
// you may not use this file except in compliance with the License.
|
||||||
|
// You may obtain a copy of the License at
|
||||||
|
//
|
||||||
|
// http://www.apache.org/licenses/LICENSE-2.0
|
||||||
|
//
|
||||||
|
// Unless required by applicable law or agreed to in writing, software
|
||||||
|
// distributed under the License is distributed on an "AS IS" BASIS,
|
||||||
|
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||||
|
// See the License for the specific language governing permissions and
|
||||||
|
// limitations under the License.
|
||||||
|
|
||||||
|
syntax = "proto3";
|
||||||
|
|
||||||
|
package cartographer.mapping.proto;
|
||||||
|
|
||||||
|
message TSDF2D {
|
||||||
|
float truncation_distance = 1;
|
||||||
|
float max_weight = 2;
|
||||||
|
repeated int32 weight_cells = 3;
|
||||||
|
}
|
Loading…
Reference in New Issue