Make use of Submap in 2D similar to 3D. (#343)
Use mapping_2d::Submap instead of mapping::Submap in 2D. Use `finished()` instead of `finished_probability_grid()` in 2D. PAIR=wohemaster
parent
8d94fe85e9
commit
b944b19159
|
@ -67,13 +67,6 @@ class Submap {
|
||||||
// Number of RangeData inserted.
|
// Number of RangeData inserted.
|
||||||
size_t num_range_data() const { return num_range_data_; }
|
size_t num_range_data() const { return num_range_data_; }
|
||||||
|
|
||||||
// The 'finished_probability_grid' when this submap is finished and will not
|
|
||||||
// change anymore. Otherwise, this is nullptr and the next call to
|
|
||||||
// InsertRangeData() will change the submap.
|
|
||||||
const mapping_2d::ProbabilityGrid* finished_probability_grid() const {
|
|
||||||
return finished_probability_grid_;
|
|
||||||
}
|
|
||||||
|
|
||||||
// Fills data into the 'response'.
|
// Fills data into the 'response'.
|
||||||
virtual void ToResponseProto(
|
virtual void ToResponseProto(
|
||||||
const transform::Rigid3d& global_submap_pose,
|
const transform::Rigid3d& global_submap_pose,
|
||||||
|
@ -83,9 +76,8 @@ class Submap {
|
||||||
const transform::Rigid3d local_pose_;
|
const transform::Rigid3d local_pose_;
|
||||||
|
|
||||||
protected:
|
protected:
|
||||||
// TODO(hrapp): All of this should be private.
|
// TODO(hrapp): This should be private.
|
||||||
int num_range_data_ = 0;
|
int num_range_data_ = 0;
|
||||||
const mapping_2d::ProbabilityGrid* finished_probability_grid_ = nullptr;
|
|
||||||
};
|
};
|
||||||
|
|
||||||
// Submaps is a sequence of maps to which scans are matched and into which scans
|
// Submaps is a sequence of maps to which scans are matched and into which scans
|
||||||
|
|
|
@ -45,13 +45,19 @@ void GlobalTrajectoryBuilder::AddRangefinderData(
|
||||||
std::unique_ptr<LocalTrajectoryBuilder::InsertionResult> insertion_result =
|
std::unique_ptr<LocalTrajectoryBuilder::InsertionResult> insertion_result =
|
||||||
local_trajectory_builder_.AddHorizontalRangeData(
|
local_trajectory_builder_.AddHorizontalRangeData(
|
||||||
time, sensor::RangeData{origin, ranges, {}});
|
time, sensor::RangeData{origin, ranges, {}});
|
||||||
if (insertion_result != nullptr) {
|
if (insertion_result == nullptr) {
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
const Submap* const finished_submap =
|
||||||
|
insertion_result->insertion_submaps.front()->finished()
|
||||||
|
? insertion_result->insertion_submaps.front()
|
||||||
|
: nullptr;
|
||||||
sparse_pose_graph_->AddScan(
|
sparse_pose_graph_->AddScan(
|
||||||
insertion_result->time, insertion_result->tracking_to_tracking_2d,
|
insertion_result->time, insertion_result->tracking_to_tracking_2d,
|
||||||
insertion_result->range_data_in_tracking_2d,
|
insertion_result->range_data_in_tracking_2d,
|
||||||
insertion_result->pose_estimate_2d, trajectory_id_,
|
insertion_result->pose_estimate_2d, trajectory_id_,
|
||||||
insertion_result->matching_submap, insertion_result->insertion_submaps);
|
insertion_result->matching_submap, insertion_result->insertion_submaps,
|
||||||
}
|
finished_submap);
|
||||||
}
|
}
|
||||||
|
|
||||||
void GlobalTrajectoryBuilder::AddImuData(
|
void GlobalTrajectoryBuilder::AddImuData(
|
||||||
|
|
|
@ -176,9 +176,8 @@ LocalTrajectoryBuilder::AddHorizontalRangeData(
|
||||||
return nullptr;
|
return nullptr;
|
||||||
}
|
}
|
||||||
|
|
||||||
const mapping::Submap* const matching_submap =
|
const Submap* const matching_submap = submaps_.Get(submaps_.matching_index());
|
||||||
submaps_.Get(submaps_.matching_index());
|
std::vector<const Submap*> insertion_submaps;
|
||||||
std::vector<const mapping::Submap*> insertion_submaps;
|
|
||||||
for (int insertion_index : submaps_.insertion_indices()) {
|
for (int insertion_index : submaps_.insertion_indices()) {
|
||||||
insertion_submaps.push_back(submaps_.Get(insertion_index));
|
insertion_submaps.push_back(submaps_.Get(insertion_index));
|
||||||
}
|
}
|
||||||
|
|
|
@ -41,8 +41,8 @@ class LocalTrajectoryBuilder {
|
||||||
public:
|
public:
|
||||||
struct InsertionResult {
|
struct InsertionResult {
|
||||||
common::Time time;
|
common::Time time;
|
||||||
const mapping::Submap* matching_submap;
|
const Submap* matching_submap;
|
||||||
std::vector<const mapping::Submap*> insertion_submaps;
|
std::vector<const Submap*> insertion_submaps;
|
||||||
transform::Rigid3d tracking_to_tracking_2d;
|
transform::Rigid3d tracking_to_tracking_2d;
|
||||||
sensor::RangeData range_data_in_tracking_2d;
|
sensor::RangeData range_data_in_tracking_2d;
|
||||||
transform::Rigid2d pose_estimate_2d;
|
transform::Rigid2d pose_estimate_2d;
|
||||||
|
|
|
@ -53,7 +53,7 @@ SparsePoseGraph::~SparsePoseGraph() {
|
||||||
}
|
}
|
||||||
|
|
||||||
void SparsePoseGraph::GrowSubmapTransformsAsNeeded(
|
void SparsePoseGraph::GrowSubmapTransformsAsNeeded(
|
||||||
const std::vector<const mapping::Submap*>& insertion_submaps) {
|
const std::vector<const Submap*>& insertion_submaps) {
|
||||||
CHECK(!insertion_submaps.empty());
|
CHECK(!insertion_submaps.empty());
|
||||||
const mapping::SubmapId first_submap_id = GetSubmapId(insertion_submaps[0]);
|
const mapping::SubmapId first_submap_id = GetSubmapId(insertion_submaps[0]);
|
||||||
const int trajectory_id = first_submap_id.trajectory_id;
|
const int trajectory_id = first_submap_id.trajectory_id;
|
||||||
|
@ -91,8 +91,9 @@ void SparsePoseGraph::GrowSubmapTransformsAsNeeded(
|
||||||
void SparsePoseGraph::AddScan(
|
void SparsePoseGraph::AddScan(
|
||||||
common::Time time, const transform::Rigid3d& tracking_to_pose,
|
common::Time time, const transform::Rigid3d& tracking_to_pose,
|
||||||
const sensor::RangeData& range_data_in_pose, const transform::Rigid2d& pose,
|
const sensor::RangeData& range_data_in_pose, const transform::Rigid2d& pose,
|
||||||
const int trajectory_id, const mapping::Submap* const matching_submap,
|
const int trajectory_id, const Submap* const matching_submap,
|
||||||
const std::vector<const mapping::Submap*>& insertion_submaps) {
|
const std::vector<const Submap*>& insertion_submaps,
|
||||||
|
const Submap* const finished_submap) {
|
||||||
const transform::Rigid3d optimized_pose(
|
const transform::Rigid3d optimized_pose(
|
||||||
GetLocalToGlobalTransform(trajectory_id) * transform::Embed3D(pose));
|
GetLocalToGlobalTransform(trajectory_id) * transform::Embed3D(pose));
|
||||||
|
|
||||||
|
@ -115,10 +116,6 @@ void SparsePoseGraph::AddScan(
|
||||||
submap_ids_.emplace(insertion_submaps.back(), submap_id);
|
submap_ids_.emplace(insertion_submaps.back(), submap_id);
|
||||||
submap_data_.at(submap_id).submap = insertion_submaps.back();
|
submap_data_.at(submap_id).submap = insertion_submaps.back();
|
||||||
}
|
}
|
||||||
const mapping::Submap* const finished_submap =
|
|
||||||
insertion_submaps.front()->finished_probability_grid() != nullptr
|
|
||||||
? insertion_submaps.front()
|
|
||||||
: nullptr;
|
|
||||||
|
|
||||||
// Make sure we have a sampler for this trajectory.
|
// Make sure we have a sampler for this trajectory.
|
||||||
if (!global_localization_samplers_[trajectory_id]) {
|
if (!global_localization_samplers_[trajectory_id]) {
|
||||||
|
@ -188,8 +185,7 @@ void SparsePoseGraph::ComputeConstraint(const mapping::NodeId& node_id,
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
void SparsePoseGraph::ComputeConstraintsForOldScans(
|
void SparsePoseGraph::ComputeConstraintsForOldScans(const Submap* submap) {
|
||||||
const mapping::Submap* submap) {
|
|
||||||
const auto submap_id = GetSubmapId(submap);
|
const auto submap_id = GetSubmapId(submap);
|
||||||
const auto& submap_data = submap_data_.at(submap_id);
|
const auto& submap_data = submap_data_.at(submap_id);
|
||||||
|
|
||||||
|
@ -209,9 +205,8 @@ void SparsePoseGraph::ComputeConstraintsForOldScans(
|
||||||
}
|
}
|
||||||
|
|
||||||
void SparsePoseGraph::ComputeConstraintsForScan(
|
void SparsePoseGraph::ComputeConstraintsForScan(
|
||||||
const mapping::Submap* matching_submap,
|
const Submap* matching_submap, std::vector<const Submap*> insertion_submaps,
|
||||||
std::vector<const mapping::Submap*> insertion_submaps,
|
const Submap* finished_submap, const transform::Rigid2d& pose) {
|
||||||
const mapping::Submap* finished_submap, const transform::Rigid2d& pose) {
|
|
||||||
GrowSubmapTransformsAsNeeded(insertion_submaps);
|
GrowSubmapTransformsAsNeeded(insertion_submaps);
|
||||||
const mapping::SubmapId matching_id = GetSubmapId(matching_submap);
|
const mapping::SubmapId matching_id = GetSubmapId(matching_submap);
|
||||||
const transform::Rigid2d optimized_pose =
|
const transform::Rigid2d optimized_pose =
|
||||||
|
@ -231,7 +226,7 @@ void SparsePoseGraph::ComputeConstraintsForScan(
|
||||||
const auto& scan_data = trajectory_nodes_.at(node_id).constant_data;
|
const auto& scan_data = trajectory_nodes_.at(node_id).constant_data;
|
||||||
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);
|
||||||
for (const mapping::Submap* submap : insertion_submaps) {
|
for (const Submap* submap : insertion_submaps) {
|
||||||
const mapping::SubmapId submap_id = GetSubmapId(submap);
|
const mapping::SubmapId submap_id = GetSubmapId(submap);
|
||||||
CHECK(submap_data_.at(submap_id).state == SubmapState::kActive);
|
CHECK(submap_data_.at(submap_id).state == SubmapState::kActive);
|
||||||
submap_data_.at(submap_id).node_ids.emplace(node_id);
|
submap_data_.at(submap_id).node_ids.emplace(node_id);
|
||||||
|
|
|
@ -66,13 +66,15 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
|
||||||
// that will later be optimized. The 'tracking_to_pose' is remembered so
|
// that will later be optimized. The 'tracking_to_pose' is remembered so
|
||||||
// that the optimized pose can be embedded into 3D. The 'pose' was determined
|
// that the optimized pose can be embedded into 3D. The 'pose' was determined
|
||||||
// by scan matching against the 'matching_submap' and the scan was inserted
|
// by scan matching against the 'matching_submap' and the scan was inserted
|
||||||
// into the 'insertion_submaps'.
|
// into the 'insertion_submaps'. If not 'nullptr', 'finished_submap' is a
|
||||||
|
// freshly finished submap. It is also contained in 'insertion_submaps' for
|
||||||
|
// the last time.
|
||||||
void AddScan(common::Time time, const transform::Rigid3d& tracking_to_pose,
|
void AddScan(common::Time time, const transform::Rigid3d& tracking_to_pose,
|
||||||
const sensor::RangeData& range_data_in_pose,
|
const sensor::RangeData& range_data_in_pose,
|
||||||
const transform::Rigid2d& pose, int trajectory_id,
|
const transform::Rigid2d& pose, int trajectory_id,
|
||||||
const mapping::Submap* matching_submap,
|
const Submap* matching_submap,
|
||||||
const std::vector<const mapping::Submap*>& insertion_submaps)
|
const std::vector<const Submap*>& insertion_submaps,
|
||||||
EXCLUDES(mutex_);
|
const Submap* finished_submap) EXCLUDES(mutex_);
|
||||||
|
|
||||||
// Adds new IMU data to be used in the optimization.
|
// Adds new IMU data to be used in the optimization.
|
||||||
void AddImuData(int trajectory_id, common::Time time,
|
void AddImuData(int trajectory_id, common::Time time,
|
||||||
|
@ -97,7 +99,7 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
|
||||||
// Likewise, all new scans are matched against submaps which are finished.
|
// Likewise, all new scans are matched against submaps which are finished.
|
||||||
enum class SubmapState { kActive, kFinished, kTrimmed };
|
enum class SubmapState { kActive, kFinished, kTrimmed };
|
||||||
struct SubmapData {
|
struct SubmapData {
|
||||||
const mapping::Submap* submap = nullptr;
|
const Submap* submap = nullptr;
|
||||||
|
|
||||||
// IDs 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
|
||||||
|
@ -110,8 +112,7 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
|
||||||
// Handles a new work item.
|
// Handles a new work item.
|
||||||
void AddWorkItem(std::function<void()> work_item) REQUIRES(mutex_);
|
void AddWorkItem(std::function<void()> work_item) REQUIRES(mutex_);
|
||||||
|
|
||||||
mapping::SubmapId GetSubmapId(const mapping::Submap* submap) const
|
mapping::SubmapId GetSubmapId(const Submap* submap) const REQUIRES(mutex_) {
|
||||||
REQUIRES(mutex_) {
|
|
||||||
const auto iterator = submap_ids_.find(submap);
|
const auto iterator = submap_ids_.find(submap);
|
||||||
CHECK(iterator != submap_ids_.end());
|
CHECK(iterator != submap_ids_.end());
|
||||||
return iterator->second;
|
return iterator->second;
|
||||||
|
@ -120,14 +121,13 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
|
||||||
// Grows the optimization problem to have an entry for every element of
|
// Grows the optimization problem to have an entry for every element of
|
||||||
// 'insertion_submaps'.
|
// 'insertion_submaps'.
|
||||||
void GrowSubmapTransformsAsNeeded(
|
void GrowSubmapTransformsAsNeeded(
|
||||||
const std::vector<const mapping::Submap*>& insertion_submaps)
|
const std::vector<const Submap*>& insertion_submaps) REQUIRES(mutex_);
|
||||||
REQUIRES(mutex_);
|
|
||||||
|
|
||||||
// Adds constraints for a scan, and starts scan matching in the background.
|
// Adds constraints for a scan, and starts scan matching in the background.
|
||||||
void ComputeConstraintsForScan(
|
void ComputeConstraintsForScan(const Submap* matching_submap,
|
||||||
const mapping::Submap* matching_submap,
|
std::vector<const Submap*> insertion_submaps,
|
||||||
std::vector<const mapping::Submap*> insertion_submaps,
|
const Submap* finished_submap,
|
||||||
const mapping::Submap* finished_submap, const transform::Rigid2d& pose)
|
const transform::Rigid2d& pose)
|
||||||
REQUIRES(mutex_);
|
REQUIRES(mutex_);
|
||||||
|
|
||||||
// Computes constraints for a scan and submap pair.
|
// Computes constraints for a scan and submap pair.
|
||||||
|
@ -135,8 +135,7 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
|
||||||
const mapping::SubmapId& submap_id) REQUIRES(mutex_);
|
const mapping::SubmapId& submap_id) REQUIRES(mutex_);
|
||||||
|
|
||||||
// Adds constraints for older scans whenever a new submap is finished.
|
// Adds constraints for older scans whenever a new submap is finished.
|
||||||
void ComputeConstraintsForOldScans(const mapping::Submap* submap)
|
void ComputeConstraintsForOldScans(const Submap* submap) REQUIRES(mutex_);
|
||||||
REQUIRES(mutex_);
|
|
||||||
|
|
||||||
// Registers the callback to run the optimization once all constraints have
|
// Registers the callback to run the optimization once all constraints have
|
||||||
// been computed, that will also do all work that queue up in 'scan_queue_'.
|
// been computed, that will also do all work that queue up in 'scan_queue_'.
|
||||||
|
@ -185,8 +184,7 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
|
||||||
|
|
||||||
// Submaps get assigned an ID and state as soon as they are seen, even
|
// Submaps get assigned an ID and state as soon as they are seen, even
|
||||||
// before they take part in the background computations.
|
// before they take part in the background computations.
|
||||||
std::map<const mapping::Submap*, mapping::SubmapId> submap_ids_
|
std::map<const Submap*, mapping::SubmapId> submap_ids_ GUARDED_BY(mutex_);
|
||||||
GUARDED_BY(mutex_);
|
|
||||||
mapping::NestedVectorsById<SubmapData, mapping::SubmapId> submap_data_
|
mapping::NestedVectorsById<SubmapData, mapping::SubmapId> submap_data_
|
||||||
GUARDED_BY(mutex_);
|
GUARDED_BY(mutex_);
|
||||||
|
|
||||||
|
|
|
@ -38,7 +38,7 @@ namespace cartographer {
|
||||||
namespace mapping_2d {
|
namespace mapping_2d {
|
||||||
namespace sparse_pose_graph {
|
namespace sparse_pose_graph {
|
||||||
|
|
||||||
transform::Rigid2d ComputeSubmapPose(const mapping::Submap& submap) {
|
transform::Rigid2d ComputeSubmapPose(const Submap& submap) {
|
||||||
return transform::Project2D(submap.local_pose());
|
return transform::Project2D(submap.local_pose());
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -60,7 +60,7 @@ ConstraintBuilder::~ConstraintBuilder() {
|
||||||
}
|
}
|
||||||
|
|
||||||
void ConstraintBuilder::MaybeAddConstraint(
|
void ConstraintBuilder::MaybeAddConstraint(
|
||||||
const mapping::SubmapId& submap_id, const mapping::Submap* const submap,
|
const mapping::SubmapId& submap_id, const Submap* const submap,
|
||||||
const mapping::NodeId& node_id, const sensor::PointCloud* const point_cloud,
|
const mapping::NodeId& node_id, const sensor::PointCloud* const point_cloud,
|
||||||
const transform::Rigid2d& initial_relative_pose) {
|
const transform::Rigid2d& initial_relative_pose) {
|
||||||
if (initial_relative_pose.translation().norm() >
|
if (initial_relative_pose.translation().norm() >
|
||||||
|
@ -74,7 +74,7 @@ void ConstraintBuilder::MaybeAddConstraint(
|
||||||
++pending_computations_[current_computation_];
|
++pending_computations_[current_computation_];
|
||||||
const int current_computation = current_computation_;
|
const int current_computation = current_computation_;
|
||||||
ScheduleSubmapScanMatcherConstructionAndQueueWorkItem(
|
ScheduleSubmapScanMatcherConstructionAndQueueWorkItem(
|
||||||
submap_id, submap->finished_probability_grid(), [=]() EXCLUDES(mutex_) {
|
submap_id, &submap->probability_grid(), [=]() EXCLUDES(mutex_) {
|
||||||
ComputeConstraint(submap_id, submap, node_id,
|
ComputeConstraint(submap_id, submap, node_id,
|
||||||
false, /* match_full_submap */
|
false, /* match_full_submap */
|
||||||
nullptr, /* trajectory_connectivity */
|
nullptr, /* trajectory_connectivity */
|
||||||
|
@ -85,7 +85,7 @@ void ConstraintBuilder::MaybeAddConstraint(
|
||||||
}
|
}
|
||||||
|
|
||||||
void ConstraintBuilder::MaybeAddGlobalConstraint(
|
void ConstraintBuilder::MaybeAddGlobalConstraint(
|
||||||
const mapping::SubmapId& submap_id, const mapping::Submap* const submap,
|
const mapping::SubmapId& submap_id, const Submap* const submap,
|
||||||
const mapping::NodeId& node_id, const sensor::PointCloud* const point_cloud,
|
const mapping::NodeId& node_id, const sensor::PointCloud* const point_cloud,
|
||||||
mapping::TrajectoryConnectivity* const trajectory_connectivity) {
|
mapping::TrajectoryConnectivity* const trajectory_connectivity) {
|
||||||
common::MutexLocker locker(&mutex_);
|
common::MutexLocker locker(&mutex_);
|
||||||
|
@ -94,7 +94,7 @@ void ConstraintBuilder::MaybeAddGlobalConstraint(
|
||||||
++pending_computations_[current_computation_];
|
++pending_computations_[current_computation_];
|
||||||
const int current_computation = current_computation_;
|
const int current_computation = current_computation_;
|
||||||
ScheduleSubmapScanMatcherConstructionAndQueueWorkItem(
|
ScheduleSubmapScanMatcherConstructionAndQueueWorkItem(
|
||||||
submap_id, submap->finished_probability_grid(), [=]() EXCLUDES(mutex_) {
|
submap_id, &submap->probability_grid(), [=]() EXCLUDES(mutex_) {
|
||||||
ComputeConstraint(submap_id, submap, node_id,
|
ComputeConstraint(submap_id, submap, node_id,
|
||||||
true, /* match_full_submap */
|
true, /* match_full_submap */
|
||||||
trajectory_connectivity, point_cloud,
|
trajectory_connectivity, point_cloud,
|
||||||
|
@ -159,7 +159,7 @@ ConstraintBuilder::GetSubmapScanMatcher(const mapping::SubmapId& submap_id) {
|
||||||
}
|
}
|
||||||
|
|
||||||
void ConstraintBuilder::ComputeConstraint(
|
void ConstraintBuilder::ComputeConstraint(
|
||||||
const mapping::SubmapId& submap_id, const mapping::Submap* const submap,
|
const mapping::SubmapId& submap_id, const Submap* const submap,
|
||||||
const mapping::NodeId& node_id, bool match_full_submap,
|
const mapping::NodeId& node_id, bool match_full_submap,
|
||||||
mapping::TrajectoryConnectivity* trajectory_connectivity,
|
mapping::TrajectoryConnectivity* trajectory_connectivity,
|
||||||
const sensor::PointCloud* const point_cloud,
|
const sensor::PointCloud* const point_cloud,
|
||||||
|
|
|
@ -35,7 +35,7 @@
|
||||||
#include "cartographer/mapping/trajectory_connectivity.h"
|
#include "cartographer/mapping/trajectory_connectivity.h"
|
||||||
#include "cartographer/mapping_2d/scan_matching/ceres_scan_matcher.h"
|
#include "cartographer/mapping_2d/scan_matching/ceres_scan_matcher.h"
|
||||||
#include "cartographer/mapping_2d/scan_matching/fast_correlative_scan_matcher.h"
|
#include "cartographer/mapping_2d/scan_matching/fast_correlative_scan_matcher.h"
|
||||||
|
#include "cartographer/mapping_2d/submaps.h"
|
||||||
#include "cartographer/mapping_3d/scan_matching/ceres_scan_matcher.h"
|
#include "cartographer/mapping_3d/scan_matching/ceres_scan_matcher.h"
|
||||||
#include "cartographer/mapping_3d/scan_matching/fast_correlative_scan_matcher.h"
|
#include "cartographer/mapping_3d/scan_matching/fast_correlative_scan_matcher.h"
|
||||||
#include "cartographer/sensor/point_cloud.h"
|
#include "cartographer/sensor/point_cloud.h"
|
||||||
|
@ -47,7 +47,7 @@ namespace sparse_pose_graph {
|
||||||
|
|
||||||
// Returns (map <- submap) where 'submap' is a coordinate system at the origin
|
// Returns (map <- submap) where 'submap' is a coordinate system at the origin
|
||||||
// of the Submap.
|
// of the Submap.
|
||||||
transform::Rigid2d ComputeSubmapPose(const mapping::Submap& submap);
|
transform::Rigid2d ComputeSubmapPose(const Submap& submap);
|
||||||
|
|
||||||
// Asynchronously computes constraints.
|
// Asynchronously computes constraints.
|
||||||
//
|
//
|
||||||
|
@ -78,8 +78,7 @@ class ConstraintBuilder {
|
||||||
// The pointees of 'submap' and 'point_cloud' must stay valid until all
|
// The pointees of 'submap' and 'point_cloud' must stay valid until all
|
||||||
// computations are finished.
|
// computations are finished.
|
||||||
void MaybeAddConstraint(const mapping::SubmapId& submap_id,
|
void MaybeAddConstraint(const mapping::SubmapId& submap_id,
|
||||||
const mapping::Submap* submap,
|
const Submap* submap, const mapping::NodeId& node_id,
|
||||||
const mapping::NodeId& node_id,
|
|
||||||
const sensor::PointCloud* point_cloud,
|
const sensor::PointCloud* point_cloud,
|
||||||
const transform::Rigid2d& initial_relative_pose);
|
const transform::Rigid2d& initial_relative_pose);
|
||||||
|
|
||||||
|
@ -92,7 +91,7 @@ class ConstraintBuilder {
|
||||||
// The pointees of 'submap' and 'point_cloud' must stay valid until all
|
// The pointees of 'submap' and 'point_cloud' must stay valid until all
|
||||||
// computations are finished.
|
// computations are finished.
|
||||||
void MaybeAddGlobalConstraint(
|
void MaybeAddGlobalConstraint(
|
||||||
const mapping::SubmapId& submap_id, const mapping::Submap* submap,
|
const mapping::SubmapId& submap_id, const Submap* submap,
|
||||||
const mapping::NodeId& node_id, const sensor::PointCloud* point_cloud,
|
const mapping::NodeId& node_id, const sensor::PointCloud* point_cloud,
|
||||||
mapping::TrajectoryConnectivity* trajectory_connectivity);
|
mapping::TrajectoryConnectivity* trajectory_connectivity);
|
||||||
|
|
||||||
|
@ -138,7 +137,7 @@ class ConstraintBuilder {
|
||||||
// 'trajectory_connectivity'.
|
// 'trajectory_connectivity'.
|
||||||
// As output, it may create a new Constraint in 'constraint'.
|
// As output, it may create a new Constraint in 'constraint'.
|
||||||
void ComputeConstraint(
|
void ComputeConstraint(
|
||||||
const mapping::SubmapId& submap_id, const mapping::Submap* submap,
|
const mapping::SubmapId& submap_id, const Submap* submap,
|
||||||
const mapping::NodeId& node_id, bool match_full_submap,
|
const mapping::NodeId& node_id, bool match_full_submap,
|
||||||
mapping::TrajectoryConnectivity* trajectory_connectivity,
|
mapping::TrajectoryConnectivity* trajectory_connectivity,
|
||||||
const sensor::PointCloud* point_cloud,
|
const sensor::PointCloud* point_cloud,
|
||||||
|
|
|
@ -149,9 +149,9 @@ class SparsePoseGraphTest : public ::testing::Test {
|
||||||
const sensor::PointCloud new_point_cloud = sensor::TransformPointCloud(
|
const sensor::PointCloud new_point_cloud = sensor::TransformPointCloud(
|
||||||
point_cloud_,
|
point_cloud_,
|
||||||
transform::Embed3D(current_pose_.inverse().cast<float>()));
|
transform::Embed3D(current_pose_.inverse().cast<float>()));
|
||||||
const mapping::Submap* const matching_submap =
|
const Submap* const matching_submap =
|
||||||
submaps_->Get(submaps_->matching_index());
|
submaps_->Get(submaps_->matching_index());
|
||||||
std::vector<const mapping::Submap*> insertion_submaps;
|
std::vector<const Submap*> insertion_submaps;
|
||||||
for (int insertion_index : submaps_->insertion_indices()) {
|
for (int insertion_index : submaps_->insertion_indices()) {
|
||||||
insertion_submaps.push_back(submaps_->Get(insertion_index));
|
insertion_submaps.push_back(submaps_->Get(insertion_index));
|
||||||
}
|
}
|
||||||
|
@ -161,9 +161,14 @@ class SparsePoseGraphTest : public ::testing::Test {
|
||||||
constexpr int kTrajectoryId = 0;
|
constexpr int kTrajectoryId = 0;
|
||||||
submaps_->InsertRangeData(TransformRangeData(
|
submaps_->InsertRangeData(TransformRangeData(
|
||||||
range_data, transform::Embed3D(pose_estimate.cast<float>())));
|
range_data, transform::Embed3D(pose_estimate.cast<float>())));
|
||||||
sparse_pose_graph_->AddScan(
|
|
||||||
common::FromUniversal(0), transform::Rigid3d::Identity(), range_data,
|
const Submap* const finished_submap = insertion_submaps.front()->finished()
|
||||||
pose_estimate, kTrajectoryId, matching_submap, insertion_submaps);
|
? insertion_submaps.front()
|
||||||
|
: nullptr;
|
||||||
|
sparse_pose_graph_->AddScan(common::FromUniversal(0),
|
||||||
|
transform::Rigid3d::Identity(), range_data,
|
||||||
|
pose_estimate, kTrajectoryId, matching_submap,
|
||||||
|
insertion_submaps, finished_submap);
|
||||||
}
|
}
|
||||||
|
|
||||||
void MoveRelative(const transform::Rigid2d& movement) {
|
void MoveRelative(const transform::Rigid2d& movement) {
|
||||||
|
|
|
@ -162,7 +162,7 @@ Submaps::Submaps(const proto::SubmapsOptions& options)
|
||||||
void Submaps::InsertRangeData(const sensor::RangeData& range_data) {
|
void Submaps::InsertRangeData(const sensor::RangeData& range_data) {
|
||||||
for (const int index : insertion_indices()) {
|
for (const int index : insertion_indices()) {
|
||||||
Submap* submap = submaps_[index].get();
|
Submap* submap = submaps_[index].get();
|
||||||
CHECK(submap->finished_probability_grid_ == nullptr);
|
CHECK(!submap->finished_);
|
||||||
range_data_inserter_.Insert(range_data, &submap->probability_grid_);
|
range_data_inserter_.Insert(range_data, &submap->probability_grid_);
|
||||||
++submap->num_range_data_;
|
++submap->num_range_data_;
|
||||||
}
|
}
|
||||||
|
@ -184,10 +184,10 @@ void Submaps::FinishSubmap(int index) {
|
||||||
// Crop the finished Submap before inserting a new Submap to reduce peak
|
// Crop the finished Submap before inserting a new Submap to reduce peak
|
||||||
// memory usage a bit.
|
// memory usage a bit.
|
||||||
Submap* submap = submaps_[index].get();
|
Submap* submap = submaps_[index].get();
|
||||||
CHECK(submap->finished_probability_grid_ == nullptr);
|
CHECK(!submap->finished_);
|
||||||
submap->probability_grid_ =
|
submap->probability_grid_ =
|
||||||
ComputeCroppedProbabilityGrid(submap->probability_grid_);
|
ComputeCroppedProbabilityGrid(submap->probability_grid_);
|
||||||
submap->finished_probability_grid_ = &submap->probability_grid_;
|
submap->finished_ = true;
|
||||||
if (options_.output_debug_images()) {
|
if (options_.output_debug_images()) {
|
||||||
// Output the Submap that won't be changed from now on.
|
// Output the Submap that won't be changed from now on.
|
||||||
WriteDebugImage("submap" + std::to_string(index) + ".webp",
|
WriteDebugImage("submap" + std::to_string(index) + ".webp",
|
||||||
|
|
|
@ -46,6 +46,7 @@ class Submap : public mapping::Submap {
|
||||||
Submap(const MapLimits& limits, const Eigen::Vector2f& origin);
|
Submap(const MapLimits& limits, const Eigen::Vector2f& origin);
|
||||||
|
|
||||||
const ProbabilityGrid& probability_grid() const { return probability_grid_; }
|
const ProbabilityGrid& probability_grid() const { return probability_grid_; }
|
||||||
|
const bool finished() const { return finished_; }
|
||||||
|
|
||||||
void ToResponseProto(
|
void ToResponseProto(
|
||||||
const transform::Rigid3d& global_submap_pose,
|
const transform::Rigid3d& global_submap_pose,
|
||||||
|
@ -56,6 +57,7 @@ class Submap : public mapping::Submap {
|
||||||
friend class Submaps;
|
friend class Submaps;
|
||||||
|
|
||||||
ProbabilityGrid probability_grid_;
|
ProbabilityGrid probability_grid_;
|
||||||
|
bool finished_ = false;
|
||||||
};
|
};
|
||||||
|
|
||||||
// A container of Submaps.
|
// A container of Submaps.
|
||||||
|
|
|
@ -55,15 +55,19 @@ void GlobalTrajectoryBuilder::AddRangefinderData(
|
||||||
const sensor::PointCloud& ranges) {
|
const sensor::PointCloud& ranges) {
|
||||||
auto insertion_result =
|
auto insertion_result =
|
||||||
local_trajectory_builder_->AddRangefinderData(time, origin, ranges);
|
local_trajectory_builder_->AddRangefinderData(time, origin, ranges);
|
||||||
|
|
||||||
if (insertion_result == nullptr) {
|
if (insertion_result == nullptr) {
|
||||||
return;
|
return;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
const Submap* const finished_submap =
|
||||||
|
insertion_result->insertion_submaps.front()->finished()
|
||||||
|
? insertion_result->insertion_submaps.front()
|
||||||
|
: nullptr;
|
||||||
sparse_pose_graph_->AddScan(
|
sparse_pose_graph_->AddScan(
|
||||||
insertion_result->time, insertion_result->range_data_in_tracking,
|
insertion_result->time, insertion_result->range_data_in_tracking,
|
||||||
insertion_result->pose_observation, trajectory_id_,
|
insertion_result->pose_observation, trajectory_id_,
|
||||||
insertion_result->matching_submap, insertion_result->insertion_submaps);
|
insertion_result->matching_submap, insertion_result->insertion_submaps,
|
||||||
|
finished_submap);
|
||||||
}
|
}
|
||||||
|
|
||||||
void GlobalTrajectoryBuilder::AddOdometerData(const common::Time time,
|
void GlobalTrajectoryBuilder::AddOdometerData(const common::Time time,
|
||||||
|
|
|
@ -90,7 +90,8 @@ void SparsePoseGraph::AddScan(
|
||||||
common::Time time, const sensor::RangeData& range_data_in_tracking,
|
common::Time time, const sensor::RangeData& range_data_in_tracking,
|
||||||
const transform::Rigid3d& pose, const int trajectory_id,
|
const transform::Rigid3d& pose, const int trajectory_id,
|
||||||
const Submap* const matching_submap,
|
const Submap* const matching_submap,
|
||||||
const std::vector<const Submap*>& insertion_submaps) {
|
const std::vector<const Submap*>& insertion_submaps,
|
||||||
|
const Submap* const finished_submap) {
|
||||||
const transform::Rigid3d optimized_pose(
|
const transform::Rigid3d optimized_pose(
|
||||||
GetLocalToGlobalTransform(trajectory_id) * pose);
|
GetLocalToGlobalTransform(trajectory_id) * pose);
|
||||||
|
|
||||||
|
@ -113,9 +114,6 @@ void SparsePoseGraph::AddScan(
|
||||||
submap_ids_.emplace(insertion_submaps.back(), submap_id);
|
submap_ids_.emplace(insertion_submaps.back(), submap_id);
|
||||||
submap_data_.at(submap_id).submap = insertion_submaps.back();
|
submap_data_.at(submap_id).submap = insertion_submaps.back();
|
||||||
}
|
}
|
||||||
const Submap* const finished_submap = insertion_submaps.front()->finished()
|
|
||||||
? insertion_submaps.front()
|
|
||||||
: nullptr;
|
|
||||||
|
|
||||||
// Make sure we have a sampler for this trajectory.
|
// Make sure we have a sampler for this trajectory.
|
||||||
if (!global_localization_samplers_[trajectory_id]) {
|
if (!global_localization_samplers_[trajectory_id]) {
|
||||||
|
|
|
@ -63,13 +63,15 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
|
||||||
// Adds a new 'range_data_in_tracking' observation at 'time', and a 'pose'
|
// Adds a new 'range_data_in_tracking' observation at 'time', and a 'pose'
|
||||||
// that will later be optimized. The 'pose' was determined by scan matching
|
// that will later be optimized. The 'pose' was determined by scan matching
|
||||||
// against the 'matching_submap' and the scan was inserted into the
|
// against the 'matching_submap' and the scan was inserted into the
|
||||||
// 'insertion_submaps'.
|
// 'insertion_submaps'. If not 'nullptr', 'finished_submap' is a freshly
|
||||||
|
// finished submap. It is also contained in 'insertion_submaps' for the last
|
||||||
|
// time.
|
||||||
void AddScan(common::Time time,
|
void AddScan(common::Time time,
|
||||||
const sensor::RangeData& range_data_in_tracking,
|
const sensor::RangeData& range_data_in_tracking,
|
||||||
const transform::Rigid3d& pose, int trajectory_id,
|
const transform::Rigid3d& pose, int trajectory_id,
|
||||||
const Submap* matching_submap,
|
const Submap* matching_submap,
|
||||||
const std::vector<const Submap*>& insertion_submaps)
|
const std::vector<const Submap*>& insertion_submaps,
|
||||||
EXCLUDES(mutex_);
|
const Submap* finished_submap) EXCLUDES(mutex_);
|
||||||
|
|
||||||
// Adds new IMU data to be used in the optimization.
|
// Adds new IMU data to be used in the optimization.
|
||||||
void AddImuData(int trajectory_id, common::Time time,
|
void AddImuData(int trajectory_id, common::Time time,
|
||||||
|
|
Loading…
Reference in New Issue