12345qiupeng
/
cartographer
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Always use the origin as the origin for grids. (#294) 

Also removes convenience functions from mapping_3d::Submap.

PAIR=SirVer
master
Wolfgang Hess 2017-05-18 12:00:45 +02:00 committed by GitHub
parent 33ce5dee37
commit 9033fad1ab
24 changed files with 162 additions and 182 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
cartographer/io/outlier_removing_points_processor.cc
View File

@ -36,7 +36,7 @@ OutlierRemovingPointsProcessor::OutlierRemovingPointsProcessor(
: voxel_size_(voxel_size),
next_(next),
state_(State::kPhase1),
voxels_(voxel_size_, Eigen::Vector3f::Zero()) {
voxels_(voxel_size_) {
LOG(INFO) << "Marking hits...";
}

5
cartographer/io/xray_points_processor.cc
View File

@ -142,9 +142,8 @@ XRayPointsProcessor::XRayPointsProcessor(
output_filename_(output_filename),
transform_(transform) {
for (size_t i = 0; i < (floors_.empty() ? 1 : floors.size()); ++i) {
aggregations_.emplace_back(Aggregation{
mapping_3d::HybridGridBase<bool>(voxel_size, Eigen::Vector3f::Zero()),
{}});
aggregations_.emplace_back(
Aggregation{mapping_3d::HybridGridBase<bool>(voxel_size), {}});
}
}

30
cartographer/mapping_3d/hybrid_grid.h
View File

@ -415,20 +415,18 @@ class HybridGridBase : public Grid<ValueType> {
public:
using Iterator = typename Grid<ValueType>::Iterator;
// Creates a new tree-based probability grid around 'origin' which becomes the
// center of the cell at index (0, 0, 0). Each voxel has edge length
// 'resolution'.
HybridGridBase(const float resolution, const Eigen::Vector3f& origin)
: resolution_(resolution), origin_(origin) {}
// Creates a new tree-based probability grid with voxels having edge length
// 'resolution' around the origin which becomes the center of the cell at
// index (0, 0, 0).
explicit HybridGridBase(const float resolution) : resolution_(resolution) {}
float resolution() const { return resolution_; }
Eigen::Vector3f origin() const { return origin_; }
// Returns the index of the cell containing the 'point'. Indices are integer
// vectors identifying cells, for this the relative distance from the origin
// is rounded to the next multiple of the resolution.
// vectors identifying cells, for this the coordinates are rounded to the next
// multiple of the resolution.
Eigen::Array3i GetCellIndex(const Eigen::Vector3f& point) const {
Eigen::Array3f index = (point - origin_).array() / resolution_;
Eigen::Array3f index = point.array() / resolution_;
return Eigen::Array3i(common::RoundToInt(index.x()),
common::RoundToInt(index.y()),
common::RoundToInt(index.z()));
@ -444,7 +442,7 @@ class HybridGridBase : public Grid<ValueType> {
// Returns the center of the cell at 'index'.
Eigen::Vector3f GetCenterOfCell(const Eigen::Array3i& index) const {
return index.matrix().cast<float>() * resolution_ + origin_;
return index.matrix().cast<float>() * resolution_;
}
// Iterator functions for range-based for loops.
@ -459,20 +457,17 @@ class HybridGridBase : public Grid<ValueType> {
private:
// Edge length of each voxel.
const float resolution_;
// Position of the center of the octree.
const Eigen::Vector3f origin_;
};
// A grid containing probability values stored using 15 bits, and an update
// marker per voxel.
class HybridGrid : public HybridGridBase<uint16> {
public:
HybridGrid(const float resolution, const Eigen::Vector3f& origin)
: HybridGridBase<uint16>(resolution, origin) {}
explicit HybridGrid(const float resolution)
: HybridGridBase<uint16>(resolution) {}
HybridGrid(const proto::HybridGrid& proto)
: HybridGrid(proto.resolution(), transform::ToEigen(proto.origin())) {
explicit HybridGrid(const proto::HybridGrid& proto)
: HybridGrid(proto.resolution()) {
CHECK_EQ(proto.values_size(), proto.x_indices_size());
CHECK_EQ(proto.values_size(), proto.y_indices_size());
CHECK_EQ(proto.values_size(), proto.z_indices_size());
@ -535,7 +530,6 @@ class HybridGrid : public HybridGridBase<uint16> {
inline proto::HybridGrid ToProto(const HybridGrid& grid) {
proto::HybridGrid result;
result.set_resolution(grid.resolution());
*result.mutable_origin() = transform::ToProto(grid.origin());
for (const auto it : grid) {
result.add_x_indices(it.first.x());
result.add_y_indices(it.first.y());

46
cartographer/mapping_3d/hybrid_grid_test.cc
View File

@ -27,7 +27,7 @@ namespace mapping_3d {
namespace {
TEST(HybridGridTest, ApplyOdds) {
HybridGrid hybrid_grid(1.f, Eigen::Vector3f(-0.5f, -0.5f, -0.5f));
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)));
@ -74,18 +74,18 @@ TEST(HybridGridTest, ApplyOdds) {
}
TEST(HybridGridTest, GetProbability) {
HybridGrid hybrid_grid(1.f, Eigen::Vector3f(-0.5f, -0.5f, -0.5f));
HybridGrid hybrid_grid(1.f);
hybrid_grid.SetProbability(
hybrid_grid.GetCellIndex(Eigen::Vector3f(-0.5f, 0.5f, 0.5f)),
hybrid_grid.GetCellIndex(Eigen::Vector3f(0.f, 1.f, 1.f)),
mapping::kMaxProbability);
EXPECT_NEAR(hybrid_grid.GetProbability(
hybrid_grid.GetCellIndex(Eigen::Vector3f(-0.5f, 0.5f, 0.5f))),
hybrid_grid.GetCellIndex(Eigen::Vector3f(0.f, 1.f, 1.f))),
mapping::kMaxProbability, 1e-6);
for (const Eigen::Array3i& index :
{hybrid_grid.GetCellIndex(Eigen::Vector3f(-0.5f, 1.5, 0.5f)),
hybrid_grid.GetCellIndex(Eigen::Vector3f(.5f, 0.5, 0.5f)),
hybrid_grid.GetCellIndex(Eigen::Vector3f(0.5f, 1.5, 0.5f))}) {
{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));
}
}
@ -93,43 +93,42 @@ TEST(HybridGridTest, GetProbability) {
MATCHER_P(AllCwiseEqual, index, "") { return (arg == index).all(); }
TEST(HybridGridTest, GetCellIndex) {
HybridGrid hybrid_grid(2.f, Eigen::Vector3f(-7.f, -13.f, -2.f));
HybridGrid hybrid_grid(2.f);
EXPECT_THAT(hybrid_grid.GetCellIndex(Eigen::Vector3f(-7.f, -13.f, -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(-7.f, 13.f, 8.f)),
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(7.f, -13.f, 8.f)),
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(7.f, 13.f, -2.f)),
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(1.5f, -1.5f, -1.5f)),
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(0.5f, -0.5f, -0.5f)),
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(-0.5f, 1.5f, 0.5f)),
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(-1.5f, 0.5f, 1.5f)),
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, Eigen::Vector3f(-7.f, -13.f, -2.f));
HybridGrid hybrid_grid(2.f);
const Eigen::Array3i index(3, 2, 1);
const Eigen::Vector3f center = hybrid_grid.GetCenterOfCell(index);
EXPECT_NEAR(-1.f, center.x(), 1e-6);
EXPECT_NEAR(-9.f, center.y(), 1e-6);
EXPECT_NEAR(0.f, center.z(), 1e-6);
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, Eigen::Vector3f(-7.f, -12.f, 0.f)), values_() {
RandomHybridGridTest() : hybrid_grid_(2.f), values_() {
std::mt19937 rng(1285120005);
std::uniform_real_distribution<float> value_distribution(
mapping::kMinProbability, mapping::kMaxProbability);
@ -189,9 +188,6 @@ TEST_F(RandomHybridGridTest, TestIteration) {
TEST_F(RandomHybridGridTest, ToProto) {
const auto proto = ToProto(hybrid_grid_);
EXPECT_EQ(hybrid_grid_.resolution(), proto.resolution());
EXPECT_EQ(hybrid_grid_.origin().x(), proto.origin().x());
EXPECT_EQ(hybrid_grid_.origin().y(), proto.origin().y());
EXPECT_EQ(hybrid_grid_.origin().z(), proto.origin().z());
ASSERT_EQ(proto.x_indices_size(), proto.y_indices_size());
ASSERT_EQ(proto.x_indices_size(), proto.z_indices_size());

21
cartographer/mapping_3d/kalman_local_trajectory_builder.cc
View File

@ -140,19 +140,24 @@ KalmanLocalTrajectoryBuilder::AddAccumulatedRangeData(
pose_tracker_->GetPoseEstimateMeanAndCovariance(
time, &pose_prediction, &unused_covariance_prediction);
transform::Rigid3d initial_ceres_pose = pose_prediction;
const Submap* const matching_submap =
submaps_->Get(submaps_->matching_index());
transform::Rigid3d initial_ceres_pose =
matching_submap->local_pose().inverse() * pose_prediction;
sensor::AdaptiveVoxelFilter adaptive_voxel_filter(
options_.high_resolution_adaptive_voxel_filter_options());
const sensor::PointCloud filtered_point_cloud_in_tracking =
adaptive_voxel_filter.Filter(filtered_range_data.returns);
if (options_.kalman_local_trajectory_builder_options()
.use_online_correlative_scan_matching()) {
// We take a copy since we use 'intial_ceres_pose' as an output argument.
const transform::Rigid3d initial_pose = initial_ceres_pose;
real_time_correlative_scan_matcher_->Match(
pose_prediction, filtered_point_cloud_in_tracking,
submaps_->high_resolution_matching_grid(), &initial_ceres_pose);
initial_pose, filtered_point_cloud_in_tracking,
matching_submap->high_resolution_hybrid_grid, &initial_ceres_pose);
}
transform::Rigid3d pose_observation;
transform::Rigid3d pose_observation_in_submap;
ceres::Solver::Summary summary;
sensor::AdaptiveVoxelFilter low_resolution_adaptive_voxel_filter(
@ -161,10 +166,12 @@ KalmanLocalTrajectoryBuilder::AddAccumulatedRangeData(
low_resolution_adaptive_voxel_filter.Filter(filtered_range_data.returns);
ceres_scan_matcher_->Match(scan_matcher_pose_estimate_, initial_ceres_pose,
{{&filtered_point_cloud_in_tracking,
&submaps_->high_resolution_matching_grid()},
&matching_submap->high_resolution_hybrid_grid},
{&low_resolution_point_cloud_in_tracking,
&submaps_->low_resolution_matching_grid()}},
&pose_observation, &summary);
&matching_submap->low_resolution_hybrid_grid}},
&pose_observation_in_submap, &summary);
const transform::Rigid3d pose_observation =
matching_submap->local_pose() * pose_observation_in_submap;
pose_tracker_->AddPoseObservation(
time, pose_observation,
options_.kalman_local_trajectory_builder_options()

36
cartographer/mapping_3d/optimizing_local_trajectory_builder.cc
View File

@ -176,7 +176,8 @@ OptimizingLocalTrajectoryBuilder::AddRangefinderData(
batches_.push_back(
Batch{time, point_cloud, high_resolution_filtered_points,
low_resolution_filtered_points,
State{{{1., 0., 0., 0.}}, {{0., 0., 0.}}, {{0., 0., 0.}}}});
State(Eigen::Vector3d::Zero(), Eigen::Quaterniond::Identity(),
Eigen::Vector3d::Zero())});
} else {
const Batch& last_batch = batches_.back();
batches_.push_back(Batch{
@ -214,6 +215,19 @@ void OptimizingLocalTrajectoryBuilder::RemoveObsoleteSensorData() {
}
}
void OptimizingLocalTrajectoryBuilder::TransformStates(
const transform::Rigid3d& transform) {
for (Batch& batch : batches_) {
const transform::Rigid3d new_pose = transform * batch.state.ToRigid();
const auto& velocity = batch.state.velocity;
const Eigen::Vector3d new_velocity =
transform.rotation() *
Eigen::Vector3d(velocity[0], velocity[1], velocity[2]);
batch.state =
State(new_pose.translation(), new_pose.rotation(), new_velocity);
}
}
std::unique_ptr<OptimizingLocalTrajectoryBuilder::InsertionResult>
OptimizingLocalTrajectoryBuilder::MaybeOptimize(const common::Time time) {
// TODO(hrapp): Make the number of optimizations configurable.
@ -223,6 +237,12 @@ OptimizingLocalTrajectoryBuilder::MaybeOptimize(const common::Time time) {
}
ceres::Problem problem;
const Submap* const matching_submap =
submaps_->Get(submaps_->matching_index());
// We transform the states in 'batches_' in place to be in the submap frame as
// expected by the OccupiedSpaceCostFunctor. This is reverted after solving
// the optimization problem.
TransformStates(matching_submap->local_pose().inverse());
for (size_t i = 0; i < batches_.size(); ++i) {
Batch& batch = batches_[i];
problem.AddResidualBlock(
@ -234,7 +254,7 @@ OptimizingLocalTrajectoryBuilder::MaybeOptimize(const common::Time time) {
std::sqrt(static_cast<double>(
batch.high_resolution_filtered_points.size())),
batch.high_resolution_filtered_points,
submaps_->high_resolution_matching_grid()),
matching_submap->high_resolution_hybrid_grid),
batch.high_resolution_filtered_points.size()),
nullptr, batch.state.translation.data(), batch.state.rotation.data());
problem.AddResidualBlock(
@ -246,15 +266,15 @@ OptimizingLocalTrajectoryBuilder::MaybeOptimize(const common::Time time) {
std::sqrt(static_cast<double>(
batch.low_resolution_filtered_points.size())),
batch.low_resolution_filtered_points,
submaps_->low_resolution_matching_grid()),
matching_submap->low_resolution_hybrid_grid),
batch.low_resolution_filtered_points.size()),
nullptr, batch.state.translation.data(), batch.state.rotation.data());
if (i == 0) {
problem.SetParameterBlockConstant(batch.state.translation.data());
problem.SetParameterBlockConstant(batch.state.rotation.data());
problem.AddParameterBlock(batch.state.velocity.data(), 3);
problem.SetParameterBlockConstant(batch.state.velocity.data());
problem.SetParameterBlockConstant(batch.state.rotation.data());
} else {
problem.SetParameterization(batch.state.rotation.data(),
new ceres::QuaternionParameterization());
@ -329,6 +349,9 @@ OptimizingLocalTrajectoryBuilder::MaybeOptimize(const common::Time time) {
ceres::Solver::Summary summary;
ceres::Solve(ceres_solver_options_, &problem, &summary);
// The optimized states in 'batches_' are in the submap frame and we transform
// them in place to be in the local SLAM frame again.
TransformStates(matching_submap->local_pose());
if (num_accumulated_ < options_.scans_per_accumulation()) {
return nullptr;
}
@ -433,10 +456,7 @@ OptimizingLocalTrajectoryBuilder::PredictState(const State& start_state,
start_rotation * result.delta_velocity -
gravity_constant_ * delta_time_seconds * Eigen::Vector3d::UnitZ();
return State{
{{orientation.w(), orientation.x(), orientation.y(), orientation.z()}},
{{position.x(), position.y(), position.z()}},
{{velocity.x(), velocity.y(), velocity.z()}}};
return State(position, orientation, velocity);
}
} // namespace mapping_3d

11
cartographer/mapping_3d/optimizing_local_trajectory_builder.h
View File

@ -61,12 +61,16 @@ class OptimizingLocalTrajectoryBuilder
private:
struct State {
// TODO(hrapp): This should maybe use a CeresPose.
// Rotation quaternion as (w, x, y, z).
std::array<double, 4> rotation;
std::array<double, 3> translation;
std::array<double, 4> rotation; // Rotation quaternion as (w, x, y, z).
std::array<double, 3> velocity;
State(const Eigen::Vector3d& translation,
const Eigen::Quaterniond& rotation, const Eigen::Vector3d& velocity)
: translation{{translation.x(), translation.y(), translation.z()}},
rotation{{rotation.w(), rotation.x(), rotation.y(), rotation.z()}},
velocity{{velocity.x(), velocity.y(), velocity.z()}} {}
Eigen::Quaterniond ToQuaternion() const {
return Eigen::Quaterniond(rotation[0], rotation[1], rotation[2],
rotation[3]);
@ -107,6 +111,7 @@ class OptimizingLocalTrajectoryBuilder
const common::Time time, const sensor::RangeData& range_data_in_tracking,
const transform::Rigid3d& pose_observation);
void TransformStates(const transform::Rigid3d& transform);
std::unique_ptr<InsertionResult> MaybeOptimize(common::Time time);
const proto::LocalTrajectoryBuilderOptions options_;

3
cartographer/mapping_3d/proto/hybrid_grid.proto
View File

@ -14,13 +14,10 @@
syntax = "proto2";
import "cartographer/transform/proto/transform.proto";
package cartographer.mapping_3d.proto;
message HybridGrid {
optional float resolution = 1;
optional transform.proto.Vector3f origin = 2;
// '{x, y, z}_indices[i]' is the index of 'values[i]'.
repeated sint32 x_indices = 3 [packed = true];
repeated sint32 y_indices = 4 [packed = true];

38
cartographer/mapping_3d/range_data_inserter_test.cc
View File

@ -28,8 +28,7 @@ namespace {
class RangeDataInserterTest : public ::testing::Test {
protected:
RangeDataInserterTest()
: hybrid_grid_(1.f, Eigen::Vector3f(0.5f, 0.5f, 0.5f)) {
RangeDataInserterTest() : hybrid_grid_(1.f) {
auto parameter_dictionary = common::MakeDictionary(
"return { "
"hit_probability = 0.7, "
@ -41,11 +40,9 @@ class RangeDataInserterTest : public ::testing::Test {
}
void InsertPointCloud() {
const Eigen::Vector3f origin = Eigen::Vector3f(0.5f, 0.5f, -3.5f);
sensor::PointCloud returns = {{-2.5f, -0.5f, 4.5f},
{-1.5f, 0.5f, 4.5f},
{-0.5f, 1.5f, 4.5f},
{0.5f, 2.5f, 4.5f}};
const Eigen::Vector3f origin = Eigen::Vector3f(0.f, 0.f, -4.f);
sensor::PointCloud returns = {
{-3.f, -1.f, 4.f}, {-2.f, 0.f, 4.f}, {-1.f, 1.f, 4.f}, {0.f, 2.f, 4.f}};
range_data_inserter_->Insert(sensor::RangeData{origin, returns, {}},
&hybrid_grid_);
}
@ -70,19 +67,19 @@ class RangeDataInserterTest : public ::testing::Test {
TEST_F(RangeDataInserterTest, InsertPointCloud) {
InsertPointCloud();
EXPECT_NEAR(options().miss_probability(), GetProbability(0.5f, 0.5f, -3.5f),
EXPECT_NEAR(options().miss_probability(), GetProbability(0.f, 0.f, -4.f),
1e-4);
EXPECT_NEAR(options().miss_probability(), GetProbability(0.5f, 0.5f, -2.5f),
EXPECT_NEAR(options().miss_probability(), GetProbability(0.f, 0.f, -3.f),
1e-4);
EXPECT_NEAR(options().miss_probability(), GetProbability(0.5f, 0.5f, -1.5f),
EXPECT_NEAR(options().miss_probability(), GetProbability(0.f, 0.f, -2.f),
1e-4);
for (int x = -4; x <= 4; ++x) {
for (int y = -4; y <= 4; ++y) {
if (x < -3 || x > 0 || y != x + 2) {
EXPECT_FALSE(IsKnown(x + 0.5f, y + 0.5f, 4.5f));
EXPECT_FALSE(IsKnown(x, y, 4.f));
} else {
EXPECT_NEAR(options().hit_probability(),
GetProbability(x + 0.5f, y + 0.5f, 4.5f), 1e-4);
EXPECT_NEAR(options().hit_probability(), GetProbability(x, y, 4.f),
1e-4);
}
}
}
@ -90,22 +87,19 @@ TEST_F(RangeDataInserterTest, InsertPointCloud) {
TEST_F(RangeDataInserterTest, ProbabilityProgression) {
InsertPointCloud();
EXPECT_NEAR(options().hit_probability(), GetProbability(-1.5f, 0.5f, 4.5f),
EXPECT_NEAR(options().hit_probability(), GetProbability(-2.f, 0.f, 4.f),
1e-4);
EXPECT_NEAR(options().miss_probability(), GetProbability(-1.5f, 0.5f, 3.5f),
EXPECT_NEAR(options().miss_probability(), GetProbability(-2.f, 0.f, 3.f),
1e-4);
EXPECT_NEAR(options().miss_probability(), GetProbability(0.5f, 0.5f, -2.5f),
EXPECT_NEAR(options().miss_probability(), GetProbability(0.f, 0.f, -3.f),
1e-4);
for (int i = 0; i < 1000; ++i) {
InsertPointCloud();
}
EXPECT_NEAR(mapping::kMaxProbability, GetProbability(-1.5f, 0.5f, 4.5f),
1e-3);
EXPECT_NEAR(mapping::kMinProbability, GetProbability(-1.5f, 0.5f, 3.5f),
1e-3);
EXPECT_NEAR(mapping::kMinProbability, GetProbability(0.5f, 0.5f, -2.5f),
1e-3);
EXPECT_NEAR(mapping::kMaxProbability, GetProbability(-2.f, 0.f, 4.f), 1e-3);
EXPECT_NEAR(mapping::kMinProbability, GetProbability(-2.f, 0.f, 3.f), 1e-3);
EXPECT_NEAR(mapping::kMinProbability, GetProbability(0.f, 0.f, -3.f), 1e-3);
}
} // namespace

18
cartographer/mapping_3d/scan_matching/ceres_scan_matcher_test.cc
View File

@ -34,10 +34,9 @@ namespace {
class CeresScanMatcherTest : public ::testing::Test {
protected:
CeresScanMatcherTest()
: hybrid_grid_(1.f /* resolution */,
Eigen::Vector3f(0.5f, 0.5f, 0.5f) /* origin */),
: hybrid_grid_(1.f),
expected_pose_(
transform::Rigid3d::Translation(Eigen::Vector3d(-0.5, 0.5, 0.5))) {
transform::Rigid3d::Translation(Eigen::Vector3d(-1., 0., 0.))) {
for (const auto& point :
{Eigen::Vector3f(-3.f, 2.f, 0.f), Eigen::Vector3f(-4.f, 2.f, 0.f),
Eigen::Vector3f(-5.f, 2.f, 0.f), Eigen::Vector3f(-6.f, 2.f, 0.f),
@ -84,23 +83,23 @@ class CeresScanMatcherTest : public ::testing::Test {
TEST_F(CeresScanMatcherTest, PerfectEstimate) {
TestFromInitialPose(
transform::Rigid3d::Translation(Eigen::Vector3d(-0.5, 0.5, 0.5)));
transform::Rigid3d::Translation(Eigen::Vector3d(-1., 0., 0.)));
}
TEST_F(CeresScanMatcherTest, AlongX) {
ceres_scan_matcher_.reset(new CeresScanMatcher(options_));
TestFromInitialPose(
transform::Rigid3d::Translation(Eigen::Vector3d(-0.3, 0.5, 0.5)));
transform::Rigid3d::Translation(Eigen::Vector3d(-0.8, 0., 0.)));
}
TEST_F(CeresScanMatcherTest, AlongZ) {
TestFromInitialPose(
transform::Rigid3d::Translation(Eigen::Vector3d(-0.5, 0.5, 0.3)));
transform::Rigid3d::Translation(Eigen::Vector3d(-1., 0., -0.2)));
}
TEST_F(CeresScanMatcherTest, AlongXYZ) {
TestFromInitialPose(
transform::Rigid3d::Translation(Eigen::Vector3d(-0.4, 0.3, 0.7)));
transform::Rigid3d::Translation(Eigen::Vector3d(-0.9, -0.2, 0.2)));
}
TEST_F(CeresScanMatcherTest, FullPoseCorrection) {
@ -112,9 +111,8 @@ TEST_F(CeresScanMatcherTest, FullPoseCorrection) {
expected_pose_ = expected_pose_ * additional_transform.inverse();
// ...starting initially with rotation around x.
TestFromInitialPose(
transform::Rigid3d::Translation(Eigen::Vector3d(-0.45, 0.45, 0.55)) *
transform::Rigid3d::Rotation(
Eigen::AngleAxisd(0.05, Eigen::Vector3d(1., 0., 0.))));
transform::Rigid3d(Eigen::Vector3d(-0.95, -0.05, 0.05),
Eigen::AngleAxisd(0.05, Eigen::Vector3d(1., 0., 0.))));
}
} // namespace

3
cartographer/mapping_3d/scan_matching/fast_correlative_scan_matcher.cc
View File

@ -95,7 +95,6 @@ FastCorrelativeScanMatcher::FastCorrelativeScanMatcher(
const proto::FastCorrelativeScanMatcherOptions& options)
: options_(options),
resolution_(hybrid_grid.resolution()),
origin_(hybrid_grid.origin()),
width_in_voxels_(hybrid_grid.grid_size()),
precomputation_grid_stack_(
common::make_unique<PrecomputationGridStack>(hybrid_grid, options)),
@ -122,7 +121,7 @@ bool FastCorrelativeScanMatcher::MatchFullSubmap(
const sensor::PointCloud& coarse_point_cloud,
const sensor::PointCloud& fine_point_cloud, const float min_score,
float* const score, transform::Rigid3d* const pose_estimate) const {
const transform::Rigid3d initial_pose_estimate(origin_.cast<double>(),
const transform::Rigid3d initial_pose_estimate(Eigen::Vector3d::Zero(),
gravity_alignment);
float max_point_distance = 0.f;
for (const Eigen::Vector3f& point : coarse_point_cloud) {

1
cartographer/mapping_3d/scan_matching/fast_correlative_scan_matcher.h
View File

@ -136,7 +136,6 @@ class FastCorrelativeScanMatcher {
const proto::FastCorrelativeScanMatcherOptions options_;
const float resolution_;
const Eigen::Vector3f origin_;
const int width_in_voxels_;
std::unique_ptr<PrecomputationGridStack> precomputation_grid_stack_;
RotationalScanMatcher rotational_scan_matcher_;

3
cartographer/mapping_3d/scan_matching/fast_correlative_scan_matcher_test.cc
View File

@ -87,8 +87,7 @@ TEST(FastCorrelativeScanMatcherTest, CorrectPose) {
transform::Rigid3f::Rotation(
Eigen::AngleAxisf(theta, Eigen::Vector3f::UnitZ()));
HybridGrid hybrid_grid(0.05f /* resolution */,
Eigen::Vector3f(0.5f, 1.5f, 2.5f) /* origin */);
HybridGrid hybrid_grid(0.05f);
hybrid_grid.StartUpdate();
range_data_inserter.Insert(
sensor::RangeData{

4
cartographer/mapping_3d/scan_matching/interpolated_grid_test.cc
View File

@ -28,9 +28,7 @@ namespace {
class InterpolatedGridTest : public ::testing::Test {
protected:
InterpolatedGridTest()
: hybrid_grid_(0.1f /* resolution */,
Eigen::Vector3f(1.5f, 2.5f, 3.5f) /* origin */),
interpolated_grid_(hybrid_grid_) {
: hybrid_grid_(0.1f), interpolated_grid_(hybrid_grid_) {
for (const auto& point :
{Eigen::Vector3f(-3.f, 2.f, 0.f), Eigen::Vector3f(-4.f, 2.f, 0.f),
Eigen::Vector3f(-5.f, 2.f, 0.f), Eigen::Vector3f(-6.f, 2.f, 0.f),

4
cartographer/mapping_3d/scan_matching/precomputation_grid.cc
View File

@ -48,7 +48,7 @@ Eigen::Array3i CellIndexAtHalfResolution(const Eigen::Array3i& cell_index) {
} // namespace
PrecomputationGrid ConvertToPrecomputationGrid(const HybridGrid& hybrid_grid) {
PrecomputationGrid result(hybrid_grid.resolution(), hybrid_grid.origin());
PrecomputationGrid result(hybrid_grid.resolution());
for (auto it = HybridGrid::Iterator(hybrid_grid); !it.Done(); it.Next()) {
const int cell_value = common::RoundToInt(
(mapping::ValueToProbability(it.GetValue()) -
@ -64,7 +64,7 @@ PrecomputationGrid ConvertToPrecomputationGrid(const HybridGrid& hybrid_grid) {
PrecomputationGrid PrecomputeGrid(const PrecomputationGrid& grid,
const bool half_resolution,
const Eigen::Array3i& shift) {
PrecomputationGrid result(grid.resolution(), grid.origin());
PrecomputationGrid result(grid.resolution());
for (auto it = PrecomputationGrid::Iterator(grid); !it.Done(); it.Next()) {
for (int i = 0; i != 8; ++i) {
// We use this value to update 8 values in the resulting grid, at

4
cartographer/mapping_3d/scan_matching/precomputation_grid.h
View File

@ -25,8 +25,8 @@ namespace scan_matching {
class PrecomputationGrid : public HybridGridBase<uint8> {
public:
PrecomputationGrid(const float resolution, const Eigen::Vector3f& origin)
: HybridGridBase<uint8>(resolution, origin) {}
explicit PrecomputationGrid(const float resolution)
: HybridGridBase<uint8>(resolution) {}
// Maps values from [0, 255] to [kMinProbability, kMaxProbability].
static float ToProbability(float value) {

2
cartographer/mapping_3d/scan_matching/precomputation_grid_test.cc
View File

@ -29,7 +29,7 @@ namespace scan_matching {
namespace {
TEST(PrecomputedGridGeneratorTest, TestAgainstNaiveAlgorithm) {
HybridGrid hybrid_grid(2.f, Eigen::Vector3f(-7.f, -12.f, 0.f));
HybridGrid hybrid_grid(2.f);
std::mt19937 rng(23847);
std::uniform_int_distribution<int> coordinate_distribution(-50, 49);

34
cartographer/mapping_3d/scan_matching/real_time_correlative_scan_matcher_test.cc
View File

@ -36,9 +36,8 @@ namespace {
class RealTimeCorrelativeScanMatcherTest : public ::testing::Test {
protected:
RealTimeCorrelativeScanMatcherTest()
: hybrid_grid_(0.1f /* resolution */,
Eigen::Vector3f(0.5f, 0.5f, 0.5f) /* origin */),
expected_pose_(Eigen::Vector3d(-0.5, 0.5, 0.5),
: hybrid_grid_(0.1f),
expected_pose_(Eigen::Vector3d(-1., 0., 0.),
Eigen::Quaterniond::Identity()) {
for (const auto& point :
{Eigen::Vector3f(-3.f, 2.f, 0.f), Eigen::Vector3f(-4.f, 2.f, 0.f),
@ -82,43 +81,40 @@ class RealTimeCorrelativeScanMatcherTest : public ::testing::Test {
TEST_F(RealTimeCorrelativeScanMatcherTest, PerfectEstimate) {
TestFromInitialPose(
transform::Rigid3d::Translation(Eigen::Vector3d(-0.5, 0.5, 0.5)));
transform::Rigid3d::Translation(Eigen::Vector3d(-1., 0., 0.)));
}
TEST_F(RealTimeCorrelativeScanMatcherTest, AlongX) {
TestFromInitialPose(
transform::Rigid3d::Translation(Eigen::Vector3d(-0.3, 0.5, 0.5)));
transform::Rigid3d::Translation(Eigen::Vector3d(-0.8, 0., 0.)));
}
TEST_F(RealTimeCorrelativeScanMatcherTest, AlongZ) {
TestFromInitialPose(
transform::Rigid3d::Translation(Eigen::Vector3d(-0.5, 0.5, 0.3)));
transform::Rigid3d::Translation(Eigen::Vector3d(-1., 0., -0.2)));
}
TEST_F(RealTimeCorrelativeScanMatcherTest, AlongXYZ) {
TestFromInitialPose(
transform::Rigid3d::Translation(Eigen::Vector3d(-0.4, 0.3, 0.7)));
transform::Rigid3d::Translation(Eigen::Vector3d(-0.9, -0.2, 0.2)));
}
TEST_F(RealTimeCorrelativeScanMatcherTest, RotationAroundX) {
TestFromInitialPose(
transform::Rigid3d::Translation(Eigen::Vector3d(-0.5, 0.5, 0.5)) *
transform::Rigid3d::Rotation(
Eigen::AngleAxisd(0.8 / 180. * M_PI, Eigen::Vector3d(1., 0., 0.))));
TestFromInitialPose(transform::Rigid3d(
Eigen::Vector3d(-1., 0., 0.),
Eigen::AngleAxisd(0.8 / 180. * M_PI, Eigen::Vector3d(1., 0., 0.))));
}
TEST_F(RealTimeCorrelativeScanMatcherTest, RotationAroundY) {
TestFromInitialPose(
transform::Rigid3d::Translation(Eigen::Vector3d(-0.5, 0.5, 0.5)) *
transform::Rigid3d::Rotation(
Eigen::AngleAxisd(0.8 / 180. * M_PI, Eigen::Vector3d(0., 1., 0.))));
TestFromInitialPose(transform::Rigid3d(
Eigen::Vector3d(-1., 0., 0.),
Eigen::AngleAxisd(0.8 / 180. * M_PI, Eigen::Vector3d(0., 1., 0.))));
}
TEST_F(RealTimeCorrelativeScanMatcherTest, RotationAroundYZ) {
TestFromInitialPose(
transform::Rigid3d::Translation(Eigen::Vector3d(-0.5, 0.5, 0.5)) *
transform::Rigid3d::Rotation(
Eigen::AngleAxisd(0.8 / 180. * M_PI, Eigen::Vector3d(0., 1., 1.))));
TestFromInitialPose(transform::Rigid3d(
Eigen::Vector3d(-1., 0., 0.),
Eigen::AngleAxisd(0.8 / 180. * M_PI, Eigen::Vector3d(0., 1., 1.))));
}
} // namespace

34
cartographer/mapping_3d/sparse_pose_graph/constraint_builder.cc
View File

@ -61,9 +61,8 @@ void ConstraintBuilder::MaybeAddConstraint(
const mapping::NodeId& node_id,
const sensor::CompressedPointCloud* const compressed_point_cloud,
const std::vector<mapping::TrajectoryNode>& submap_nodes,
const transform::Rigid3d& initial_relative_pose) {
if (initial_relative_pose.translation().norm() >
options_.max_constraint_distance()) {
const transform::Rigid3d& initial_pose) {
if (initial_pose.translation().norm() > options_.max_constraint_distance()) {
return;
}
if (sampler_.Pulse()) {
@ -75,10 +74,10 @@ void ConstraintBuilder::MaybeAddConstraint(
ScheduleSubmapScanMatcherConstructionAndQueueWorkItem(
submap_id, submap_nodes, &submap->high_resolution_hybrid_grid,
[=]() EXCLUDES(mutex_) {
ComputeConstraint(
submap_id, submap, node_id, false, /* match_full_submap */
nullptr, /* trajectory_connectivity */
compressed_point_cloud, initial_relative_pose, constraint);
ComputeConstraint(submap_id, submap, node_id,
false, /* match_full_submap */
nullptr, /* trajectory_connectivity */
compressed_point_cloud, initial_pose, constraint);
FinishComputation(current_computation);
});
}
@ -171,23 +170,19 @@ void ConstraintBuilder::ComputeConstraint(
const mapping::NodeId& node_id, bool match_full_submap,
mapping::TrajectoryConnectivity* trajectory_connectivity,
const sensor::CompressedPointCloud* const compressed_point_cloud,
const transform::Rigid3d& initial_relative_pose,
const transform::Rigid3d& initial_pose,
std::unique_ptr<OptimizationProblem::Constraint>* constraint) {
const transform::Rigid3d initial_pose =
submap->local_pose() * initial_relative_pose;
const SubmapScanMatcher* const submap_scan_matcher =
GetSubmapScanMatcher(submap_id);
const sensor::PointCloud point_cloud = compressed_point_cloud->Decompress();
const sensor::PointCloud filtered_point_cloud =
adaptive_voxel_filter_.Filter(point_cloud);
// The 'constraint_transform' (i <- j) is computed from:
// - a 'filtered_point_cloud' in j,
// - the initial guess 'initial_pose' for (map <- j),
// - the result 'pose_estimate' of Match() (map <- j).
// - the submap->pose() (map <- i)
// The 'constraint_transform' (submap 'i' <- scan 'j') is computed from the
// initial guess 'initial_pose' for (submap 'i' <- scan 'j') and a
// 'filtered_point_cloud' in 'j'.
float score = 0.;
transform::Rigid3d pose_estimate = transform::Rigid3d::Identity();
transform::Rigid3d pose_estimate;
if (match_full_submap) {
if (submap_scan_matcher->fast_correlative_scan_matcher->MatchFullSubmap(
@ -220,13 +215,12 @@ void ConstraintBuilder::ComputeConstraint(
// effect that, in the absence of better information, we prefer the original
// CSM estimate.
ceres::Solver::Summary unused_summary;
transform::Rigid3d constraint_transform;
ceres_scan_matcher_.Match(
pose_estimate, pose_estimate,
{{&filtered_point_cloud, submap_scan_matcher->hybrid_grid}},
&pose_estimate, &unused_summary);
&constraint_transform, &unused_summary);
const transform::Rigid3d constraint_transform =
submap->local_pose().inverse() * pose_estimate;
constraint->reset(new OptimizationProblem::Constraint{
submap_id,
node_id,
@ -243,7 +237,7 @@ void ConstraintBuilder::ComputeConstraint(
info << " matches";
} else {
const transform::Rigid3d difference =
initial_pose.inverse() * pose_estimate;
initial_pose.inverse() * constraint_transform;
info << " differs by translation " << std::setprecision(2)
<< difference.translation().norm() << " rotation "
<< std::setprecision(3) << transform::GetAngle(difference);

8
cartographer/mapping_3d/sparse_pose_graph/constraint_builder.h
View File

@ -68,8 +68,8 @@ class ConstraintBuilder {
ConstraintBuilder& operator=(const ConstraintBuilder&) = delete;
// Schedules exploring a new constraint between 'submap' identified by
// 'submap_id', and the 'point_cloud' for 'node_id'.
// The 'initial_relative_pose' is relative to the 'submap'.
// 'submap_id', and the 'point_cloud' for 'node_id'. The 'intial_pose' is
// relative to the 'submap'.
//
// The pointees of 'submap' and 'compressed_point_cloud' must stay valid until
// all computations are finished.
@ -78,7 +78,7 @@ class ConstraintBuilder {
const mapping::NodeId& node_id,
const sensor::CompressedPointCloud* compressed_point_cloud,
const std::vector<mapping::TrajectoryNode>& submap_nodes,
const transform::Rigid3d& initial_relative_pose);
const transform::Rigid3d& initial_pose);
// Schedules exploring a new constraint between 'submap' identified by
// 'submap_id' and the 'compressed_point_cloud' for 'node_id'.
@ -141,7 +141,7 @@ class ConstraintBuilder {
const mapping::NodeId& node_id, bool match_full_submap,
mapping::TrajectoryConnectivity* trajectory_connectivity,
const sensor::CompressedPointCloud* compressed_point_cloud,
const transform::Rigid3d& initial_relative_pose,
const transform::Rigid3d& initial_pose,
std::unique_ptr<Constraint>* constraint) EXCLUDES(mutex_);
// Decrements the 'pending_computations_' count. If all computations are done,

26
cartographer/mapping_3d/submaps.cc
View File

@ -219,8 +219,8 @@ proto::SubmapsOptions CreateSubmapsOptions(
Submap::Submap(const float high_resolution, const float low_resolution,
const Eigen::Vector3f& origin)
: mapping::Submap(origin),
high_resolution_hybrid_grid(high_resolution, origin),
low_resolution_hybrid_grid(low_resolution, origin) {}
high_resolution_hybrid_grid(high_resolution),
low_resolution_hybrid_grid(low_resolution) {}
Submaps::Submaps(const proto::SubmapsOptions& options)
: options_(options),
@ -250,9 +250,7 @@ void Submaps::SubmapToProto(
Eigen::Array2i min_index(INT_MAX, INT_MAX);
Eigen::Array2i max_index(INT_MIN, INT_MIN);
const std::vector<Eigen::Array4i> voxel_indices_and_probabilities =
ExtractVoxelData(hybrid_grid,
(global_submap_pose * Get(index)->local_pose().inverse())
.cast<float>(),
ExtractVoxelData(hybrid_grid, global_submap_pose.cast<float>(),
&min_index, &max_index);
const int width = max_index.y() - min_index.y() + 1;
@ -276,11 +274,13 @@ void Submaps::SubmapToProto(
void Submaps::InsertRangeData(const sensor::RangeData& range_data) {
for (const int index : insertion_indices()) {
Submap* submap = submaps_[index].get();
const sensor::RangeData transformed_range_data = sensor::TransformRangeData(
range_data, submap->local_pose().inverse().cast<float>());
range_data_inserter_.Insert(
FilterRangeDataByMaxRange(range_data,
FilterRangeDataByMaxRange(transformed_range_data,
options_.high_resolution_max_range()),
&submap->high_resolution_hybrid_grid);
range_data_inserter_.Insert(range_data,
range_data_inserter_.Insert(transformed_range_data,
&submap->low_resolution_hybrid_grid);
++submap->num_range_data;
}
@ -290,14 +290,6 @@ void Submaps::InsertRangeData(const sensor::RangeData& range_data) {
}
}
const HybridGrid& Submaps::high_resolution_matching_grid() const {
return submaps_[matching_index()]->high_resolution_hybrid_grid;
}
const HybridGrid& Submaps::low_resolution_matching_grid() const {
return submaps_[matching_index()]->low_resolution_hybrid_grid;
}
void Submaps::AddSubmap(const Eigen::Vector3f& origin) {
if (size() > 1) {
Submap* submap = submaps_[size() - 2].get();
@ -350,9 +342,9 @@ std::vector<Eigen::Array4i> Submaps::ExtractVoxelData(
continue;
}
const Eigen::Vector3f cell_center_local =
const Eigen::Vector3f cell_center_submap =
hybrid_grid.GetCenterOfCell(it.GetCellIndex());
const Eigen::Vector3f cell_center_global = transform * cell_center_local;
const Eigen::Vector3f cell_center_global = transform * cell_center_submap;
const Eigen::Array4i voxel_index_and_probability(
common::RoundToInt(cell_center_global.x() * resolution_inverse),
common::RoundToInt(cell_center_global.y() * resolution_inverse),

6
cartographer/mapping_3d/submaps.h
View File

@ -72,12 +72,6 @@ class Submaps : public mapping::Submaps {
// Inserts 'range_data' into the Submap collection.
void InsertRangeData(const sensor::RangeData& range_data);
// Returns the 'high_resolution' HybridGrid to be used for matching.
const HybridGrid& high_resolution_matching_grid() const;
// Returns the 'low_resolution' HybridGrid to be used for matching.
const HybridGrid& low_resolution_matching_grid() const;
private:
struct PixelData {
int min_z = INT_MAX;

2
cartographer/sensor/compressed_point_cloud.cc
View File

@ -104,7 +104,7 @@ CompressedPointCloud::CompressedPointCloud(const PointCloud& point_cloud)
int index;
};
using Blocks = mapping_3d::HybridGridBase<std::vector<RasterPoint>>;
Blocks blocks(kPrecision, Eigen::Vector3f::Zero());
Blocks blocks(kPrecision);
int num_blocks = 0;
CHECK_LE(point_cloud.size(), std::numeric_limits<int>::max());
for (int point_index = 0; point_index < static_cast<int>(point_cloud.size());

3
cartographer/sensor/voxel_filter.cc
View File

@ -83,8 +83,7 @@ PointCloud VoxelFiltered(const PointCloud& point_cloud, const float size) {
return voxel_filter.point_cloud();
}
VoxelFilter::VoxelFilter(const float size)
: voxels_(size, Eigen::Vector3f::Zero()) {}
VoxelFilter::VoxelFilter(const float size) : voxels_(size) {}
void VoxelFilter::InsertPointCloud(const PointCloud& point_cloud) {
for (const Eigen::Vector3f& point : point_cloud) {