Remove the deque for constant node data. (#326)
This is needed in preparation of deleting nodes. Related to #283.master
parent
0c3aa32ad2
commit
f07f888dab
|
@ -17,7 +17,7 @@
|
||||||
#ifndef CARTOGRAPHER_MAPPING_TRAJECTORY_NODE_H_
|
#ifndef CARTOGRAPHER_MAPPING_TRAJECTORY_NODE_H_
|
||||||
#define CARTOGRAPHER_MAPPING_TRAJECTORY_NODE_H_
|
#define CARTOGRAPHER_MAPPING_TRAJECTORY_NODE_H_
|
||||||
|
|
||||||
#include <deque>
|
#include <memory>
|
||||||
#include <vector>
|
#include <vector>
|
||||||
|
|
||||||
#include "Eigen/Core"
|
#include "Eigen/Core"
|
||||||
|
@ -31,7 +31,7 @@ namespace mapping {
|
||||||
class Submaps;
|
class Submaps;
|
||||||
|
|
||||||
struct TrajectoryNode {
|
struct TrajectoryNode {
|
||||||
struct ConstantData {
|
struct Data {
|
||||||
common::Time time;
|
common::Time time;
|
||||||
|
|
||||||
// Range data in 'pose' frame. Only used in the 2D case.
|
// 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; }
|
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;
|
transform::Rigid3d pose;
|
||||||
};
|
};
|
||||||
|
|
|
@ -97,14 +97,15 @@ void SparsePoseGraph::AddScan(
|
||||||
GetLocalToGlobalTransform(trajectory_id) * transform::Embed3D(pose));
|
GetLocalToGlobalTransform(trajectory_id) * transform::Embed3D(pose));
|
||||||
|
|
||||||
common::MutexLocker locker(&mutex_);
|
common::MutexLocker locker(&mutex_);
|
||||||
constant_node_data_.push_back(mapping::TrajectoryNode::ConstantData{
|
trajectory_nodes_.Append(
|
||||||
time, range_data_in_pose,
|
trajectory_id,
|
||||||
Compress(sensor::RangeData{Eigen::Vector3f::Zero(), {}, {}}),
|
mapping::TrajectoryNode{
|
||||||
trajectory_id, tracking_to_pose});
|
std::make_shared<const mapping::TrajectoryNode::Data>(
|
||||||
trajectory_nodes_.Append(trajectory_id,
|
mapping::TrajectoryNode::Data{
|
||||||
mapping::TrajectoryNode{
|
time, range_data_in_pose,
|
||||||
&constant_node_data_.back(), optimized_pose,
|
Compress(sensor::RangeData{Eigen::Vector3f::Zero(), {}, {}}),
|
||||||
});
|
trajectory_id, tracking_to_pose}),
|
||||||
|
optimized_pose});
|
||||||
++num_trajectory_nodes_;
|
++num_trajectory_nodes_;
|
||||||
trajectory_connectivity_.Add(trajectory_id);
|
trajectory_connectivity_.Add(trajectory_id);
|
||||||
|
|
||||||
|
@ -224,8 +225,7 @@ void SparsePoseGraph::ComputeConstraintsForScan(
|
||||||
.at(matching_id.trajectory_id)
|
.at(matching_id.trajectory_id)
|
||||||
.size())
|
.size())
|
||||||
: 0};
|
: 0};
|
||||||
const mapping::TrajectoryNode::ConstantData* const scan_data =
|
const auto& scan_data = trajectory_nodes_.at(node_id).constant_data;
|
||||||
trajectory_nodes_.at(node_id).constant_data;
|
|
||||||
CHECK_EQ(scan_data->trajectory_id, matching_id.trajectory_id);
|
CHECK_EQ(scan_data->trajectory_id, matching_id.trajectory_id);
|
||||||
optimization_problem_.AddTrajectoryNode(
|
optimization_problem_.AddTrajectoryNode(
|
||||||
matching_id.trajectory_id, scan_data->time, pose, optimized_pose);
|
matching_id.trajectory_id, scan_data->time, pose, optimized_pose);
|
||||||
|
@ -466,8 +466,8 @@ std::vector<transform::Rigid3d> SparsePoseGraph::ExtrapolateSubmapTransforms(
|
||||||
} else if (result.empty()) {
|
} else if (result.empty()) {
|
||||||
result.push_back(transform::Rigid3d::Identity());
|
result.push_back(transform::Rigid3d::Identity());
|
||||||
} else {
|
} else {
|
||||||
// Extrapolate to the remaining submaps. Accessing 'local_pose' in Submaps
|
// Extrapolate to the remaining submaps. Accessing 'local_pose' in
|
||||||
// is okay, since the member is const.
|
// Submaps is okay, since the member is const.
|
||||||
const mapping::SubmapId previous_submap_id{
|
const mapping::SubmapId previous_submap_id{
|
||||||
trajectory_id, static_cast<int>(result.size()) - 1};
|
trajectory_id, static_cast<int>(result.size()) - 1};
|
||||||
result.push_back(
|
result.push_back(
|
||||||
|
@ -493,8 +493,8 @@ int SparsePoseGraph::TrimmingHandle::num_submaps(
|
||||||
|
|
||||||
void SparsePoseGraph::TrimmingHandle::MarkSubmapAsTrimmed(
|
void SparsePoseGraph::TrimmingHandle::MarkSubmapAsTrimmed(
|
||||||
const mapping::SubmapId& submap_id) {
|
const mapping::SubmapId& submap_id) {
|
||||||
// TODO(hrapp): We have to make sure that the trajectory has been finished if
|
// TODO(hrapp): We have to make sure that the trajectory has been finished
|
||||||
// we want to delete the last submaps.
|
// if we want to delete the last submaps.
|
||||||
CHECK(parent_->submap_data_.at(submap_id).state == SubmapState::kFinished);
|
CHECK(parent_->submap_data_.at(submap_id).state == SubmapState::kFinished);
|
||||||
|
|
||||||
// Compile all nodes that are still INTRA_SUBMAP constrained once the submap
|
// 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.
|
// Mark the submap with 'submap_id' as trimmed and remove its data.
|
||||||
parent_->submap_data_.at(submap_id).state = SubmapState::kTrimmed;
|
parent_->submap_data_.at(submap_id).state = SubmapState::kTrimmed;
|
||||||
parent_->constraint_builder_.DeleteScanMatcher(submap_id);
|
parent_->constraint_builder_.DeleteScanMatcher(submap_id);
|
||||||
|
|
||||||
// TODO(hrapp): Make 'Submap' object thread safe and remove submap data in
|
// TODO(hrapp): Make 'Submap' object thread safe and remove submap data in
|
||||||
// there.
|
// there.
|
||||||
|
|
||||||
|
@ -548,8 +547,8 @@ void SparsePoseGraph::TrimmingHandle::MarkSubmapAsTrimmed(
|
||||||
// TODO(whess): The optimization problem should no longer include the submap
|
// TODO(whess): The optimization problem should no longer include the submap
|
||||||
// and the removed nodes.
|
// and the removed nodes.
|
||||||
|
|
||||||
// TODO(whess): If the first submap is gone, we want to tie the first not yet
|
// TODO(whess): If the first submap is gone, we want to tie the first not
|
||||||
// trimmed submap to be set fixed to its current pose.
|
// yet trimmed submap to be set fixed to its current pose.
|
||||||
}
|
}
|
||||||
|
|
||||||
} // namespace mapping_2d
|
} // namespace mapping_2d
|
||||||
|
|
|
@ -194,10 +194,6 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
|
||||||
std::map<int, size_t> reverse_connected_components_;
|
std::map<int, size_t> reverse_connected_components_;
|
||||||
|
|
||||||
// Data that are currently being shown.
|
// 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>
|
mapping::NestedVectorsById<mapping::TrajectoryNode, mapping::NodeId>
|
||||||
trajectory_nodes_ GUARDED_BY(mutex_);
|
trajectory_nodes_ GUARDED_BY(mutex_);
|
||||||
int num_trajectory_nodes_ GUARDED_BY(mutex_) = 0;
|
int num_trajectory_nodes_ GUARDED_BY(mutex_) = 0;
|
||||||
|
|
|
@ -95,13 +95,15 @@ void SparsePoseGraph::AddScan(
|
||||||
GetLocalToGlobalTransform(trajectory_id) * pose);
|
GetLocalToGlobalTransform(trajectory_id) * pose);
|
||||||
|
|
||||||
common::MutexLocker locker(&mutex_);
|
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_nodes_.Append(
|
||||||
trajectory_id,
|
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_;
|
++num_trajectory_nodes_;
|
||||||
trajectory_connectivity_.Add(trajectory_id);
|
trajectory_connectivity_.Add(trajectory_id);
|
||||||
|
|
||||||
|
@ -240,8 +242,7 @@ void SparsePoseGraph::ComputeConstraintsForScan(
|
||||||
.at(matching_id.trajectory_id)
|
.at(matching_id.trajectory_id)
|
||||||
.size())
|
.size())
|
||||||
: 0};
|
: 0};
|
||||||
const mapping::TrajectoryNode::ConstantData* const scan_data =
|
const auto& scan_data = trajectory_nodes_.at(node_id).constant_data;
|
||||||
trajectory_nodes_.at(node_id).constant_data;
|
|
||||||
CHECK_EQ(scan_data->trajectory_id, matching_id.trajectory_id);
|
CHECK_EQ(scan_data->trajectory_id, matching_id.trajectory_id);
|
||||||
optimization_problem_.AddTrajectoryNode(matching_id.trajectory_id,
|
optimization_problem_.AddTrajectoryNode(matching_id.trajectory_id,
|
||||||
scan_data->time, optimized_pose);
|
scan_data->time, optimized_pose);
|
||||||
|
|
|
@ -191,10 +191,6 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
|
||||||
std::map<int, size_t> reverse_connected_components_;
|
std::map<int, size_t> reverse_connected_components_;
|
||||||
|
|
||||||
// Data that are currently being shown.
|
// 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>
|
mapping::NestedVectorsById<mapping::TrajectoryNode, mapping::NodeId>
|
||||||
trajectory_nodes_ GUARDED_BY(mutex_);
|
trajectory_nodes_ GUARDED_BY(mutex_);
|
||||||
int num_trajectory_nodes_ GUARDED_BY(mutex_) = 0;
|
int num_trajectory_nodes_ GUARDED_BY(mutex_) = 0;
|
||||||
|
|
Loading…
Reference in New Issue