cartographer/cartographer/mapping/3d/hybrid_grid_test.cc

248 lines
9.3 KiB
C++

/*
* Copyright 2016 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/3d/hybrid_grid.h"
#include <map>
#include <random>
#include <tuple>
#include "gmock/gmock.h"
namespace cartographer {
namespace mapping {
namespace {
TEST(HybridGridTest, ApplyOdds) {
HybridGrid hybrid_grid(1.f);
EXPECT_FALSE(hybrid_grid.IsKnown(Eigen::Array3i(0, 0, 0)));
EXPECT_FALSE(hybrid_grid.IsKnown(Eigen::Array3i(0, 1, 0)));
EXPECT_FALSE(hybrid_grid.IsKnown(Eigen::Array3i(1, 0, 0)));
EXPECT_FALSE(hybrid_grid.IsKnown(Eigen::Array3i(1, 1, 0)));
EXPECT_FALSE(hybrid_grid.IsKnown(Eigen::Array3i(0, 0, 1)));
EXPECT_FALSE(hybrid_grid.IsKnown(Eigen::Array3i(0, 1, 1)));
EXPECT_FALSE(hybrid_grid.IsKnown(Eigen::Array3i(1, 0, 1)));
EXPECT_FALSE(hybrid_grid.IsKnown(Eigen::Array3i(1, 1, 1)));
hybrid_grid.SetProbability(Eigen::Array3i(1, 0, 1), 0.5f);
hybrid_grid.ApplyLookupTable(Eigen::Array3i(1, 0, 1),
ComputeLookupTableToApplyOdds(Odds(0.9f)));
hybrid_grid.FinishUpdate();
EXPECT_GT(hybrid_grid.GetProbability(Eigen::Array3i(1, 0, 1)), 0.5f);
hybrid_grid.SetProbability(Eigen::Array3i(0, 1, 0), 0.5f);
hybrid_grid.ApplyLookupTable(Eigen::Array3i(0, 1, 0),
ComputeLookupTableToApplyOdds(Odds(0.1f)));
hybrid_grid.FinishUpdate();
EXPECT_LT(hybrid_grid.GetProbability(Eigen::Array3i(0, 1, 0)), 0.5f);
// Tests adding odds to an unknown cell.
hybrid_grid.ApplyLookupTable(Eigen::Array3i(1, 1, 1),
ComputeLookupTableToApplyOdds(Odds(0.42f)));
EXPECT_NEAR(hybrid_grid.GetProbability(Eigen::Array3i(1, 1, 1)), 0.42f, 1e-4);
// Tests that further updates are ignored if FinishUpdate() isn't called.
hybrid_grid.ApplyLookupTable(Eigen::Array3i(1, 1, 1),
ComputeLookupTableToApplyOdds(Odds(0.9f)));
EXPECT_NEAR(hybrid_grid.GetProbability(Eigen::Array3i(1, 1, 1)), 0.42f, 1e-4);
hybrid_grid.FinishUpdate();
hybrid_grid.ApplyLookupTable(Eigen::Array3i(1, 1, 1),
ComputeLookupTableToApplyOdds(Odds(0.9f)));
EXPECT_GT(hybrid_grid.GetProbability(Eigen::Array3i(1, 1, 1)), 0.42f);
}
TEST(HybridGridTest, GetProbability) {
HybridGrid hybrid_grid(1.f);
hybrid_grid.SetProbability(
hybrid_grid.GetCellIndex(Eigen::Vector3f(0.f, 1.f, 1.f)),
kMaxProbability);
EXPECT_NEAR(hybrid_grid.GetProbability(
hybrid_grid.GetCellIndex(Eigen::Vector3f(0.f, 1.f, 1.f))),
kMaxProbability, 1e-6);
for (const Eigen::Array3i& index :
{hybrid_grid.GetCellIndex(Eigen::Vector3f(0.f, 2.f, 1.f)),
hybrid_grid.GetCellIndex(Eigen::Vector3f(1.f, 1.f, 1.f)),
hybrid_grid.GetCellIndex(Eigen::Vector3f(1.f, 2.f, 1.f))}) {
EXPECT_FALSE(hybrid_grid.IsKnown(index));
}
}
TEST(HybridGridTest, GetIntensity) {
IntensityHybridGrid hybrid_grid(1.f);
const Eigen::Array3i cell_index =
hybrid_grid.GetCellIndex(Eigen::Vector3f(0.f, 1.f, 1.f));
const float intensity = 58.0f;
EXPECT_NEAR(hybrid_grid.GetIntensity(cell_index), 0.0f, 1e-9);
hybrid_grid.AddIntensity(cell_index, intensity);
EXPECT_NEAR(hybrid_grid.GetIntensity(cell_index), intensity, 1e-9);
for (const Eigen::Array3i& index :
{hybrid_grid.GetCellIndex(Eigen::Vector3f(0.f, 2.f, 1.f)),
hybrid_grid.GetCellIndex(Eigen::Vector3f(1.f, 1.f, 1.f)),
hybrid_grid.GetCellIndex(Eigen::Vector3f(1.f, 2.f, 1.f))}) {
EXPECT_NEAR(hybrid_grid.GetIntensity(index), 0.0f, 1e-9);
}
}
MATCHER_P(AllCwiseEqual, index, "") { return (arg == index).all(); }
TEST(HybridGridTest, GetCellIndex) {
HybridGrid hybrid_grid(2.f);
EXPECT_THAT(hybrid_grid.GetCellIndex(Eigen::Vector3f(0.f, 0.f, 0.f)),
AllCwiseEqual(Eigen::Array3i(0, 0, 0)));
EXPECT_THAT(hybrid_grid.GetCellIndex(Eigen::Vector3f(0.f, 26.f, 10.f)),
AllCwiseEqual(Eigen::Array3i(0, 13, 5)));
EXPECT_THAT(hybrid_grid.GetCellIndex(Eigen::Vector3f(14.f, 0.f, 10.f)),
AllCwiseEqual(Eigen::Array3i(7, 0, 5)));
EXPECT_THAT(hybrid_grid.GetCellIndex(Eigen::Vector3f(14.f, 26.f, 0.f)),
AllCwiseEqual(Eigen::Array3i(7, 13, 0)));
// Check around the origin.
EXPECT_THAT(hybrid_grid.GetCellIndex(Eigen::Vector3f(8.5f, 11.5f, 0.5f)),
AllCwiseEqual(Eigen::Array3i(4, 6, 0)));
EXPECT_THAT(hybrid_grid.GetCellIndex(Eigen::Vector3f(7.5f, 12.5f, 1.5f)),
AllCwiseEqual(Eigen::Array3i(4, 6, 1)));
EXPECT_THAT(hybrid_grid.GetCellIndex(Eigen::Vector3f(6.5f, 14.5f, 2.5f)),
AllCwiseEqual(Eigen::Array3i(3, 7, 1)));
EXPECT_THAT(hybrid_grid.GetCellIndex(Eigen::Vector3f(5.5f, 13.5f, 3.5f)),
AllCwiseEqual(Eigen::Array3i(3, 7, 2)));
}
TEST(HybridGridTest, GetCenterOfCell) {
HybridGrid hybrid_grid(2.f);
const Eigen::Array3i index(3, 2, 1);
const Eigen::Vector3f center = hybrid_grid.GetCenterOfCell(index);
EXPECT_NEAR(6.f, center.x(), 1e-6);
EXPECT_NEAR(4.f, center.y(), 1e-6);
EXPECT_NEAR(2.f, center.z(), 1e-6);
EXPECT_THAT(hybrid_grid.GetCellIndex(center), AllCwiseEqual(index));
}
class RandomHybridGridTest : public ::testing::Test {
public:
RandomHybridGridTest() : hybrid_grid_(2.f), values_() {
std::mt19937 rng(1285120005);
std::uniform_real_distribution<float> value_distribution(kMinProbability,
kMaxProbability);
std::uniform_int_distribution<int> xyz_distribution(-3000, 2999);
for (int i = 0; i < 10000; ++i) {
const auto x = xyz_distribution(rng);
const auto y = xyz_distribution(rng);
const auto z = xyz_distribution(rng);
values_.emplace(std::make_tuple(x, y, z), value_distribution(rng));
}
for (const auto& pair : values_) {
const Eigen::Array3i cell_index(std::get<0>(pair.first),
std::get<1>(pair.first),
std::get<2>(pair.first));
hybrid_grid_.SetProbability(cell_index, pair.second);
}
}
protected:
HybridGrid hybrid_grid_;
using ValueMap = std::map<std::tuple<int, int, int>, float>;
ValueMap values_;
};
TEST_F(RandomHybridGridTest, TestIteration) {
for (auto it = HybridGrid::Iterator(hybrid_grid_); !it.Done(); it.Next()) {
const Eigen::Array3i cell_index = it.GetCellIndex();
const float iterator_probability = ValueToProbability(it.GetValue());
EXPECT_EQ(iterator_probability, hybrid_grid_.GetProbability(cell_index));
const std::tuple<int, int, int> key =
std::make_tuple(cell_index[0], cell_index[1], cell_index[2]);
EXPECT_TRUE(values_.count(key));
EXPECT_NEAR(values_[key], iterator_probability, 1e-4);
values_.erase(key);
}
// Test that range based loop is equivalent to using the iterator.
auto it = HybridGrid::Iterator(hybrid_grid_);
for (const auto& cell : hybrid_grid_) {
ASSERT_FALSE(it.Done());
EXPECT_THAT(cell.first, AllCwiseEqual(it.GetCellIndex()));
EXPECT_EQ(cell.second, it.GetValue());
it.Next();
}
// Now 'values_' must not contain values.
for (const auto& pair : values_) {
const Eigen::Array3i cell_index(std::get<0>(pair.first),
std::get<1>(pair.first),
std::get<2>(pair.first));
ADD_FAILURE() << cell_index << " Probability: " << pair.second;
}
}
TEST_F(RandomHybridGridTest, ToProto) {
const auto proto = hybrid_grid_.ToProto();
EXPECT_EQ(hybrid_grid_.resolution(), proto.resolution());
ASSERT_EQ(proto.x_indices_size(), proto.y_indices_size());
ASSERT_EQ(proto.x_indices_size(), proto.z_indices_size());
ASSERT_EQ(proto.x_indices_size(), proto.values_size());
ValueMap proto_map;
for (int i = 0; i < proto.x_indices_size(); ++i) {
proto_map[std::make_tuple(proto.x_indices(i), proto.y_indices(i),
proto.z_indices(i))] = proto.values(i);
}
// Get hybrid_grid_ into the same format.
ValueMap hybrid_grid_map;
for (const auto i : hybrid_grid_) {
hybrid_grid_map[std::make_tuple(i.first.x(), i.first.y(), i.first.z())] =
i.second;
}
EXPECT_EQ(proto_map, hybrid_grid_map);
}
struct EigenComparator {
bool operator()(const Eigen::Vector3i& lhs,
const Eigen::Vector3i& rhs) const {
return std::forward_as_tuple(lhs.x(), lhs.y(), lhs.z()) <
std::forward_as_tuple(rhs.x(), rhs.y(), rhs.z());
}
};
TEST_F(RandomHybridGridTest, FromProto) {
const HybridGrid constructed_grid(hybrid_grid_.ToProto());
std::map<Eigen::Vector3i, float, EigenComparator> member_map;
for (const auto& cell : hybrid_grid_) {
member_map.insert(cell);
}
std::map<Eigen::Vector3i, float, EigenComparator> constructed_map;
for (const auto& cell : constructed_grid) {
constructed_map.insert(cell);
}
EXPECT_EQ(member_map, constructed_map);
}
} // namespace
} // namespace mapping
} // namespace cartographer