parent
f26e4366c9
commit
8392a7e62c
|
@ -215,7 +215,7 @@ void SparsePoseGraph::ComputeConstraintsForOldScans(
|
||||||
submap_states_.at(submap_id.trajectory_id).at(submap_id.submap_index);
|
submap_states_.at(submap_id.trajectory_id).at(submap_id.submap_index);
|
||||||
const int num_nodes = scan_index_to_node_id_.size();
|
const int num_nodes = scan_index_to_node_id_.size();
|
||||||
for (int scan_index = 0; scan_index < num_nodes; ++scan_index) {
|
for (int scan_index = 0; scan_index < num_nodes; ++scan_index) {
|
||||||
if (submap_state.scan_indices.count(scan_index) == 0) {
|
if (submap_state.node_ids.count(scan_index_to_node_id_[scan_index]) == 0) {
|
||||||
ComputeConstraint(scan_index, submap_id);
|
ComputeConstraint(scan_index, submap_id);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
@ -235,9 +235,10 @@ void SparsePoseGraph::ComputeConstraintsForScan(
|
||||||
.pose *
|
.pose *
|
||||||
sparse_pose_graph::ComputeSubmapPose(*matching_submap).inverse() * pose;
|
sparse_pose_graph::ComputeSubmapPose(*matching_submap).inverse() * pose;
|
||||||
CHECK_EQ(scan_index, scan_index_to_node_id_.size());
|
CHECK_EQ(scan_index, scan_index_to_node_id_.size());
|
||||||
scan_index_to_node_id_.push_back(
|
const mapping::NodeId node_id{
|
||||||
mapping::NodeId{matching_id.trajectory_id,
|
matching_id.trajectory_id,
|
||||||
num_nodes_in_trajectory_[matching_id.trajectory_id]});
|
num_nodes_in_trajectory_[matching_id.trajectory_id]};
|
||||||
|
scan_index_to_node_id_.push_back(node_id);
|
||||||
++num_nodes_in_trajectory_[matching_id.trajectory_id];
|
++num_nodes_in_trajectory_[matching_id.trajectory_id];
|
||||||
const mapping::TrajectoryNode::ConstantData* const scan_data =
|
const mapping::TrajectoryNode::ConstantData* const scan_data =
|
||||||
trajectory_nodes_[scan_index].constant_data;
|
trajectory_nodes_[scan_index].constant_data;
|
||||||
|
@ -252,7 +253,7 @@ void SparsePoseGraph::ComputeConstraintsForScan(
|
||||||
.finished);
|
.finished);
|
||||||
submap_states_.at(submap_id.trajectory_id)
|
submap_states_.at(submap_id.trajectory_id)
|
||||||
.at(submap_id.submap_index)
|
.at(submap_id.submap_index)
|
||||||
.scan_indices.emplace(scan_index);
|
.node_ids.emplace(node_id);
|
||||||
// Unchanged covariance as (submap <- map) is a translation.
|
// Unchanged covariance as (submap <- map) is a translation.
|
||||||
const transform::Rigid2d constraint_transform =
|
const transform::Rigid2d constraint_transform =
|
||||||
sparse_pose_graph::ComputeSubmapPose(*submap).inverse() * pose;
|
sparse_pose_graph::ComputeSubmapPose(*submap).inverse() * pose;
|
||||||
|
@ -260,7 +261,7 @@ void SparsePoseGraph::ComputeConstraintsForScan(
|
||||||
constexpr double kFakeOrientationCovariance = 1e-6;
|
constexpr double kFakeOrientationCovariance = 1e-6;
|
||||||
constraints_.push_back(Constraint{
|
constraints_.push_back(Constraint{
|
||||||
submap_id,
|
submap_id,
|
||||||
scan_index_to_node_id_.at(scan_index),
|
node_id,
|
||||||
{transform::Embed3D(constraint_transform),
|
{transform::Embed3D(constraint_transform),
|
||||||
common::ComputeSpdMatrixSqrtInverse(
|
common::ComputeSpdMatrixSqrtInverse(
|
||||||
kalman_filter::Embed3D(covariance, kFakePositionCovariance,
|
kalman_filter::Embed3D(covariance, kFakePositionCovariance,
|
||||||
|
@ -278,7 +279,7 @@ void SparsePoseGraph::ComputeConstraintsForScan(
|
||||||
if (submap_states_.at(trajectory_id).at(submap_index).finished) {
|
if (submap_states_.at(trajectory_id).at(submap_index).finished) {
|
||||||
CHECK_EQ(submap_states_.at(trajectory_id)
|
CHECK_EQ(submap_states_.at(trajectory_id)
|
||||||
.at(submap_index)
|
.at(submap_index)
|
||||||
.scan_indices.count(scan_index),
|
.node_ids.count(node_id),
|
||||||
0);
|
0);
|
||||||
ComputeConstraint(scan_index,
|
ComputeConstraint(scan_index,
|
||||||
mapping::SubmapId{static_cast<int>(trajectory_id),
|
mapping::SubmapId{static_cast<int>(trajectory_id),
|
||||||
|
@ -388,9 +389,10 @@ void SparsePoseGraph::RunOptimization() {
|
||||||
const auto& node_data = optimization_problem_.node_data();
|
const auto& node_data = optimization_problem_.node_data();
|
||||||
const size_t num_optimized_poses = scan_index_to_node_id_.size();
|
const size_t num_optimized_poses = scan_index_to_node_id_.size();
|
||||||
for (size_t i = 0; i != num_optimized_poses; ++i) {
|
for (size_t i = 0; i != num_optimized_poses; ++i) {
|
||||||
|
const mapping::NodeId node_id = scan_index_to_node_id_[i];
|
||||||
trajectory_nodes_[i].pose =
|
trajectory_nodes_[i].pose =
|
||||||
transform::Embed3D(node_data.at(scan_index_to_node_id_[i].trajectory_id)
|
transform::Embed3D(node_data.at(node_id.trajectory_id)
|
||||||
.at(scan_index_to_node_id_[i].node_index)
|
.at(node_id.node_index)
|
||||||
.point_cloud_pose);
|
.point_cloud_pose);
|
||||||
}
|
}
|
||||||
// Extrapolate all point cloud poses that were added later.
|
// Extrapolate all point cloud poses that were added later.
|
||||||
|
|
|
@ -98,10 +98,10 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
|
||||||
struct SubmapState {
|
struct SubmapState {
|
||||||
const mapping::Submap* submap = nullptr;
|
const mapping::Submap* submap = nullptr;
|
||||||
|
|
||||||
// Indices of the scans that were inserted into this map together with
|
// IDs of the scans that were inserted into this map together with
|
||||||
// constraints for them. They are not to be matched again when this submap
|
// constraints for them. They are not to be matched again when this submap
|
||||||
// becomes 'finished'.
|
// becomes 'finished'.
|
||||||
std::set<int> scan_indices;
|
std::set<mapping::NodeId> node_ids;
|
||||||
|
|
||||||
// Whether in the current state of the background thread this submap is
|
// Whether in the current state of the background thread this submap is
|
||||||
// finished. When this transitions to true, all scans are tried to match
|
// finished. When this transitions to true, all scans are tried to match
|
||||||
|
|
|
@ -211,7 +211,7 @@ void SparsePoseGraph::ComputeConstraintsForOldScans(const Submap* submap) {
|
||||||
submap_states_.at(submap_id.trajectory_id).at(submap_id.submap_index);
|
submap_states_.at(submap_id.trajectory_id).at(submap_id.submap_index);
|
||||||
const int num_nodes = scan_index_to_node_id_.size();
|
const int num_nodes = scan_index_to_node_id_.size();
|
||||||
for (int scan_index = 0; scan_index < num_nodes; ++scan_index) {
|
for (int scan_index = 0; scan_index < num_nodes; ++scan_index) {
|
||||||
if (submap_state.scan_indices.count(scan_index) == 0) {
|
if (submap_state.node_ids.count(scan_index_to_node_id_[scan_index]) == 0) {
|
||||||
ComputeConstraint(scan_index, submap_id);
|
ComputeConstraint(scan_index, submap_id);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
@ -231,9 +231,10 @@ void SparsePoseGraph::ComputeConstraintsForScan(
|
||||||
.pose *
|
.pose *
|
||||||
matching_submap->local_pose().inverse() * pose;
|
matching_submap->local_pose().inverse() * pose;
|
||||||
CHECK_EQ(scan_index, scan_index_to_node_id_.size());
|
CHECK_EQ(scan_index, scan_index_to_node_id_.size());
|
||||||
scan_index_to_node_id_.push_back(
|
const mapping::NodeId node_id{
|
||||||
mapping::NodeId{matching_id.trajectory_id,
|
matching_id.trajectory_id,
|
||||||
num_nodes_in_trajectory_[matching_id.trajectory_id]});
|
num_nodes_in_trajectory_[matching_id.trajectory_id]};
|
||||||
|
scan_index_to_node_id_.push_back(node_id);
|
||||||
++num_nodes_in_trajectory_[matching_id.trajectory_id];
|
++num_nodes_in_trajectory_[matching_id.trajectory_id];
|
||||||
const mapping::TrajectoryNode::ConstantData* const scan_data =
|
const mapping::TrajectoryNode::ConstantData* const scan_data =
|
||||||
trajectory_nodes_[scan_index].constant_data;
|
trajectory_nodes_[scan_index].constant_data;
|
||||||
|
@ -248,13 +249,13 @@ void SparsePoseGraph::ComputeConstraintsForScan(
|
||||||
.finished);
|
.finished);
|
||||||
submap_states_.at(submap_id.trajectory_id)
|
submap_states_.at(submap_id.trajectory_id)
|
||||||
.at(submap_id.submap_index)
|
.at(submap_id.submap_index)
|
||||||
.scan_indices.emplace(scan_index);
|
.node_ids.emplace(node_id);
|
||||||
// Unchanged covariance as (submap <- map) is a translation.
|
// Unchanged covariance as (submap <- map) is a translation.
|
||||||
const transform::Rigid3d constraint_transform =
|
const transform::Rigid3d constraint_transform =
|
||||||
submap->local_pose().inverse() * pose;
|
submap->local_pose().inverse() * pose;
|
||||||
constraints_.push_back(
|
constraints_.push_back(
|
||||||
Constraint{submap_id,
|
Constraint{submap_id,
|
||||||
scan_index_to_node_id_.at(scan_index),
|
node_id,
|
||||||
{constraint_transform,
|
{constraint_transform,
|
||||||
common::ComputeSpdMatrixSqrtInverse(
|
common::ComputeSpdMatrixSqrtInverse(
|
||||||
covariance, options_.constraint_builder_options()
|
covariance, options_.constraint_builder_options()
|
||||||
|
@ -270,7 +271,7 @@ void SparsePoseGraph::ComputeConstraintsForScan(
|
||||||
if (submap_states_.at(trajectory_id).at(submap_index).finished) {
|
if (submap_states_.at(trajectory_id).at(submap_index).finished) {
|
||||||
CHECK_EQ(submap_states_.at(trajectory_id)
|
CHECK_EQ(submap_states_.at(trajectory_id)
|
||||||
.at(submap_index)
|
.at(submap_index)
|
||||||
.scan_indices.count(scan_index),
|
.node_ids.count(node_id),
|
||||||
0);
|
0);
|
||||||
ComputeConstraint(scan_index,
|
ComputeConstraint(scan_index,
|
||||||
mapping::SubmapId{static_cast<int>(trajectory_id),
|
mapping::SubmapId{static_cast<int>(trajectory_id),
|
||||||
|
@ -380,10 +381,10 @@ void SparsePoseGraph::RunOptimization() {
|
||||||
const auto& node_data = optimization_problem_.node_data();
|
const auto& node_data = optimization_problem_.node_data();
|
||||||
const size_t num_optimized_poses = scan_index_to_node_id_.size();
|
const size_t num_optimized_poses = scan_index_to_node_id_.size();
|
||||||
for (size_t i = 0; i != num_optimized_poses; ++i) {
|
for (size_t i = 0; i != num_optimized_poses; ++i) {
|
||||||
trajectory_nodes_[i].pose =
|
const mapping::NodeId node_id = scan_index_to_node_id_[i];
|
||||||
node_data.at(scan_index_to_node_id_[i].trajectory_id)
|
trajectory_nodes_[i].pose = node_data.at(node_id.trajectory_id)
|
||||||
.at(scan_index_to_node_id_[i].node_index)
|
.at(node_id.node_index)
|
||||||
.point_cloud_pose;
|
.point_cloud_pose;
|
||||||
}
|
}
|
||||||
// Extrapolate all point cloud poses that were added later.
|
// Extrapolate all point cloud poses that were added later.
|
||||||
std::unordered_map<const mapping::Submaps*, transform::Rigid3d>
|
std::unordered_map<const mapping::Submaps*, transform::Rigid3d>
|
||||||
|
|
|
@ -100,10 +100,10 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
|
||||||
struct SubmapState {
|
struct SubmapState {
|
||||||
const Submap* submap = nullptr;
|
const Submap* submap = nullptr;
|
||||||
|
|
||||||
// Indices of the scans that were inserted into this map together with
|
// IDs of the scans that were inserted into this map together with
|
||||||
// constraints for them. They are not to be matched again when this submap
|
// constraints for them. They are not to be matched again when this submap
|
||||||
// becomes 'finished'.
|
// becomes 'finished'.
|
||||||
std::set<int> scan_indices;
|
std::set<mapping::NodeId> node_ids;
|
||||||
|
|
||||||
// Whether in the current state of the background thread this submap is
|
// Whether in the current state of the background thread this submap is
|
||||||
// finished. When this transitions to true, all scans are tried to match
|
// finished. When this transitions to true, all scans are tried to match
|
||||||
|
|
Loading…
Reference in New Issue