Move implementation of ProbabilityGrid to .cc file. (#924)
Alexander Belyaev
GitHub
parent
ab05459f1c
commit
96d5e2819c
|
|
@ -19,6 +19,7 @@
|
|||
|
||||
#include "Eigen/Core"
|
||||
#include "Eigen/Geometry"
|
||||
#include "cartographer/mapping/probability_values.h"
|
||||
#include "cartographer/mapping_2d/probability_grid.h"
|
||||
#include "cartographer/sensor/point_cloud.h"
|
||||
#include "ceres/ceres.h"
|
||||
|
|
|
|||
|
|
@ -23,6 +23,7 @@
|
|||
#include "cartographer/io/draw_trajectories.h"
|
||||
#include "cartographer/io/image.h"
|
||||
#include "cartographer/io/points_batch.h"
|
||||
#include "cartographer/mapping/probability_values.h"
|
||||
|
||||
namespace cartographer {
|
||||
namespace io {
|
||||
|
|
|
|||
|
|
@ -0,0 +1,164 @@
|
|||
#include "cartographer/mapping_2d/probability_grid.h"
|
||||
|
||||
#include <limits>
|
||||
|
||||
#include "cartographer/mapping/probability_values.h"
|
||||
|
||||
namespace cartographer {
|
||||
namespace mapping_2d {
|
||||
namespace {
|
||||
|
||||
// Converts a 'cell_index' into an index into 'cells_'.
|
||||
int ToFlatIndex(const Eigen::Array2i& cell_index, const MapLimits& limits) {
|
||||
CHECK(limits.Contains(cell_index)) << cell_index;
|
||||
return limits.cell_limits().num_x_cells * cell_index.y() + cell_index.x();
|
||||
}
|
||||
|
||||
} // namespace
|
||||
|
||||
ProbabilityGrid::ProbabilityGrid(const MapLimits& limits)
|
||||
: limits_(limits),
|
||||
cells_(
|
||||
limits_.cell_limits().num_x_cells * limits_.cell_limits().num_y_cells,
|
||||
mapping::kUnknownProbabilityValue) {}
|
||||
|
||||
ProbabilityGrid::ProbabilityGrid(const proto::ProbabilityGrid& proto)
|
||||
: limits_(proto.limits()), cells_() {
|
||||
if (proto.has_known_cells_box()) {
|
||||
const auto& box = proto.known_cells_box();
|
||||
known_cells_box_ =
|
||||
Eigen::AlignedBox2i(Eigen::Vector2i(box.min_x(), box.min_y()),
|
||||
Eigen::Vector2i(box.max_x(), box.max_y()));
|
||||
}
|
||||
cells_.reserve(proto.cells_size());
|
||||
for (const auto& cell : proto.cells()) {
|
||||
CHECK_LE(cell, std::numeric_limits<uint16>::max());
|
||||
cells_.push_back(cell);
|
||||
}
|
||||
}
|
||||
|
||||
// Finishes the update sequence.
|
||||
void ProbabilityGrid::FinishUpdate() {
|
||||
while (!update_indices_.empty()) {
|
||||
DCHECK_GE(cells_[update_indices_.back()], mapping::kUpdateMarker);
|
||||
cells_[update_indices_.back()] -= mapping::kUpdateMarker;
|
||||
update_indices_.pop_back();
|
||||
}
|
||||
}
|
||||
|
||||
// Sets the probability of the cell at 'cell_index' to the given
|
||||
// 'probability'. Only allowed if the cell was unknown before.
|
||||
void ProbabilityGrid::SetProbability(const Eigen::Array2i& cell_index,
|
||||
const float probability) {
|
||||
uint16& cell = cells_[ToFlatIndex(cell_index, limits_)];
|
||||
CHECK_EQ(cell, mapping::kUnknownProbabilityValue);
|
||||
cell = mapping::ProbabilityToValue(probability);
|
||||
known_cells_box_.extend(cell_index.matrix());
|
||||
}
|
||||
|
||||
// Applies the 'odds' specified when calling ComputeLookupTableToApplyOdds()
|
||||
// to the probability of the cell at 'cell_index' if the cell has not already
|
||||
// been updated. Multiple updates of the same cell will be ignored until
|
||||
// FinishUpdate() is called. Returns true if the cell was updated.
|
||||
//
|
||||
// If this is the first call to ApplyOdds() for the specified cell, its value
|
||||
// will be set to probability corresponding to 'odds'.
|
||||
bool ProbabilityGrid::ApplyLookupTable(const Eigen::Array2i& cell_index,
|
||||
const std::vector<uint16>& table) {
|
||||
DCHECK_EQ(table.size(), mapping::kUpdateMarker);
|
||||
const int flat_index = ToFlatIndex(cell_index, limits_);
|
||||
uint16* cell = &cells_[flat_index];
|
||||
if (*cell >= mapping::kUpdateMarker) {
|
||||
return false;
|
||||
}
|
||||
update_indices_.push_back(flat_index);
|
||||
*cell = table[*cell];
|
||||
DCHECK_GE(*cell, mapping::kUpdateMarker);
|
||||
known_cells_box_.extend(cell_index.matrix());
|
||||
return true;
|
||||
}
|
||||
|
||||
// Returns the probability of the cell with 'cell_index'.
|
||||
float ProbabilityGrid::GetProbability(const Eigen::Array2i& cell_index) const {
|
||||
if (limits_.Contains(cell_index)) {
|
||||
return mapping::ValueToProbability(
|
||||
cells_[ToFlatIndex(cell_index, limits_)]);
|
||||
}
|
||||
return mapping::kMinProbability;
|
||||
}
|
||||
|
||||
// Returns true if the probability at the specified index is known.
|
||||
bool ProbabilityGrid::IsKnown(const Eigen::Array2i& cell_index) const {
|
||||
return limits_.Contains(cell_index) &&
|
||||
cells_[ToFlatIndex(cell_index, limits_)] !=
|
||||
mapping::kUnknownProbabilityValue;
|
||||
}
|
||||
|
||||
// Fills in 'offset' and 'limits' to define a subregion of that contains all
|
||||
// known cells.
|
||||
void ProbabilityGrid::ComputeCroppedLimits(Eigen::Array2i* const offset,
|
||||
CellLimits* const limits) const {
|
||||
if (known_cells_box_.isEmpty()) {
|
||||
*offset = Eigen::Array2i::Zero();
|
||||
*limits = CellLimits(1, 1);
|
||||
return;
|
||||
}
|
||||
*offset = known_cells_box_.min().array();
|
||||
*limits = CellLimits(known_cells_box_.sizes().x() + 1,
|
||||
known_cells_box_.sizes().y() + 1);
|
||||
}
|
||||
|
||||
// Grows the map as necessary to include 'point'. This changes the meaning of
|
||||
// these coordinates going forward. This method must be called immediately
|
||||
// after 'FinishUpdate', before any calls to 'ApplyLookupTable'.
|
||||
void ProbabilityGrid::GrowLimits(const Eigen::Vector2f& point) {
|
||||
CHECK(update_indices_.empty());
|
||||
while (!limits_.Contains(limits_.GetCellIndex(point))) {
|
||||
const int x_offset = limits_.cell_limits().num_x_cells / 2;
|
||||
const int y_offset = limits_.cell_limits().num_y_cells / 2;
|
||||
const MapLimits new_limits(
|
||||
limits_.resolution(),
|
||||
limits_.max() +
|
||||
limits_.resolution() * Eigen::Vector2d(y_offset, x_offset),
|
||||
CellLimits(2 * limits_.cell_limits().num_x_cells,
|
||||
2 * limits_.cell_limits().num_y_cells));
|
||||
const int stride = new_limits.cell_limits().num_x_cells;
|
||||
const int offset = x_offset + stride * y_offset;
|
||||
const int new_size = new_limits.cell_limits().num_x_cells *
|
||||
new_limits.cell_limits().num_y_cells;
|
||||
std::vector<uint16> new_cells(new_size, mapping::kUnknownProbabilityValue);
|
||||
for (int i = 0; i < limits_.cell_limits().num_y_cells; ++i) {
|
||||
for (int j = 0; j < limits_.cell_limits().num_x_cells; ++j) {
|
||||
new_cells[offset + j + i * stride] =
|
||||
cells_[j + i * limits_.cell_limits().num_x_cells];
|
||||
}
|
||||
}
|
||||
cells_ = new_cells;
|
||||
limits_ = new_limits;
|
||||
if (!known_cells_box_.isEmpty()) {
|
||||
known_cells_box_.translate(Eigen::Vector2i(x_offset, y_offset));
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
proto::ProbabilityGrid ProbabilityGrid::ToProto() const {
|
||||
proto::ProbabilityGrid result;
|
||||
*result.mutable_limits() = cartographer::mapping_2d::ToProto(limits_);
|
||||
result.mutable_cells()->Reserve(cells_.size());
|
||||
for (const auto& cell : cells_) {
|
||||
result.mutable_cells()->Add(cell);
|
||||
}
|
||||
CHECK(update_indices_.empty()) << "Serializing a grid during an update is "
|
||||
"not supported. Finish the update first.";
|
||||
if (!known_cells_box_.isEmpty()) {
|
||||
auto* const box = result.mutable_known_cells_box();
|
||||
box->set_max_x(known_cells_box_.max().x());
|
||||
box->set_max_y(known_cells_box_.max().y());
|
||||
box->set_min_x(known_cells_box_.min().x());
|
||||
box->set_min_y(known_cells_box_.min().y());
|
||||
}
|
||||
return result;
|
||||
}
|
||||
|
||||
} // namespace mapping_2d
|
||||
} // namespace cartographer
|
||||
|
|
@ -17,21 +17,12 @@
|
|||
#ifndef CARTOGRAPHER_MAPPING_2D_PROBABILITY_GRID_H_
|
||||
#define CARTOGRAPHER_MAPPING_2D_PROBABILITY_GRID_H_
|
||||
|
||||
#include <algorithm>
|
||||
#include <cmath>
|
||||
#include <iostream>
|
||||
#include <limits>
|
||||
#include <memory>
|
||||
#include <string>
|
||||
#include <vector>
|
||||
|
||||
#include "cartographer/common/math.h"
|
||||
#include "cartographer/common/port.h"
|
||||
#include "cartographer/mapping/probability_values.h"
|
||||
#include "cartographer/mapping_2d/map_limits.h"
|
||||
#include "cartographer/mapping_2d/proto/probability_grid.pb.h"
|
||||
#include "cartographer/mapping_2d/xy_index.h"
|
||||
#include "glog/logging.h"
|
||||
|
||||
namespace cartographer {
|
||||
namespace mapping_2d {
|
||||
|
|
@ -39,48 +30,19 @@ namespace mapping_2d {
|
|||
// Represents a 2D grid of probabilities.
|
||||
class ProbabilityGrid {
|
||||
public:
|
||||
explicit ProbabilityGrid(const MapLimits& limits)
|
||||
: limits_(limits),
|
||||
cells_(limits_.cell_limits().num_x_cells *
|
||||
limits_.cell_limits().num_y_cells,
|
||||
mapping::kUnknownProbabilityValue) {}
|
||||
|
||||
explicit ProbabilityGrid(const proto::ProbabilityGrid& proto)
|
||||
: limits_(proto.limits()), cells_() {
|
||||
if (proto.has_known_cells_box()) {
|
||||
const auto& box = proto.known_cells_box();
|
||||
known_cells_box_ =
|
||||
Eigen::AlignedBox2i(Eigen::Vector2i(box.min_x(), box.min_y()),
|
||||
Eigen::Vector2i(box.max_x(), box.max_y()));
|
||||
}
|
||||
cells_.reserve(proto.cells_size());
|
||||
for (const auto cell : proto.cells()) {
|
||||
CHECK_LE(cell, std::numeric_limits<uint16>::max());
|
||||
cells_.push_back(cell);
|
||||
}
|
||||
}
|
||||
explicit ProbabilityGrid(const MapLimits& limits);
|
||||
explicit ProbabilityGrid(const proto::ProbabilityGrid& proto);
|
||||
|
||||
// Returns the limits of this ProbabilityGrid.
|
||||
const MapLimits& limits() const { return limits_; }
|
||||
|
||||
// Finishes the update sequence.
|
||||
void FinishUpdate() {
|
||||
while (!update_indices_.empty()) {
|
||||
DCHECK_GE(cells_[update_indices_.back()], mapping::kUpdateMarker);
|
||||
cells_[update_indices_.back()] -= mapping::kUpdateMarker;
|
||||
update_indices_.pop_back();
|
||||
}
|
||||
}
|
||||
void FinishUpdate();
|
||||
|
||||
// Sets the probability of the cell at 'cell_index' to the given
|
||||
// 'probability'. Only allowed if the cell was unknown before.
|
||||
void SetProbability(const Eigen::Array2i& cell_index,
|
||||
const float probability) {
|
||||
uint16& cell = cells_[ToFlatIndex(cell_index)];
|
||||
CHECK_EQ(cell, mapping::kUnknownProbabilityValue);
|
||||
cell = mapping::ProbabilityToValue(probability);
|
||||
known_cells_box_.extend(cell_index.matrix());
|
||||
}
|
||||
const float probability);
|
||||
|
||||
// Applies the 'odds' specified when calling ComputeLookupTableToApplyOdds()
|
||||
// to the probability of the cell at 'cell_index' if the cell has not already
|
||||
|
|
@ -90,108 +52,27 @@ class ProbabilityGrid {
|
|||
// If this is the first call to ApplyOdds() for the specified cell, its value
|
||||
// will be set to probability corresponding to 'odds'.
|
||||
bool ApplyLookupTable(const Eigen::Array2i& cell_index,
|
||||
const std::vector<uint16>& table) {
|
||||
DCHECK_EQ(table.size(), mapping::kUpdateMarker);
|
||||
const int flat_index = ToFlatIndex(cell_index);
|
||||
uint16& cell = cells_[flat_index];
|
||||
if (cell >= mapping::kUpdateMarker) {
|
||||
return false;
|
||||
}
|
||||
update_indices_.push_back(flat_index);
|
||||
cell = table[cell];
|
||||
DCHECK_GE(cell, mapping::kUpdateMarker);
|
||||
known_cells_box_.extend(cell_index.matrix());
|
||||
return true;
|
||||
}
|
||||
const std::vector<uint16>& table);
|
||||
|
||||
// Returns the probability of the cell with 'cell_index'.
|
||||
float GetProbability(const Eigen::Array2i& cell_index) const {
|
||||
if (limits_.Contains(cell_index)) {
|
||||
return mapping::ValueToProbability(cells_[ToFlatIndex(cell_index)]);
|
||||
}
|
||||
return mapping::kMinProbability;
|
||||
}
|
||||
float GetProbability(const Eigen::Array2i& cell_index) const;
|
||||
|
||||
// Returns true if the probability at the specified index is known.
|
||||
bool IsKnown(const Eigen::Array2i& cell_index) const {
|
||||
return limits_.Contains(cell_index) &&
|
||||
cells_[ToFlatIndex(cell_index)] != mapping::kUnknownProbabilityValue;
|
||||
}
|
||||
bool IsKnown(const Eigen::Array2i& cell_index) const;
|
||||
|
||||
// Fills in 'offset' and 'limits' to define a subregion of that contains all
|
||||
// known cells.
|
||||
void ComputeCroppedLimits(Eigen::Array2i* const offset,
|
||||
CellLimits* const limits) const {
|
||||
if (known_cells_box_.isEmpty()) {
|
||||
*offset = Eigen::Array2i::Zero();
|
||||
*limits = CellLimits(1, 1);
|
||||
} else {
|
||||
*offset = known_cells_box_.min().array();
|
||||
*limits = CellLimits(known_cells_box_.sizes().x() + 1,
|
||||
known_cells_box_.sizes().y() + 1);
|
||||
}
|
||||
}
|
||||
CellLimits* const limits) const;
|
||||
|
||||
// Grows the map as necessary to include 'point'. This changes the meaning of
|
||||
// these coordinates going forward. This method must be called immediately
|
||||
// after 'FinishUpdate', before any calls to 'ApplyLookupTable'.
|
||||
void GrowLimits(const Eigen::Vector2f& point) {
|
||||
CHECK(update_indices_.empty());
|
||||
while (!limits_.Contains(limits_.GetCellIndex(point))) {
|
||||
const int x_offset = limits_.cell_limits().num_x_cells / 2;
|
||||
const int y_offset = limits_.cell_limits().num_y_cells / 2;
|
||||
const MapLimits new_limits(
|
||||
limits_.resolution(),
|
||||
limits_.max() +
|
||||
limits_.resolution() * Eigen::Vector2d(y_offset, x_offset),
|
||||
CellLimits(2 * limits_.cell_limits().num_x_cells,
|
||||
2 * limits_.cell_limits().num_y_cells));
|
||||
const int stride = new_limits.cell_limits().num_x_cells;
|
||||
const int offset = x_offset + stride * y_offset;
|
||||
const int new_size = new_limits.cell_limits().num_x_cells *
|
||||
new_limits.cell_limits().num_y_cells;
|
||||
std::vector<uint16> new_cells(new_size,
|
||||
mapping::kUnknownProbabilityValue);
|
||||
for (int i = 0; i < limits_.cell_limits().num_y_cells; ++i) {
|
||||
for (int j = 0; j < limits_.cell_limits().num_x_cells; ++j) {
|
||||
new_cells[offset + j + i * stride] =
|
||||
cells_[j + i * limits_.cell_limits().num_x_cells];
|
||||
}
|
||||
}
|
||||
cells_ = new_cells;
|
||||
limits_ = new_limits;
|
||||
if (!known_cells_box_.isEmpty()) {
|
||||
known_cells_box_.translate(Eigen::Vector2i(x_offset, y_offset));
|
||||
}
|
||||
}
|
||||
}
|
||||
void GrowLimits(const Eigen::Vector2f& point);
|
||||
|
||||
proto::ProbabilityGrid ToProto() const {
|
||||
proto::ProbabilityGrid result;
|
||||
*result.mutable_limits() = cartographer::mapping_2d::ToProto(limits_);
|
||||
result.mutable_cells()->Reserve(cells_.size());
|
||||
for (const auto cell : cells_) {
|
||||
result.mutable_cells()->Add(cell);
|
||||
}
|
||||
CHECK(update_indices_.empty()) << "Serializing a grid during an update is "
|
||||
"not supported. Finish the update first.";
|
||||
if (!known_cells_box_.isEmpty()) {
|
||||
auto* const box = result.mutable_known_cells_box();
|
||||
box->set_max_x(known_cells_box_.max().x());
|
||||
box->set_max_y(known_cells_box_.max().y());
|
||||
box->set_min_x(known_cells_box_.min().x());
|
||||
box->set_min_y(known_cells_box_.min().y());
|
||||
}
|
||||
return result;
|
||||
}
|
||||
proto::ProbabilityGrid ToProto() const;
|
||||
|
||||
private:
|
||||
// Converts a 'cell_index' into an index into 'cells_'.
|
||||
int ToFlatIndex(const Eigen::Array2i& cell_index) const {
|
||||
CHECK(limits_.Contains(cell_index)) << cell_index;
|
||||
return limits_.cell_limits().num_x_cells * cell_index.y() + cell_index.x();
|
||||
}
|
||||
|
||||
MapLimits limits_;
|
||||
std::vector<uint16> cells_; // Highest bit is update marker.
|
||||
std::vector<int> update_indices_;
|
||||
|
|
|
|||
|
|
@ -17,14 +17,17 @@
|
|||
#include "cartographer/mapping_2d/probability_grid.h"
|
||||
|
||||
#include <random>
|
||||
#include <vector>
|
||||
|
||||
#include "cartographer/mapping/probability_values.h"
|
||||
#include "gtest/gtest.h"
|
||||
|
||||
namespace cartographer {
|
||||
namespace mapping_2d {
|
||||
namespace {
|
||||
|
||||
using Eigen::Array2i;
|
||||
using Eigen::Vector2f;
|
||||
|
||||
TEST(ProbabilityGridTest, ProtoConstructor) {
|
||||
proto::ProbabilityGrid proto;
|
||||
const MapLimits limits(1., {2., 3.}, CellLimits(4., 5.));
|
||||
|
|
@ -61,49 +64,47 @@ TEST(ProbabilityGridTest, ApplyOdds) {
|
|||
MapLimits(1., Eigen::Vector2d(1., 1.), CellLimits(2, 2)));
|
||||
const MapLimits& limits = probability_grid.limits();
|
||||
|
||||
EXPECT_TRUE(limits.Contains(Eigen::Array2i(0, 0)));
|
||||
EXPECT_TRUE(limits.Contains(Eigen::Array2i(0, 1)));
|
||||
EXPECT_TRUE(limits.Contains(Eigen::Array2i(1, 0)));
|
||||
EXPECT_TRUE(limits.Contains(Eigen::Array2i(1, 1)));
|
||||
EXPECT_FALSE(probability_grid.IsKnown(Eigen::Array2i(0, 0)));
|
||||
EXPECT_FALSE(probability_grid.IsKnown(Eigen::Array2i(0, 1)));
|
||||
EXPECT_FALSE(probability_grid.IsKnown(Eigen::Array2i(1, 0)));
|
||||
EXPECT_FALSE(probability_grid.IsKnown(Eigen::Array2i(1, 1)));
|
||||
EXPECT_TRUE(limits.Contains(Array2i(0, 0)));
|
||||
EXPECT_TRUE(limits.Contains(Array2i(0, 1)));
|
||||
EXPECT_TRUE(limits.Contains(Array2i(1, 0)));
|
||||
EXPECT_TRUE(limits.Contains(Array2i(1, 1)));
|
||||
EXPECT_FALSE(probability_grid.IsKnown(Array2i(0, 0)));
|
||||
EXPECT_FALSE(probability_grid.IsKnown(Array2i(0, 1)));
|
||||
EXPECT_FALSE(probability_grid.IsKnown(Array2i(1, 0)));
|
||||
EXPECT_FALSE(probability_grid.IsKnown(Array2i(1, 1)));
|
||||
|
||||
probability_grid.SetProbability(Eigen::Array2i(1, 0), 0.5);
|
||||
probability_grid.SetProbability(Array2i(1, 0), 0.5);
|
||||
|
||||
probability_grid.ApplyLookupTable(
|
||||
Eigen::Array2i(1, 0),
|
||||
Array2i(1, 0),
|
||||
mapping::ComputeLookupTableToApplyOdds(mapping::Odds(0.9)));
|
||||
probability_grid.FinishUpdate();
|
||||
EXPECT_GT(probability_grid.GetProbability(Eigen::Array2i(1, 0)), 0.5);
|
||||
EXPECT_GT(probability_grid.GetProbability(Array2i(1, 0)), 0.5);
|
||||
|
||||
probability_grid.SetProbability(Eigen::Array2i(0, 1), 0.5);
|
||||
probability_grid.SetProbability(Array2i(0, 1), 0.5);
|
||||
|
||||
probability_grid.ApplyLookupTable(
|
||||
Eigen::Array2i(0, 1),
|
||||
Array2i(0, 1),
|
||||
mapping::ComputeLookupTableToApplyOdds(mapping::Odds(0.1)));
|
||||
probability_grid.FinishUpdate();
|
||||
EXPECT_LT(probability_grid.GetProbability(Eigen::Array2i(0, 1)), 0.5);
|
||||
EXPECT_LT(probability_grid.GetProbability(Array2i(0, 1)), 0.5);
|
||||
|
||||
// Tests adding odds to an unknown cell.
|
||||
probability_grid.ApplyLookupTable(
|
||||
Eigen::Array2i(1, 1),
|
||||
Array2i(1, 1),
|
||||
mapping::ComputeLookupTableToApplyOdds(mapping::Odds(0.42)));
|
||||
EXPECT_NEAR(probability_grid.GetProbability(Eigen::Array2i(1, 1)), 0.42,
|
||||
1e-4);
|
||||
EXPECT_NEAR(probability_grid.GetProbability(Array2i(1, 1)), 0.42, 1e-4);
|
||||
|
||||
// Tests that further updates are ignored if FinishUpdate() isn't called.
|
||||
probability_grid.ApplyLookupTable(
|
||||
Eigen::Array2i(1, 1),
|
||||
Array2i(1, 1),
|
||||
mapping::ComputeLookupTableToApplyOdds(mapping::Odds(0.9)));
|
||||
EXPECT_NEAR(probability_grid.GetProbability(Eigen::Array2i(1, 1)), 0.42,
|
||||
1e-4);
|
||||
EXPECT_NEAR(probability_grid.GetProbability(Array2i(1, 1)), 0.42, 1e-4);
|
||||
probability_grid.FinishUpdate();
|
||||
probability_grid.ApplyLookupTable(
|
||||
Eigen::Array2i(1, 1),
|
||||
Array2i(1, 1),
|
||||
mapping::ComputeLookupTableToApplyOdds(mapping::Odds(0.9)));
|
||||
EXPECT_GT(probability_grid.GetProbability(Eigen::Array2i(1, 1)), 0.42);
|
||||
EXPECT_GT(probability_grid.GetProbability(Array2i(1, 1)), 0.42);
|
||||
}
|
||||
|
||||
TEST(ProbabilityGridTest, GetProbability) {
|
||||
|
|
@ -118,16 +119,14 @@ TEST(ProbabilityGridTest, GetProbability) {
|
|||
ASSERT_EQ(2, cell_limits.num_x_cells);
|
||||
ASSERT_EQ(2, cell_limits.num_y_cells);
|
||||
|
||||
probability_grid.SetProbability(
|
||||
limits.GetCellIndex(Eigen::Vector2f(-0.5f, 0.5f)),
|
||||
probability_grid.SetProbability(limits.GetCellIndex(Vector2f(-0.5f, 0.5f)),
|
||||
mapping::kMaxProbability);
|
||||
EXPECT_NEAR(probability_grid.GetProbability(
|
||||
limits.GetCellIndex(Eigen::Vector2f(-0.5f, 0.5f))),
|
||||
limits.GetCellIndex(Vector2f(-0.5f, 0.5f))),
|
||||
mapping::kMaxProbability, 1e-6);
|
||||
for (const Eigen::Array2i& xy_index :
|
||||
{limits.GetCellIndex(Eigen::Vector2f(-0.5f, 1.5f)),
|
||||
limits.GetCellIndex(Eigen::Vector2f(0.5f, 0.5f)),
|
||||
limits.GetCellIndex(Eigen::Vector2f(0.5f, 1.5f))}) {
|
||||
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(probability_grid.IsKnown(xy_index));
|
||||
}
|
||||
|
|
@ -142,34 +141,25 @@ TEST(ProbabilityGridTest, GetCellIndex) {
|
|||
ASSERT_EQ(14, cell_limits.num_x_cells);
|
||||
ASSERT_EQ(8, cell_limits.num_y_cells);
|
||||
EXPECT_TRUE(
|
||||
(Eigen::Array2i(0, 0) == limits.GetCellIndex(Eigen::Vector2f(7.f, 13.f)))
|
||||
.all());
|
||||
EXPECT_TRUE((Eigen::Array2i(13, 0) ==
|
||||
limits.GetCellIndex(Eigen::Vector2f(7.f, -13.f)))
|
||||
.all());
|
||||
(Array2i(0, 0) == limits.GetCellIndex(Vector2f(7.f, 13.f))).all());
|
||||
EXPECT_TRUE(
|
||||
(Eigen::Array2i(0, 7) == limits.GetCellIndex(Eigen::Vector2f(-7.f, 13.f)))
|
||||
.all());
|
||||
EXPECT_TRUE((Eigen::Array2i(13, 7) ==
|
||||
limits.GetCellIndex(Eigen::Vector2f(-7.f, -13.f)))
|
||||
.all());
|
||||
(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(
|
||||
(Eigen::Array2i(6, 3) == limits.GetCellIndex(Eigen::Vector2f(0.5f, 0.5f)))
|
||||
.all());
|
||||
(Array2i(6, 3) == limits.GetCellIndex(Vector2f(0.5f, 0.5f))).all());
|
||||
EXPECT_TRUE(
|
||||
(Eigen::Array2i(6, 3) == limits.GetCellIndex(Eigen::Vector2f(1.5f, 1.5f)))
|
||||
.all());
|
||||
EXPECT_TRUE((Eigen::Array2i(7, 3) ==
|
||||
limits.GetCellIndex(Eigen::Vector2f(0.5f, -0.5f)))
|
||||
.all());
|
||||
EXPECT_TRUE((Eigen::Array2i(6, 4) ==
|
||||
limits.GetCellIndex(Eigen::Vector2f(-0.5f, 0.5f)))
|
||||
.all());
|
||||
EXPECT_TRUE((Eigen::Array2i(7, 4) ==
|
||||
limits.GetCellIndex(Eigen::Vector2f(-0.5f, -0.5f)))
|
||||
.all());
|
||||
(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(ProbabilityGridTest, CorrectCropping) {
|
||||
|
|
@ -179,14 +169,14 @@ TEST(ProbabilityGridTest, CorrectCropping) {
|
|||
mapping::kMinProbability, mapping::kMaxProbability);
|
||||
ProbabilityGrid probability_grid(
|
||||
MapLimits(0.05, Eigen::Vector2d(10., 10.), CellLimits(400, 400)));
|
||||
for (const Eigen::Array2i& xy_index : XYIndexRangeIterator(
|
||||
Eigen::Array2i(100, 100), Eigen::Array2i(299, 299))) {
|
||||
for (const Array2i& xy_index :
|
||||
XYIndexRangeIterator(Array2i(100, 100), Array2i(299, 299))) {
|
||||
probability_grid.SetProbability(xy_index, value_distribution(rng));
|
||||
}
|
||||
Eigen::Array2i offset;
|
||||
Array2i offset;
|
||||
CellLimits limits;
|
||||
probability_grid.ComputeCroppedLimits(&offset, &limits);
|
||||
EXPECT_TRUE((offset == Eigen::Array2i(100, 100)).all());
|
||||
EXPECT_TRUE((offset == Array2i(100, 100)).all());
|
||||
EXPECT_EQ(limits.num_x_cells, 200);
|
||||
EXPECT_EQ(limits.num_y_cells, 200);
|
||||
}
|
||||
|
|
|
|||
|
|
@ -20,6 +20,7 @@
|
|||
|
||||
#include "Eigen/Core"
|
||||
#include "Eigen/Geometry"
|
||||
#include "cartographer/mapping/probability_values.h"
|
||||
#include "cartographer/mapping_2d/ray_casting.h"
|
||||
#include "cartographer/mapping_2d/xy_index.h"
|
||||
#include "glog/logging.h"
|
||||
|
|
|
|||
|
|
@ -21,6 +21,7 @@
|
|||
#include "cartographer/common/lua_parameter_dictionary.h"
|
||||
#include "cartographer/common/lua_parameter_dictionary_test_helpers.h"
|
||||
#include "cartographer/common/make_unique.h"
|
||||
#include "cartographer/mapping/probability_values.h"
|
||||
#include "cartographer/mapping_2d/probability_grid.h"
|
||||
#include "gmock/gmock.h"
|
||||
|
||||
|
|
|
|||
|
|
@ -21,6 +21,7 @@
|
|||
#include "cartographer/common/lua_parameter_dictionary.h"
|
||||
#include "cartographer/common/lua_parameter_dictionary_test_helpers.h"
|
||||
#include "cartographer/common/make_unique.h"
|
||||
#include "cartographer/mapping/probability_values.h"
|
||||
#include "cartographer/mapping_2d/probability_grid.h"
|
||||
#include "cartographer/sensor/point_cloud.h"
|
||||
#include "cartographer/transform/rigid_transform_test_helpers.h"
|
||||
|
|
|
|||
|
|
@ -30,6 +30,7 @@
|
|||
|
||||
#include "Eigen/Core"
|
||||
#include "cartographer/common/port.h"
|
||||
#include "cartographer/mapping/probability_values.h"
|
||||
#include "cartographer/mapping_2d/probability_grid.h"
|
||||
#include "cartographer/mapping_2d/scan_matching/correlative_scan_matcher.h"
|
||||
#include "cartographer/mapping_2d/scan_matching/proto/fast_correlative_scan_matcher_options.pb.h"
|
||||
|
|
|
|||
Loading…
Reference in New Issue