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

Remove the deque for constant node data. (#326) 

This is needed in preparation of deleting nodes.
Related to #283.
master
Wolfgang Hess 2017-06-09 16:53:49 +02:00 committed by GitHub
parent 0c3aa32ad2
commit f07f888dab
5 changed files with 29 additions and 35 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
cartographer/mapping/trajectory_node.h
View File

@ -17,7 +17,7 @@
#ifndef CARTOGRAPHER_MAPPING_TRAJECTORY_NODE_H_
#define CARTOGRAPHER_MAPPING_TRAJECTORY_NODE_H_
#include <deque>
#include <memory>
#include <vector>
#include "Eigen/Core"
@ -31,7 +31,7 @@ namespace mapping {
class Submaps;
struct TrajectoryNode {
struct ConstantData {
struct Data {
common::Time time;
// Range data in 'pose' frame. Only used in the 2D case.
@ -51,7 +51,9 @@ struct TrajectoryNode {
common::Time time() const { return constant_data->time; }
const ConstantData* constant_data;
// This must be a shared_ptr. If the data is used for visualization while the
// node is being deleted, it must survive until all use finishes.
std::shared_ptr<const Data> constant_data;
transform::Rigid3d pose;
};

33
cartographer/mapping_2d/sparse_pose_graph.cc
View File

@ -97,14 +97,15 @@ void SparsePoseGraph::AddScan(
GetLocalToGlobalTransform(trajectory_id) * transform::Embed3D(pose));
common::MutexLocker locker(&mutex_);
constant_node_data_.push_back(mapping::TrajectoryNode::ConstantData{
time, range_data_in_pose,
Compress(sensor::RangeData{Eigen::Vector3f::Zero(), {}, {}}),
trajectory_id, tracking_to_pose});
trajectory_nodes_.Append(trajectory_id,
mapping::TrajectoryNode{
&constant_node_data_.back(), optimized_pose,
});
trajectory_nodes_.Append(
trajectory_id,
mapping::TrajectoryNode{
std::make_shared<const mapping::TrajectoryNode::Data>(
mapping::TrajectoryNode::Data{
time, range_data_in_pose,
Compress(sensor::RangeData{Eigen::Vector3f::Zero(), {}, {}}),
trajectory_id, tracking_to_pose}),
optimized_pose});
++num_trajectory_nodes_;
trajectory_connectivity_.Add(trajectory_id);
@ -224,8 +225,7 @@ void SparsePoseGraph::ComputeConstraintsForScan(
.at(matching_id.trajectory_id)
.size())
: 0};
const mapping::TrajectoryNode::ConstantData* const scan_data =
trajectory_nodes_.at(node_id).constant_data;
const auto& scan_data = trajectory_nodes_.at(node_id).constant_data;
CHECK_EQ(scan_data->trajectory_id, matching_id.trajectory_id);
optimization_problem_.AddTrajectoryNode(
matching_id.trajectory_id, scan_data->time, pose, optimized_pose);
@ -466,8 +466,8 @@ std::vector<transform::Rigid3d> SparsePoseGraph::ExtrapolateSubmapTransforms(
} else if (result.empty()) {
result.push_back(transform::Rigid3d::Identity());
} else {
// Extrapolate to the remaining submaps. Accessing 'local_pose' in Submaps
// is okay, since the member is const.
// Extrapolate to the remaining submaps. Accessing 'local_pose' in
// Submaps is okay, since the member is const.
const mapping::SubmapId previous_submap_id{
trajectory_id, static_cast<int>(result.size()) - 1};
result.push_back(
@ -493,8 +493,8 @@ int SparsePoseGraph::TrimmingHandle::num_submaps(
void SparsePoseGraph::TrimmingHandle::MarkSubmapAsTrimmed(
const mapping::SubmapId& submap_id) {
// TODO(hrapp): We have to make sure that the trajectory has been finished if
// we want to delete the last submaps.
// TODO(hrapp): We have to make sure that the trajectory has been finished
// if we want to delete the last submaps.
CHECK(parent_->submap_data_.at(submap_id).state == SubmapState::kFinished);
// Compile all nodes that are still INTRA_SUBMAP constrained once the submap
@ -538,7 +538,6 @@ void SparsePoseGraph::TrimmingHandle::MarkSubmapAsTrimmed(
// Mark the submap with 'submap_id' as trimmed and remove its data.
parent_->submap_data_.at(submap_id).state = SubmapState::kTrimmed;
parent_->constraint_builder_.DeleteScanMatcher(submap_id);
// TODO(hrapp): Make 'Submap' object thread safe and remove submap data in
// there.
@ -548,8 +547,8 @@ void SparsePoseGraph::TrimmingHandle::MarkSubmapAsTrimmed(
// TODO(whess): The optimization problem should no longer include the submap
// and the removed nodes.
// TODO(whess): If the first submap is gone, we want to tie the first not yet
// trimmed submap to be set fixed to its current pose.
// TODO(whess): If the first submap is gone, we want to tie the first not
// yet trimmed submap to be set fixed to its current pose.
}
} // namespace mapping_2d

4
cartographer/mapping_2d/sparse_pose_graph.h
View File

@ -194,10 +194,6 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
std::map<int, size_t> reverse_connected_components_;
// Data that are currently being shown.
//
// Deque to keep references valid for the background computation when adding
// new data.
std::deque<mapping::TrajectoryNode::ConstantData> constant_node_data_;
mapping::NestedVectorsById<mapping::TrajectoryNode, mapping::NodeId>
trajectory_nodes_ GUARDED_BY(mutex_);
int num_trajectory_nodes_ GUARDED_BY(mutex_) = 0;

15
cartographer/mapping_3d/sparse_pose_graph.cc
View File

@ -95,13 +95,15 @@ void SparsePoseGraph::AddScan(
GetLocalToGlobalTransform(trajectory_id) * pose);
common::MutexLocker locker(&mutex_);
constant_node_data_.push_back(mapping::TrajectoryNode::ConstantData{
time, sensor::RangeData{Eigen::Vector3f::Zero(), {}, {}},
sensor::Compress(range_data_in_tracking), trajectory_id,
transform::Rigid3d::Identity()});
trajectory_nodes_.Append(
trajectory_id,
mapping::TrajectoryNode{&constant_node_data_.back(), optimized_pose});
mapping::TrajectoryNode{
std::make_shared<const mapping::TrajectoryNode::Data>(
mapping::TrajectoryNode::Data{
time, sensor::RangeData{Eigen::Vector3f::Zero(), {}, {}},
sensor::Compress(range_data_in_tracking), trajectory_id,
transform::Rigid3d::Identity()}),
optimized_pose});
++num_trajectory_nodes_;
trajectory_connectivity_.Add(trajectory_id);
@ -240,8 +242,7 @@ void SparsePoseGraph::ComputeConstraintsForScan(
.at(matching_id.trajectory_id)
.size())
: 0};
const mapping::TrajectoryNode::ConstantData* const scan_data =
trajectory_nodes_.at(node_id).constant_data;
const auto& scan_data = trajectory_nodes_.at(node_id).constant_data;
CHECK_EQ(scan_data->trajectory_id, matching_id.trajectory_id);
optimization_problem_.AddTrajectoryNode(matching_id.trajectory_id,
scan_data->time, optimized_pose);

4
cartographer/mapping_3d/sparse_pose_graph.h
View File

@ -191,10 +191,6 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
std::map<int, size_t> reverse_connected_components_;
// Data that are currently being shown.
//
// Deque to keep references valid for the background computation when adding
// new data.
std::deque<mapping::TrajectoryNode::ConstantData> constant_node_data_;
mapping::NestedVectorsById<mapping::TrajectoryNode, mapping::NodeId>
trajectory_nodes_ GUARDED_BY(mutex_);
int num_trajectory_nodes_ GUARDED_BY(mutex_) = 0;