Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Remove the deque for constant node data. #326

Merged
merged 1 commit into from
Jun 9, 2017
Merged

Remove the deque for constant node data. #326

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
8 changes: 5 additions & 3 deletions cartographer/mapping/trajectory_node.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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"
Expand All @@ -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.
Expand All @@ -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;
};
Expand Down
33 changes: 16 additions & 17 deletions cartographer/mapping_2d/sparse_pose_graph.cc
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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);

Expand Down Expand Up @@ -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);
Expand Down Expand Up @@ -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(
Expand All @@ -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
Expand Down Expand Up @@ -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.

Expand All @@ -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
Expand Down
4 changes: 0 additions & 4 deletions cartographer/mapping_2d/sparse_pose_graph.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down
15 changes: 8 additions & 7 deletions cartographer/mapping_3d/sparse_pose_graph.cc
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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);

Expand Down Expand Up @@ -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);
Expand Down
4 changes: 0 additions & 4 deletions cartographer/mapping_3d/sparse_pose_graph.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down