319 lines
12 KiB
C++
319 lines
12 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/internal/2d/pose_graph_2d.h"
|
||
|
|
|
||
|
|
#include <cmath>
|
||
|
|
#include <memory>
|
||
|
|
#include <random>
|
||
|
|
|
||
|
|
#include "absl/memory/memory.h"
|
||
|
|
#include "cartographer/common/internal/testing/lua_parameter_dictionary_test_helpers.h"
|
||
|
|
#include "cartographer/common/thread_pool.h"
|
||
|
|
#include "cartographer/common/time.h"
|
||
|
|
#include "cartographer/mapping/2d/probability_grid_range_data_inserter_2d.h"
|
||
|
|
#include "cartographer/mapping/2d/submap_2d.h"
|
||
|
|
#include "cartographer/transform/rigid_transform.h"
|
||
|
|
#include "cartographer/transform/rigid_transform_test_helpers.h"
|
||
|
|
#include "cartographer/transform/transform.h"
|
||
|
|
#include "gmock/gmock.h"
|
||
|
|
|
||
|
|
namespace cartographer {
|
||
|
|
namespace mapping {
|
||
|
|
namespace {
|
||
|
|
|
||
|
|
class PoseGraph2DTest : public ::testing::Test {
|
||
|
|
protected:
|
||
|
|
PoseGraph2DTest() : thread_pool_(1) {
|
||
|
|
// Builds a wavy, irregularly circular point cloud that is unique
|
||
|
|
// rotationally. This gives us good rotational texture and avoids any
|
||
|
|
// possibility of the CeresScanMatcher2D preferring free space (>
|
||
|
|
// kMinProbability) to unknown space (== kMinProbability).
|
||
|
|
for (float t = 0.f; t < 2.f * M_PI; t += 0.005f) {
|
||
|
|
const float r = (std::sin(20.f * t) + 2.f) * std::sin(t + 2.f);
|
||
|
|
point_cloud_.push_back(
|
||
|
|
{Eigen::Vector3f{r * std::sin(t), r * std::cos(t), 0.f}});
|
||
|
|
}
|
||
|
|
|
||
|
|
{
|
||
|
|
auto parameter_dictionary = common::MakeDictionary(R"text(
|
||
|
|
return {
|
||
|
|
num_range_data = 1,
|
||
|
|
grid_options_2d = {
|
||
|
|
grid_type = "PROBABILITY_GRID",
|
||
|
|
resolution = 0.05,
|
||
|
|
},
|
||
|
|
range_data_inserter = {
|
||
|
|
range_data_inserter_type = "PROBABILITY_GRID_INSERTER_2D",
|
||
|
|
probability_grid_range_data_inserter = {
|
||
|
|
insert_free_space = true,
|
||
|
|
hit_probability = 0.53,
|
||
|
|
miss_probability = 0.495,
|
||
|
|
},
|
||
|
|
tsdf_range_data_inserter = {
|
||
|
|
truncation_distance = 0.3,
|
||
|
|
maximum_weight = 10.,
|
||
|
|
update_free_space = false,
|
||
|
|
normal_estimation_options = {
|
||
|
|
num_normal_samples = 4,
|
||
|
|
sample_radius = 0.5,
|
||
|
|
},
|
||
|
|
project_sdf_distance_to_scan_normal = false,
|
||
|
|
update_weight_range_exponent = 0,
|
||
|
|
update_weight_angle_scan_normal_to_ray_kernel_bandwidth = 0,
|
||
|
|
update_weight_distance_cell_to_hit_kernel_bandwidth = 0,
|
||
|
|
},
|
||
|
|
},
|
||
|
|
})text");
|
||
|
|
active_submaps_ = absl::make_unique<ActiveSubmaps2D>(
|
||
|
|
mapping::CreateSubmapsOptions2D(parameter_dictionary.get()));
|
||
|
|
}
|
||
|
|
|
||
|
|
{
|
||
|
|
auto parameter_dictionary = common::MakeDictionary(R"text(
|
||
|
|
return {
|
||
|
|
optimize_every_n_nodes = 1000,
|
||
|
|
constraint_builder = {
|
||
|
|
sampling_ratio = 1.,
|
||
|
|
max_constraint_distance = 6.,
|
||
|
|
min_score = 0.5,
|
||
|
|
global_localization_min_score = 0.6,
|
||
|
|
loop_closure_translation_weight = 1.,
|
||
|
|
loop_closure_rotation_weight = 1.,
|
||
|
|
log_matches = true,
|
||
|
|
fast_correlative_scan_matcher = {
|
||
|
|
linear_search_window = 3.,
|
||
|
|
angular_search_window = 0.1,
|
||
|
|
branch_and_bound_depth = 3,
|
||
|
|
},
|
||
|
|
ceres_scan_matcher = {
|
||
|
|
occupied_space_weight = 20.,
|
||
|
|
translation_weight = 10.,
|
||
|
|
rotation_weight = 1.,
|
||
|
|
ceres_solver_options = {
|
||
|
|
use_nonmonotonic_steps = true,
|
||
|
|
max_num_iterations = 50,
|
||
|
|
num_threads = 1,
|
||
|
|
},
|
||
|
|
},
|
||
|
|
fast_correlative_scan_matcher_3d = {
|
||
|
|
branch_and_bound_depth = 3,
|
||
|
|
full_resolution_depth = 3,
|
||
|
|
min_rotational_score = 0.1,
|
||
|
|
min_low_resolution_score = 0.5,
|
||
|
|
linear_xy_search_window = 4.,
|
||
|
|
linear_z_search_window = 4.,
|
||
|
|
angular_search_window = 0.1,
|
||
|
|
},
|
||
|
|
ceres_scan_matcher_3d = {
|
||
|
|
occupied_space_weight_0 = 20.,
|
||
|
|
translation_weight = 10.,
|
||
|
|
rotation_weight = 1.,
|
||
|
|
only_optimize_yaw = true,
|
||
|
|
ceres_solver_options = {
|
||
|
|
use_nonmonotonic_steps = true,
|
||
|
|
max_num_iterations = 50,
|
||
|
|
num_threads = 1,
|
||
|
|
},
|
||
|
|
},
|
||
|
|
},
|
||
|
|
matcher_translation_weight = 1.,
|
||
|
|
matcher_rotation_weight = 1.,
|
||
|
|
optimization_problem = {
|
||
|
|
acceleration_weight = 1.,
|
||
|
|
rotation_weight = 1e2,
|
||
|
|
huber_scale = 1.,
|
||
|
|
local_slam_pose_translation_weight = 0.,
|
||
|
|
local_slam_pose_rotation_weight = 0.,
|
||
|
|
odometry_translation_weight = 0.,
|
||
|
|
odometry_rotation_weight = 0.,
|
||
|
|
fixed_frame_pose_translation_weight = 1e1,
|
||
|
|
fixed_frame_pose_rotation_weight = 1e2,
|
||
|
|
fixed_frame_pose_use_tolerant_loss = false,
|
||
|
|
fixed_frame_pose_tolerant_loss_param_a = 1,
|
||
|
|
fixed_frame_pose_tolerant_loss_param_b = 1,
|
||
|
|
log_solver_summary = true,
|
||
|
|
use_online_imu_extrinsics_in_3d = true,
|
||
|
|
fix_z_in_3d = false,
|
||
|
|
ceres_solver_options = {
|
||
|
|
use_nonmonotonic_steps = false,
|
||
|
|
max_num_iterations = 200,
|
||
|
|
num_threads = 1,
|
||
|
|
},
|
||
|
|
},
|
||
|
|
max_num_final_iterations = 200,
|
||
|
|
global_sampling_ratio = 0.01,
|
||
|
|
log_residual_histograms = true,
|
||
|
|
global_constraint_search_after_n_seconds = 10.0,
|
||
|
|
})text");
|
||
|
|
auto options = CreatePoseGraphOptions(parameter_dictionary.get());
|
||
|
|
pose_graph_ = absl::make_unique<PoseGraph2D>(
|
||
|
|
options,
|
||
|
|
absl::make_unique<optimization::OptimizationProblem2D>(
|
||
|
|
options.optimization_problem_options()),
|
||
|
|
&thread_pool_);
|
||
|
|
}
|
||
|
|
|
||
|
|
current_pose_ = transform::Rigid2d::Identity();
|
||
|
|
}
|
||
|
|
|
||
|
|
void MoveRelativeWithNoise(const transform::Rigid2d& movement,
|
||
|
|
const transform::Rigid2d& noise) {
|
||
|
|
current_pose_ = current_pose_ * movement;
|
||
|
|
const sensor::PointCloud new_point_cloud = sensor::TransformPointCloud(
|
||
|
|
point_cloud_,
|
||
|
|
transform::Embed3D(current_pose_.inverse().cast<float>()));
|
||
|
|
const sensor::RangeData range_data{
|
||
|
|
Eigen::Vector3f::Zero(), new_point_cloud, {}};
|
||
|
|
const transform::Rigid2d pose_estimate = noise * current_pose_;
|
||
|
|
constexpr int kTrajectoryId = 0;
|
||
|
|
active_submaps_->InsertRangeData(TransformRangeData(
|
||
|
|
range_data, transform::Embed3D(pose_estimate.cast<float>())));
|
||
|
|
std::vector<std::shared_ptr<const Submap2D>> insertion_submaps;
|
||
|
|
for (const auto& submap : active_submaps_->submaps()) {
|
||
|
|
insertion_submaps.push_back(submap);
|
||
|
|
}
|
||
|
|
pose_graph_->AddNode(
|
||
|
|
std::make_shared<const TrajectoryNode::Data>(
|
||
|
|
TrajectoryNode::Data{common::FromUniversal(0),
|
||
|
|
Eigen::Quaterniond::Identity(),
|
||
|
|
range_data.returns,
|
||
|
|
{},
|
||
|
|
{},
|
||
|
|
{},
|
||
|
|
transform::Embed3D(pose_estimate)}),
|
||
|
|
kTrajectoryId, insertion_submaps);
|
||
|
|
}
|
||
|
|
|
||
|
|
void MoveRelative(const transform::Rigid2d& movement) {
|
||
|
|
MoveRelativeWithNoise(movement, transform::Rigid2d::Identity());
|
||
|
|
}
|
||
|
|
|
||
|
|
template <typename Range>
|
||
|
|
std::vector<int> ToVectorInt(const Range& range) {
|
||
|
|
return std::vector<int>(range.begin(), range.end());
|
||
|
|
}
|
||
|
|
|
||
|
|
sensor::PointCloud point_cloud_;
|
||
|
|
std::unique_ptr<ActiveSubmaps2D> active_submaps_;
|
||
|
|
common::ThreadPool thread_pool_;
|
||
|
|
std::unique_ptr<PoseGraph2D> pose_graph_;
|
||
|
|
transform::Rigid2d current_pose_;
|
||
|
|
};
|
||
|
|
|
||
|
|
TEST_F(PoseGraph2DTest, EmptyMap) {
|
||
|
|
pose_graph_->RunFinalOptimization();
|
||
|
|
const auto nodes = pose_graph_->GetTrajectoryNodes();
|
||
|
|
EXPECT_TRUE(nodes.empty());
|
||
|
|
}
|
||
|
|
|
||
|
|
TEST_F(PoseGraph2DTest, NoMovement) {
|
||
|
|
MoveRelative(transform::Rigid2d::Identity());
|
||
|
|
MoveRelative(transform::Rigid2d::Identity());
|
||
|
|
MoveRelative(transform::Rigid2d::Identity());
|
||
|
|
pose_graph_->RunFinalOptimization();
|
||
|
|
const auto nodes = pose_graph_->GetTrajectoryNodes();
|
||
|
|
ASSERT_THAT(ToVectorInt(nodes.trajectory_ids()),
|
||
|
|
::testing::ContainerEq(std::vector<int>{0}));
|
||
|
|
EXPECT_THAT(nodes.SizeOfTrajectoryOrZero(0), ::testing::Eq(3u));
|
||
|
|
EXPECT_THAT(nodes.at(NodeId{0, 0}).global_pose,
|
||
|
|
transform::IsNearly(transform::Rigid3d::Identity(), 1e-2));
|
||
|
|
EXPECT_THAT(nodes.at(NodeId{0, 1}).global_pose,
|
||
|
|
transform::IsNearly(transform::Rigid3d::Identity(), 1e-2));
|
||
|
|
EXPECT_THAT(nodes.at(NodeId{0, 2}).global_pose,
|
||
|
|
transform::IsNearly(transform::Rigid3d::Identity(), 1e-2));
|
||
|
|
}
|
||
|
|
|
||
|
|
TEST_F(PoseGraph2DTest, NoOverlappingNodes) {
|
||
|
|
std::mt19937 rng(0);
|
||
|
|
std::uniform_real_distribution<double> distribution(-1., 1.);
|
||
|
|
std::vector<transform::Rigid2d> poses;
|
||
|
|
for (int i = 0; i != 4; ++i) {
|
||
|
|
MoveRelative(transform::Rigid2d({0.25 * distribution(rng), 5.}, 0.));
|
||
|
|
poses.emplace_back(current_pose_);
|
||
|
|
}
|
||
|
|
pose_graph_->RunFinalOptimization();
|
||
|
|
const auto nodes = pose_graph_->GetTrajectoryNodes();
|
||
|
|
ASSERT_THAT(ToVectorInt(nodes.trajectory_ids()),
|
||
|
|
::testing::ContainerEq(std::vector<int>{0}));
|
||
|
|
for (int i = 0; i != 4; ++i) {
|
||
|
|
EXPECT_THAT(
|
||
|
|
poses[i],
|
||
|
|
IsNearly(transform::Project2D(nodes.at(NodeId{0, i}).global_pose),
|
||
|
|
1e-2))
|
||
|
|
<< i;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
TEST_F(PoseGraph2DTest, ConsecutivelyOverlappingNodes) {
|
||
|
|
std::mt19937 rng(0);
|
||
|
|
std::uniform_real_distribution<double> distribution(-1., 1.);
|
||
|
|
std::vector<transform::Rigid2d> poses;
|
||
|
|
for (int i = 0; i != 5; ++i) {
|
||
|
|
MoveRelative(transform::Rigid2d({0.25 * distribution(rng), 2.}, 0.));
|
||
|
|
poses.emplace_back(current_pose_);
|
||
|
|
}
|
||
|
|
pose_graph_->RunFinalOptimization();
|
||
|
|
const auto nodes = pose_graph_->GetTrajectoryNodes();
|
||
|
|
ASSERT_THAT(ToVectorInt(nodes.trajectory_ids()),
|
||
|
|
::testing::ContainerEq(std::vector<int>{0}));
|
||
|
|
for (int i = 0; i != 5; ++i) {
|
||
|
|
EXPECT_THAT(
|
||
|
|
poses[i],
|
||
|
|
IsNearly(transform::Project2D(nodes.at(NodeId{0, i}).global_pose),
|
||
|
|
1e-2))
|
||
|
|
<< i;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
TEST_F(PoseGraph2DTest, OverlappingNodes) {
|
||
|
|
std::mt19937 rng(0);
|
||
|
|
std::uniform_real_distribution<double> distribution(-1., 1.);
|
||
|
|
std::vector<transform::Rigid2d> ground_truth;
|
||
|
|
std::vector<transform::Rigid2d> poses;
|
||
|
|
for (int i = 0; i != 5; ++i) {
|
||
|
|
const double noise_x = 0.1 * distribution(rng);
|
||
|
|
const double noise_y = 0.1 * distribution(rng);
|
||
|
|
const double noise_orientation = 0.1 * distribution(rng);
|
||
|
|
transform::Rigid2d noise({noise_x, noise_y}, noise_orientation);
|
||
|
|
MoveRelativeWithNoise(
|
||
|
|
transform::Rigid2d({0.15 * distribution(rng), 0.4}, 0.), noise);
|
||
|
|
ground_truth.emplace_back(current_pose_);
|
||
|
|
poses.emplace_back(noise * current_pose_);
|
||
|
|
}
|
||
|
|
pose_graph_->RunFinalOptimization();
|
||
|
|
const auto nodes = pose_graph_->GetTrajectoryNodes();
|
||
|
|
ASSERT_THAT(ToVectorInt(nodes.trajectory_ids()),
|
||
|
|
::testing::ContainerEq(std::vector<int>{0}));
|
||
|
|
transform::Rigid2d true_movement =
|
||
|
|
ground_truth.front().inverse() * ground_truth.back();
|
||
|
|
transform::Rigid2d movement_before = poses.front().inverse() * poses.back();
|
||
|
|
transform::Rigid2d error_before = movement_before.inverse() * true_movement;
|
||
|
|
transform::Rigid3d optimized_movement =
|
||
|
|
nodes.BeginOfTrajectory(0)->data.global_pose.inverse() *
|
||
|
|
std::prev(nodes.EndOfTrajectory(0))->data.global_pose;
|
||
|
|
transform::Rigid2d optimized_error =
|
||
|
|
transform::Project2D(optimized_movement).inverse() * true_movement;
|
||
|
|
EXPECT_THAT(std::abs(optimized_error.normalized_angle()),
|
||
|
|
::testing::Lt(std::abs(error_before.normalized_angle())));
|
||
|
|
EXPECT_THAT(optimized_error.translation().norm(),
|
||
|
|
::testing::Lt(error_before.translation().norm()));
|
||
|
|
}
|
||
|
|
|
||
|
|
} // namespace
|
||
|
|
} // namespace mapping
|
||
|
|
} // namespace cartographer
|