-
Notifications
You must be signed in to change notification settings - Fork 2.2k
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.
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
Delete SubmapData from trimmed submaps in the OptimizationProblem. #366
Delete SubmapData from trimmed submaps in the OptimizationProblem. #366
Conversation
@@ -163,7 +171,9 @@ void OptimizationProblem::Solve(const std::vector<Constraint>& constraints) { | |||
? new ceres::HuberLoss(options_.huber_scale()) | |||
: nullptr, | |||
C_submaps.at(constraint.submap_id.trajectory_id) | |||
.at(constraint.submap_id.submap_index) | |||
.at(constraint.submap_id.submap_index - | |||
trajectory_data_.at(constraint.node_id.trajectory_id) |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
constraint.submap_id.trajectory_id
otherwise multi-trajectory will not work correctly.
auto& trajectory_data = trajectory_data_.at(submap_id.trajectory_id); | ||
// We only allow trimming from the start. | ||
CHECK_EQ(trajectory_data.num_trimmed_submaps, submap_id.submap_index); | ||
submap_data_.at(submap_id.trajectory_id).pop_front(); |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Similar to nodes: Extract submap_data
and CHECK
for non-empty?
@@ -199,7 +200,8 @@ class SparsePoseGraph : public mapping::SparsePoseGraph { | |||
int num_trajectory_nodes_ GUARDED_BY(mutex_) = 0; | |||
|
|||
// Current submap transforms used for displaying data. | |||
std::vector<std::vector<sparse_pose_graph::SubmapData>> | |||
std::vector<int> num_trimmed_submaps_at_last_optimization_; |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
GUARDED_BY(mutex_)
?
@@ -402,10 +410,14 @@ void SparsePoseGraph::RunOptimization() { | |||
node_data[trajectory_id][node_data_index].point_cloud_pose); | |||
} | |||
// Extrapolate all point cloud poses that were added later. | |||
const auto local_to_new_global = ComputeLocalToGlobalTransform( | |||
optimization_problem_.submap_data(), trajectory_id); | |||
num_trimmed_submaps.push_back( |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Make a separate loop over the trajectories in submap_data()
?
11a0944
to
7f27b9e
Compare
.at(submap_id.submap_index) | ||
.pose)}; | ||
static_cast<int>(optimized_submap_transforms_.size())) { | ||
const size_t submap_index = |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
submap_data_index
?
@@ -511,18 +532,23 @@ mapping::SparsePoseGraph::SubmapData SparsePoseGraph::GetSubmapDataUnderLock( | |||
auto submap = submap_data_.at(submap_id).submap; | |||
// We already have an optimized pose. |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Move to the nested if?
bee1f59
to
bc253a7
Compare
Rviz has a limit of 16384 points per marker. To get around this, each trajectory is split into multiple markers, each up to 16384 points. Fixes cartographer-project#366.
Related to #283.