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

Fix crashing bug in localization. (#575) 

Now that we deserialize trajectory nodes, we need to make sure
that the global matcher sampler is added for the map trajectory.
master
Wolfgang Hess 2017-10-06 17:01:04 +02:00 committed by GitHub
parent 2f332eca28
commit 2434e7e40e
8 changed files with 57 additions and 48 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
cartographer/io/frame_id_filtering_points_processor.cc
View File

@ -26,8 +26,7 @@ namespace io {
std::unique_ptr<FrameIdFilteringPointsProcessor>
FrameIdFilteringPointsProcessor::FromDictionary(
common::LuaParameterDictionary* dictionary,
PointsProcessor* next) {
common::LuaParameterDictionary* dictionary, PointsProcessor* next) {
std::vector<string> keep_frames, drop_frames;
if (dictionary->HasKey("keep_frames")) {
keep_frames =
@ -44,8 +43,7 @@ FrameIdFilteringPointsProcessor::FromDictionary(
FrameIdFilteringPointsProcessor::FrameIdFilteringPointsProcessor(
const std::unordered_set<string>& keep_frame_ids,
const std::unordered_set<string>& drop_frame_ids,
PointsProcessor* next)
const std::unordered_set<string>& drop_frame_ids, PointsProcessor* next)
: keep_frame_ids_(keep_frame_ids),
drop_frame_ids_(drop_frame_ids),
next_(next) {

6
cartographer/io/frame_id_filtering_points_processor.h
View File

@ -33,11 +33,9 @@ class FrameIdFilteringPointsProcessor : public PointsProcessor {
constexpr static const char* kConfigurationFileActionName = "frame_id_filter";
FrameIdFilteringPointsProcessor(
const std::unordered_set<string>& keep_frame_ids,
const std::unordered_set<string>& drop_frame_ids,
PointsProcessor* next);
const std::unordered_set<string>& drop_frame_ids, PointsProcessor* next);
static std::unique_ptr<FrameIdFilteringPointsProcessor> FromDictionary(
common::LuaParameterDictionary* dictionary,
PointsProcessor* next);
common::LuaParameterDictionary* dictionary, PointsProcessor* next);
~FrameIdFilteringPointsProcessor() override {}
FrameIdFilteringPointsProcessor(const FrameIdFilteringPointsProcessor&) =

2
cartographer/io/points_processor_pipeline_builder.cc
View File

@ -19,8 +19,8 @@
#include "cartographer/common/make_unique.h"
#include "cartographer/io/coloring_points_processor.h"
#include "cartographer/io/counting_points_processor.h"
#include "cartographer/io/frame_id_filtering_points_processor.h"
#include "cartographer/io/fixed_ratio_sampling_points_processor.h"
#include "cartographer/io/frame_id_filtering_points_processor.h"
#include "cartographer/io/hybrid_grid_points_processor.h"
#include "cartographer/io/intensity_to_color_points_processor.h"
#include "cartographer/io/min_max_range_filtering_points_processor.h"

2
cartographer/mapping/id.h
View File

@ -33,7 +33,7 @@ struct NodeId {
bool operator==(const NodeId& other) const {
return std::forward_as_tuple(trajectory_id, node_index) ==
std::forward_as_tuple(other.trajectory_id, other.node_index);
std::forward_as_tuple(other.trajectory_id, other.node_index);
}
bool operator!=(const NodeId& other) const { return !operator==(other); }

41
cartographer/mapping_2d/sparse_pose_graph.cc
View File

@ -102,10 +102,10 @@ void SparsePoseGraph::AddScan(
GetLocalToGlobalTransform(trajectory_id) * constant_data->initial_pose);
common::MutexLocker locker(&mutex_);
AddTrajectoryIfNeeded(trajectory_id);
trajectory_nodes_.Append(
trajectory_id, mapping::TrajectoryNode{constant_data, optimized_pose});
++num_trajectory_nodes_;
trajectory_connectivity_state_.Add(trajectory_id);
// Test if the 'insertion_submap.back()' is one we never saw before.
if (trajectory_id >= submap_data_.num_trajectories() ||
@ -121,13 +121,6 @@ void SparsePoseGraph::AddScan(
submap_data_.at(submap_id).submap = insertion_submaps.back();
}
// Make sure we have a sampler for this trajectory.
if (!global_localization_samplers_[trajectory_id]) {
global_localization_samplers_[trajectory_id] =
common::make_unique<common::FixedRatioSampler>(
options_.global_sampling_ratio());
}
// We have to check this here, because it might have changed by the time we
// execute the lambda.
const bool newly_finished_submap = insertion_submaps.front()->finished();
@ -145,6 +138,16 @@ void SparsePoseGraph::AddWorkItem(const std::function<void()>& work_item) {
}
}
void SparsePoseGraph::AddTrajectoryIfNeeded(const int trajectory_id) {
trajectory_connectivity_state_.Add(trajectory_id);
// Make sure we have a sampler for this trajectory.
if (!global_localization_samplers_[trajectory_id]) {
global_localization_samplers_[trajectory_id] =
common::make_unique<common::FixedRatioSampler>(
options_.global_sampling_ratio());
}
}
void SparsePoseGraph::AddImuData(const int trajectory_id,
const sensor::ImuData& imu_data) {
common::MutexLocker locker(&mutex_);
@ -232,15 +235,15 @@ void SparsePoseGraph::ComputeConstraintsForScan(
const mapping::NodeId node_id{
matching_id.trajectory_id,
static_cast<size_t>(matching_id.trajectory_id) <
optimization_problem_.node_data().size() &&
!optimization_problem_.node_data()[matching_id.trajectory_id]
.empty()
? static_cast<int>(optimization_problem_.node_data()
.at(matching_id.trajectory_id)
.rbegin()
->first +
1)
: 0};
optimization_problem_.node_data().size() &&
!optimization_problem_.node_data()[matching_id.trajectory_id]
.empty()
? static_cast<int>(optimization_problem_.node_data()
.at(matching_id.trajectory_id)
.rbegin()
->first +
1)
: 0};
const auto& constant_data = trajectory_nodes_.at(node_id).constant_data;
const transform::Rigid2d pose = transform::Project2D(
constant_data->initial_pose *
@ -419,7 +422,7 @@ void SparsePoseGraph::AddSubmapFromProto(const int trajectory_id,
const transform::Rigid2d initial_pose_2d = transform::Project2D(initial_pose);
common::MutexLocker locker(&mutex_);
trajectory_connectivity_state_.Add(trajectory_id);
AddTrajectoryIfNeeded(trajectory_id);
const mapping::SubmapId submap_id =
submap_data_.Append(trajectory_id, SubmapData());
submap_data_.at(submap_id).submap = submap_ptr;
@ -447,7 +450,7 @@ void SparsePoseGraph::AddNodeFromProto(const int trajectory_id,
mapping::FromProto(node.node_data()));
common::MutexLocker locker(&mutex_);
trajectory_connectivity_state_.Add(trajectory_id);
AddTrajectoryIfNeeded(trajectory_id);
const mapping::NodeId node_id = trajectory_nodes_.Append(
trajectory_id, mapping::TrajectoryNode{constant_data, pose});

4
cartographer/mapping_2d/sparse_pose_graph.h
View File

@ -39,6 +39,7 @@
#include "cartographer/mapping_2d/sparse_pose_graph/optimization_problem.h"
#include "cartographer/mapping_2d/submaps.h"
#include "cartographer/sensor/fixed_frame_pose_data.h"
#include "cartographer/sensor/odometry_data.h"
#include "cartographer/sensor/point_cloud.h"
#include "cartographer/transform/rigid_transform.h"
#include "cartographer/transform/transform.h"
@ -121,6 +122,9 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
// Handles a new work item.
void AddWorkItem(const std::function<void()>& work_item) REQUIRES(mutex_);
// Adds connectivity and sampler for a trajectory if it does not exist.
void AddTrajectoryIfNeeded(int trajectory_id) REQUIRES(mutex_);
// Grows the optimization problem to have an entry for every element of
// 'insertion_submaps'. Returns the IDs for the 'insertion_submaps'.
std::vector<mapping::SubmapId> GrowSubmapTransformsAsNeeded(

41
cartographer/mapping_3d/sparse_pose_graph.cc
View File

@ -100,10 +100,10 @@ void SparsePoseGraph::AddScan(
GetLocalToGlobalTransform(trajectory_id) * constant_data->initial_pose);
common::MutexLocker locker(&mutex_);
AddTrajectoryIfNeeded(trajectory_id);
trajectory_nodes_.Append(
trajectory_id, mapping::TrajectoryNode{constant_data, optimized_pose});
++num_trajectory_nodes_;
trajectory_connectivity_state_.Add(trajectory_id);
// Test if the 'insertion_submap.back()' is one we never saw before.
if (trajectory_id >= submap_data_.num_trajectories() ||
@ -119,13 +119,6 @@ void SparsePoseGraph::AddScan(
submap_data_.at(submap_id).submap = insertion_submaps.back();
}
// Make sure we have a sampler for this trajectory.
if (!global_localization_samplers_[trajectory_id]) {
global_localization_samplers_[trajectory_id] =
common::make_unique<common::FixedRatioSampler>(
options_.global_sampling_ratio());
}
// We have to check this here, because it might have changed by the time we
// execute the lambda.
const bool newly_finished_submap = insertion_submaps.front()->finished();
@ -143,6 +136,16 @@ void SparsePoseGraph::AddWorkItem(const std::function<void()>& work_item) {
}
}
void SparsePoseGraph::AddTrajectoryIfNeeded(const int trajectory_id) {
trajectory_connectivity_state_.Add(trajectory_id);
// Make sure we have a sampler for this trajectory.
if (!global_localization_samplers_[trajectory_id]) {
global_localization_samplers_[trajectory_id] =
common::make_unique<common::FixedRatioSampler>(
options_.global_sampling_ratio());
}
}
void SparsePoseGraph::AddImuData(const int trajectory_id,
const sensor::ImuData& imu_data) {
common::MutexLocker locker(&mutex_);
@ -258,15 +261,15 @@ void SparsePoseGraph::ComputeConstraintsForScan(
const mapping::NodeId node_id{
matching_id.trajectory_id,
static_cast<size_t>(matching_id.trajectory_id) <
optimization_problem_.node_data().size() &&
!optimization_problem_.node_data()[matching_id.trajectory_id]
.empty()
? static_cast<int>(optimization_problem_.node_data()
.at(matching_id.trajectory_id)
.rbegin()
->first +
1)
: 0};
optimization_problem_.node_data().size() &&
!optimization_problem_.node_data()[matching_id.trajectory_id]
.empty()
? static_cast<int>(optimization_problem_.node_data()
.at(matching_id.trajectory_id)
.rbegin()
->first +
1)
: 0};
const auto& constant_data = trajectory_nodes_.at(node_id).constant_data;
const transform::Rigid3d& pose = constant_data->initial_pose;
const transform::Rigid3d optimized_pose =
@ -438,7 +441,7 @@ void SparsePoseGraph::AddSubmapFromProto(const int trajectory_id,
std::make_shared<const Submap>(submap.submap_3d());
common::MutexLocker locker(&mutex_);
trajectory_connectivity_state_.Add(trajectory_id);
AddTrajectoryIfNeeded(trajectory_id);
const mapping::SubmapId submap_id =
submap_data_.Append(trajectory_id, SubmapData());
submap_data_.at(submap_id).submap = submap_ptr;
@ -466,7 +469,7 @@ void SparsePoseGraph::AddNodeFromProto(const int trajectory_id,
mapping::FromProto(node.node_data()));
common::MutexLocker locker(&mutex_);
trajectory_connectivity_state_.Add(trajectory_id);
AddTrajectoryIfNeeded(trajectory_id);
const mapping::NodeId node_id = trajectory_nodes_.Append(
trajectory_id, mapping::TrajectoryNode{constant_data, pose});

3
cartographer/mapping_3d/sparse_pose_graph.h
View File

@ -122,6 +122,9 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
// Handles a new work item.
void AddWorkItem(const std::function<void()>& work_item) REQUIRES(mutex_);
// Adds connectivity and sampler for a trajectory if it does not exist.
void AddTrajectoryIfNeeded(int trajectory_id) REQUIRES(mutex_);
// Grows the optimization problem to have an entry for every element of
// 'insertion_submaps'. Returns the IDs for the 'insertion_submaps'.
std::vector<mapping::SubmapId> GrowSubmapTransformsAsNeeded(