12345qiupeng
/
cartographer
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Use (trajectory_id, submap_index) as submap IDs in places. (#271) 

Replace flat submap index through (trajectory_id, submap_index) in Constraints.
Related to #256.

PAIR=wohe
master
Holger Rapp 2017-05-10 12:26:58 +02:00 committed by Wolfgang Hess
parent 45de59b116
commit 94e8eec41d
16 changed files with 209 additions and 208 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

24
cartographer/mapping/sparse_pose_graph.cc
View File

@ -53,22 +53,6 @@ void GroupTrajectoryNodes(
}
}
// TODO(macmason): This function is very nearly a copy of GroupTrajectoryNodes.
// Consider refactoring them to share an implementation.
void GroupSubmapStates(
const std::vector<SparsePoseGraph::SubmapState>& submap_states,
const std::unordered_map<const Submaps*, int>& trajectory_ids,
std::vector<std::pair<int, int>>* new_indices) {
CHECK_NOTNULL(new_indices)->clear();
std::vector<int> submap_group_sizes(trajectory_ids.size(), 0);
for (const auto& submap_state : submap_states) {
const int id = trajectory_ids.at(submap_state.trajectory);
new_indices->emplace_back(id, submap_group_sizes[id]);
submap_group_sizes[id]++;
}
}
proto::SparsePoseGraphOptions CreateSparsePoseGraphOptions(
common::LuaParameterDictionary* const parameter_dictionary) {
proto::SparsePoseGraphOptions options;
@ -96,10 +80,6 @@ proto::SparsePoseGraph SparsePoseGraph::ToProto() {
GroupTrajectoryNodes(GetTrajectoryNodes(), trajectory_ids(), &grouped_nodes,
&grouped_node_indices);
std::vector<std::pair<int, int>> grouped_submap_indices;
GroupSubmapStates(GetSubmapStates(), trajectory_ids(),
&grouped_submap_indices);
for (const auto& constraint : constraints()) {
auto* const constraint_proto = proto.add_constraint();
*constraint_proto->mutable_relative_pose() =
@ -113,9 +93,9 @@ proto::SparsePoseGraph SparsePoseGraph::ToProto() {
constraint.pose.sqrt_Lambda_ij;
constraint_proto->mutable_submap_id()->set_trajectory_id(
grouped_submap_indices[constraint.i].first);
constraint.i.trajectory_id);
constraint_proto->mutable_submap_id()->set_submap_index(
grouped_submap_indices[constraint.i].second);
constraint.i.submap_index);
constraint_proto->mutable_scan_id()->set_trajectory_id(
grouped_node_indices[constraint.j].first);

34
cartographer/mapping/sparse_pose_graph.h
View File

@ -58,8 +58,9 @@ class SparsePoseGraph {
Eigen::Matrix<double, 6, 6> sqrt_Lambda_ij;
};
// Submap index.
int i;
// TODO(hrapp): Rename to 'submap_id' and mention that the paper calls this
// 'i'.
mapping::SubmapId i;
// Scan index.
int j;
@ -73,24 +74,6 @@ class SparsePoseGraph {
enum Tag { INTRA_SUBMAP, INTER_SUBMAP } tag;
};
struct SubmapState {
const Submap* submap = nullptr;
// Indices of the scans that were inserted into this map together with
// constraints for them. They are not to be matched again when this submap
// becomes 'finished'.
std::set<int> scan_indices;
// Whether in the current state of the background thread this submap is
// finished. When this transitions to true, all scans are tried to match
// against this submap. Likewise, all new scans are matched against submaps
// which are finished.
bool finished = false;
// The trajectory to which this SubmapState belongs.
const Submaps* trajectory = nullptr;
};
SparsePoseGraph() {}
virtual ~SparsePoseGraph() {}
@ -121,10 +104,6 @@ class SparsePoseGraph {
proto::SparsePoseGraph ToProto();
protected:
// TODO(macmason, wohe): Consider replacing this with a GroupSubmapStates,
// which would have better separation of concerns.
virtual std::vector<SubmapState> GetSubmapStates() = 0;
// Returns the collection of constraints.
virtual std::vector<Constraint> constraints() = 0;
@ -132,13 +111,6 @@ class SparsePoseGraph {
virtual const std::unordered_map<const Submaps*, int>& trajectory_ids() = 0;
};
// Like TrajectoryNodes, SubmapStates arrive in a flat vector, but need to be
// grouped by trajectory. The arguments are just as in 'GroupTrajectoryNodes'.
void GroupSubmapStates(
const std::vector<SparsePoseGraph::SubmapState>& submap_states,
const std::unordered_map<const Submaps*, int>& trajectory_ids,
std::vector<std::pair<int, int>>* new_indices);
} // namespace mapping
} // namespace cartographer

16
cartographer/mapping/submaps.h
View File

@ -63,6 +63,22 @@ inline uint8 ProbabilityToLogOddsInteger(const float probability) {
return value;
}
// Uniquely identifies a submap using a combination of a unique trajectory ID
// and a zero-based index of the submap inside that trajectory.
struct SubmapId {
int trajectory_id;
int submap_index;
bool operator<(const SubmapId& other) const {
return std::forward_as_tuple(trajectory_id, submap_index) <
std::forward_as_tuple(other.trajectory_id, other.submap_index);
}
};
inline std::ostream& operator<<(std::ostream& os, const SubmapId& v) {
return os << "(" << v.trajectory_id << ", " << v.submap_index << ")";
}
// An individual submap, which has an initial position 'origin', keeps track of
// how many range data were inserted into it, and sets the
// 'finished_probability_grid' to be used for loop closing once the map no

21
cartographer/mapping_2d/sparse_pose_graph.cc
View File

@ -64,7 +64,7 @@ void SparsePoseGraph::GrowSubmapTransformsAsNeeded(
// If we don't already have an entry for this submap, add one.
if (first_submap_index == next_submap_index) {
optimization_problem_.AddSubmap(
submap_states_[first_submap_index].trajectory,
submap_states_[first_submap_index].id.trajectory_id,
transform::Rigid2d::Identity());
}
return;
@ -78,7 +78,7 @@ void SparsePoseGraph::GrowSubmapTransformsAsNeeded(
const auto& first_submap_pose =
optimization_problem_.submap_data().at(first_submap_index).pose;
optimization_problem_.AddSubmap(
submap_states_[second_submap_index].trajectory,
submap_states_[second_submap_index].id.trajectory_id,
first_submap_pose *
sparse_pose_graph::ComputeSubmapPose(*insertion_submaps[0])
.inverse() *
@ -116,6 +116,9 @@ void SparsePoseGraph::AddScan(
submap_states_.emplace_back();
submap_states_.back().submap = insertion_submaps.back();
submap_states_.back().trajectory = trajectory;
submap_states_.back().id = mapping::SubmapId{
trajectory_ids_.at(trajectory), num_submaps_in_trajectory_[trajectory]};
++num_submaps_in_trajectory_[trajectory];
CHECK_EQ(submap_states_.size(), submap_indices_.size());
}
const mapping::Submap* const finished_submap =
@ -165,11 +168,14 @@ void SparsePoseGraph::ComputeConstraint(const int scan_index,
trajectory_nodes_[scan_index].constant_data->trajectory;
const mapping::Submaps* const submap_trajectory =
submap_states_[submap_index].trajectory;
const mapping::SubmapId submap_id = submap_states_[submap_index].id;
// Only globally match against submaps not in this trajectory.
if (scan_trajectory != submap_trajectory &&
global_localization_samplers_[scan_trajectory]->Pulse()) {
constraint_builder_.MaybeAddGlobalConstraint(
submap_index, submap_states_[submap_index].submap, scan_index,
submap_id, submap_states_[submap_index].submap, scan_index,
scan_trajectory, submap_trajectory, &trajectory_connectivity_,
&trajectory_nodes_[scan_index].constant_data->range_data_2d.returns);
} else {
@ -181,7 +187,7 @@ void SparsePoseGraph::ComputeConstraint(const int scan_index,
if (scan_trajectory == submap_trajectory ||
scan_and_submap_trajectories_connected) {
constraint_builder_.MaybeAddConstraint(
submap_index, submap_states_[submap_index].submap, scan_index,
submap_id, submap_states_[submap_index].submap, scan_index,
&trajectory_nodes_[scan_index].constant_data->range_data_2d.returns,
relative_pose);
}
@ -227,7 +233,7 @@ void SparsePoseGraph::ComputeConstraintsForScan(
constexpr double kFakePositionCovariance = 1e-6;
constexpr double kFakeOrientationCovariance = 1e-6;
constraints_.push_back(Constraint{
submap_index,
submap_states_[submap_index].id,
scan_index,
{transform::Embed3D(constraint_transform),
common::ComputeSpdMatrixSqrtInverse(
@ -387,11 +393,6 @@ std::vector<mapping::TrajectoryNode> SparsePoseGraph::GetTrajectoryNodes() {
return trajectory_nodes_;
}
std::vector<SparsePoseGraph::SubmapState> SparsePoseGraph::GetSubmapStates() {
common::MutexLocker locker(&mutex_);
return submap_states_;
}
std::vector<SparsePoseGraph::Constraint> SparsePoseGraph::constraints() {
common::MutexLocker locker(&mutex_);
return constraints_;

23
cartographer/mapping_2d/sparse_pose_graph.h
View File

@ -93,12 +93,31 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
EXCLUDES(mutex_);
protected:
std::vector<SubmapState> GetSubmapStates() override EXCLUDES(mutex_);
std::vector<Constraint> constraints() override EXCLUDES(mutex_);
const std::unordered_map<const mapping::Submaps*, int>& trajectory_ids()
override EXCLUDES(mutex_);
private:
struct SubmapState {
const mapping::Submap* submap = nullptr;
// Indices of the scans that were inserted into this map together with
// constraints for them. They are not to be matched again when this submap
// becomes 'finished'.
std::set<int> scan_indices;
// Whether in the current state of the background thread this submap is
// finished. When this transitions to true, all scans are tried to match
// against this submap. Likewise, all new scans are matched against submaps
// which are finished.
bool finished = false;
// The trajectory to which this SubmapState belongs.
const mapping::Submaps* trajectory = nullptr;
mapping::SubmapId id;
};
// Handles a new work item.
void AddWorkItem(std::function<void()> work_item) REQUIRES(mutex_);
@ -177,6 +196,8 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
// before they take part in the background computations.
std::map<const mapping::Submap*, int> submap_indices_ GUARDED_BY(mutex_);
std::vector<SubmapState> submap_states_ GUARDED_BY(mutex_);
std::map<const mapping::Submaps*, int> num_submaps_in_trajectory_
GUARDED_BY(mutex_);
// Connectivity structure of our trajectories.
std::vector<std::vector<const mapping::Submaps*>> connected_components_;

45
cartographer/mapping_2d/sparse_pose_graph/constraint_builder.cc
View File

@ -61,7 +61,7 @@ ConstraintBuilder::~ConstraintBuilder() {
}
void ConstraintBuilder::MaybeAddConstraint(
const int submap_index, const mapping::Submap* const submap,
const mapping::SubmapId& submap_id, const mapping::Submap* const submap,
const int scan_index, const sensor::PointCloud* const point_cloud,
const transform::Rigid2d& initial_relative_pose) {
if (initial_relative_pose.translation().norm() >
@ -76,9 +76,8 @@ void ConstraintBuilder::MaybeAddConstraint(
++pending_computations_[current_computation_];
const int current_computation = current_computation_;
ScheduleSubmapScanMatcherConstructionAndQueueWorkItem(
submap_index, submap->finished_probability_grid,
[=]() EXCLUDES(mutex_) {
ComputeConstraint(submap_index, submap, scan_index,
submap_id, submap->finished_probability_grid, [=]() EXCLUDES(mutex_) {
ComputeConstraint(submap_id, submap, scan_index,
nullptr, /* scan_trajectory */
nullptr, /* submap_trajectory */
false, /* match_full_submap */
@ -90,7 +89,7 @@ void ConstraintBuilder::MaybeAddConstraint(
}
void ConstraintBuilder::MaybeAddGlobalConstraint(
const int submap_index, const mapping::Submap* const submap,
const mapping::SubmapId& submap_id, const mapping::Submap* const submap,
const int scan_index, const mapping::Submaps* scan_trajectory,
const mapping::Submaps* submap_trajectory,
mapping::TrajectoryConnectivity* trajectory_connectivity,
@ -102,8 +101,8 @@ void ConstraintBuilder::MaybeAddGlobalConstraint(
++pending_computations_[current_computation_];
const int current_computation = current_computation_;
ScheduleSubmapScanMatcherConstructionAndQueueWorkItem(
submap_index, submap->finished_probability_grid, [=]() EXCLUDES(mutex_) {
ComputeConstraint(submap_index, submap, scan_index, submap_trajectory,
submap_id, submap->finished_probability_grid, [=]() EXCLUDES(mutex_) {
ComputeConstraint(submap_id, submap, scan_index, submap_trajectory,
scan_trajectory, true, /* match_full_submap */
trajectory_connectivity, point_cloud,
transform::Rigid2d::Identity(), constraint);
@ -130,47 +129,45 @@ void ConstraintBuilder::WhenDone(
}
void ConstraintBuilder::ScheduleSubmapScanMatcherConstructionAndQueueWorkItem(
const int submap_index, const ProbabilityGrid* const submap,
const mapping::SubmapId& submap_id, const ProbabilityGrid* const submap,
const std::function<void()> work_item) {
if (submap_scan_matchers_[submap_index].fast_correlative_scan_matcher !=
if (submap_scan_matchers_[submap_id].fast_correlative_scan_matcher !=
nullptr) {
thread_pool_->Schedule(work_item);
} else {
submap_queued_work_items_[submap_index].push_back(work_item);
if (submap_queued_work_items_[submap_index].size() == 1) {
submap_queued_work_items_[submap_id].push_back(work_item);
if (submap_queued_work_items_[submap_id].size() == 1) {
thread_pool_->Schedule(
std::bind(std::mem_fn(&ConstraintBuilder::ConstructSubmapScanMatcher),
this, submap_index, submap));
[=]() { ConstructSubmapScanMatcher(submap_id, submap); });
}
}
}
void ConstraintBuilder::ConstructSubmapScanMatcher(
const int submap_index, const ProbabilityGrid* const submap) {
const mapping::SubmapId& submap_id, const ProbabilityGrid* const submap) {
auto submap_scan_matcher =
common::make_unique<scan_matching::FastCorrelativeScanMatcher>(
*submap, options_.fast_correlative_scan_matcher_options());
common::MutexLocker locker(&mutex_);
submap_scan_matchers_[submap_index] = {submap,
std::move(submap_scan_matcher)};
submap_scan_matchers_[submap_id] = {submap, std::move(submap_scan_matcher)};
for (const std::function<void()>& work_item :
submap_queued_work_items_[submap_index]) {
submap_queued_work_items_[submap_id]) {
thread_pool_->Schedule(work_item);
}
submap_queued_work_items_.erase(submap_index);
submap_queued_work_items_.erase(submap_id);
}
const ConstraintBuilder::SubmapScanMatcher*
ConstraintBuilder::GetSubmapScanMatcher(const int submap_index) {
ConstraintBuilder::GetSubmapScanMatcher(const mapping::SubmapId& submap_id) {
common::MutexLocker locker(&mutex_);
const SubmapScanMatcher* submap_scan_matcher =
&submap_scan_matchers_[submap_index];
&submap_scan_matchers_[submap_id];
CHECK(submap_scan_matcher->fast_correlative_scan_matcher != nullptr);
return submap_scan_matcher;
}
void ConstraintBuilder::ComputeConstraint(
const int submap_index, const mapping::Submap* const submap,
const mapping::SubmapId& submap_id, const mapping::Submap* const submap,
const int scan_index, const mapping::Submaps* scan_trajectory,
const mapping::Submaps* submap_trajectory, bool match_full_submap,
mapping::TrajectoryConnectivity* trajectory_connectivity,
@ -180,7 +177,7 @@ void ConstraintBuilder::ComputeConstraint(
const transform::Rigid2d initial_pose =
ComputeSubmapPose(*submap) * initial_relative_pose;
const SubmapScanMatcher* const submap_scan_matcher =
GetSubmapScanMatcher(submap_index);
GetSubmapScanMatcher(submap_id);
const sensor::PointCloud filtered_point_cloud =
adaptive_voxel_filter_.Filter(*point_cloud);
@ -231,7 +228,7 @@ void ConstraintBuilder::ComputeConstraint(
constexpr double kFakePositionCovariance = 1e-6;
constexpr double kFakeOrientationCovariance = 1e-6;
constraint->reset(new Constraint{
submap_index,
submap_id,
scan_index,
{transform::Embed3D(constraint_transform),
common::ComputeSpdMatrixSqrtInverse(
@ -245,7 +242,7 @@ void ConstraintBuilder::ComputeConstraint(
initial_pose.inverse() * pose_estimate;
std::ostringstream info;
info << "Scan index " << scan_index << " with "
<< filtered_point_cloud.size() << " points on submap " << submap_index
<< filtered_point_cloud.size() << " points on submap " << submap_id
<< " differs by translation " << std::fixed << std::setprecision(2)
<< difference.translation().norm() << " rotation "
<< std::setprecision(3) << std::abs(difference.normalized_angle())

38
cartographer/mapping_2d/sparse_pose_graph/constraint_builder.h
View File

@ -72,17 +72,18 @@ class ConstraintBuilder {
ConstraintBuilder& operator=(const ConstraintBuilder&) = delete;
// Schedules exploring a new constraint between 'submap' identified by
// 'submap_index', and the 'point_cloud' for 'scan_index'.
// 'submap_id', and the 'point_cloud' for 'scan_index'.
// The 'initial_pose' is relative to the 'submap'.
//
// The pointees of 'submap' and 'point_cloud' must stay valid until all
// computations are finished.
void MaybeAddConstraint(int submap_index, const mapping::Submap* submap,
int scan_index, const sensor::PointCloud* point_cloud,
void MaybeAddConstraint(const mapping::SubmapId& submap_id,
const mapping::Submap* submap, int scan_index,
const sensor::PointCloud* point_cloud,
const transform::Rigid2d& initial_relative_pose);
// Schedules exploring a new constraint between 'submap' identified by
// 'submap_index' and the 'point_cloud' for 'scan_index'. This performs
// 'submap_id' and the 'point_cloud' for 'scan_index'. This performs
// full-submap matching.
//
// The scan at 'scan_index' should be from trajectory 'scan_trajectory', and
@ -92,8 +93,8 @@ class ConstraintBuilder {
// The pointees of 'submap' and 'point_cloud' must stay valid until all
// computations are finished.
void MaybeAddGlobalConstraint(
int submap_index, const mapping::Submap* submap, int scan_index,
const mapping::Submaps* scan_trajectory,
const mapping::SubmapId& submap_id, const mapping::Submap* submap,
int scan_index, const mapping::Submaps* scan_trajectory,
const mapping::Submaps* submap_trajectory,
mapping::TrajectoryConnectivity* trajectory_connectivity,
const sensor::PointCloud* point_cloud);
@ -118,17 +119,17 @@ class ConstraintBuilder {
// Either schedules the 'work_item', or if needed, schedules the scan matcher
// construction and queues the 'work_item'.
void ScheduleSubmapScanMatcherConstructionAndQueueWorkItem(
int submap_index, const ProbabilityGrid* submap,
const mapping::SubmapId& submap_id, const ProbabilityGrid* submap,
std::function<void()> work_item) REQUIRES(mutex_);
// Constructs the scan matcher for a 'submap', then schedules its work items.
void ConstructSubmapScanMatcher(int submap_index,
void ConstructSubmapScanMatcher(const mapping::SubmapId& submap_id,
const ProbabilityGrid* submap)
EXCLUDES(mutex_);
// Returns the scan matcher for a submap, which has to exist.
const SubmapScanMatcher* GetSubmapScanMatcher(int submap_index)
EXCLUDES(mutex_);
const SubmapScanMatcher* GetSubmapScanMatcher(
const mapping::SubmapId& submap_id) EXCLUDES(mutex_);
// Runs in a background thread and does computations for an additional
// constraint, assuming 'submap' and 'point_cloud' do not change anymore.
@ -137,8 +138,8 @@ class ConstraintBuilder {
// 'trajectory_connectivity'.
// As output, it may create a new Constraint in 'constraint'.
void ComputeConstraint(
int submap_index, const mapping::Submap* submap, int scan_index,
const mapping::Submaps* scan_trajectory,
const mapping::SubmapId& submap_id, const mapping::Submap* submap,
int scan_index, const mapping::Submaps* scan_trajectory,
const mapping::Submaps* submap_trajectory, bool match_full_submap,
mapping::TrajectoryConnectivity* trajectory_connectivity,
const sensor::PointCloud* point_cloud,
@ -170,14 +171,15 @@ class ConstraintBuilder {
// keep pointers valid when adding more entries.
std::deque<std::unique_ptr<Constraint>> constraints_ GUARDED_BY(mutex_);
// Map of already constructed scan matchers by 'submap_index'.
std::map<int, SubmapScanMatcher> submap_scan_matchers_ GUARDED_BY(mutex_);
// Map by 'submap_index' of scan matchers under construction, and the work
// to do once construction is done.
std::map<int, std::vector<std::function<void()>>> submap_queued_work_items_
// Map of already constructed scan matchers by 'submap_id'.
std::map<mapping::SubmapId, SubmapScanMatcher> submap_scan_matchers_
GUARDED_BY(mutex_);
// Map by 'submap_id' of scan matchers under construction, and the work
// to do once construction is done.
std::map<mapping::SubmapId, std::vector<std::function<void()>>>
submap_queued_work_items_ GUARDED_BY(mutex_);
common::FixedRatioSampler sampler_;
const sensor::AdaptiveVoxelFilter adaptive_voxel_filter_;
scan_matching::CeresScanMatcher ceres_scan_matcher_;

33
cartographer/mapping_2d/sparse_pose_graph/optimization_problem.cc
View File

@ -75,9 +75,9 @@ void OptimizationProblem::AddTrajectoryNode(
NodeData{trajectory, time, initial_point_cloud_pose, point_cloud_pose});
}
void OptimizationProblem::AddSubmap(const mapping::Submaps* const trajectory,
void OptimizationProblem::AddSubmap(const int trajectory_id,
const transform::Rigid2d& submap_pose) {
submap_data_.push_back(SubmapData{trajectory, submap_pose});
submap_data_.push_back(SubmapData{trajectory_id, submap_pose});
}
void OptimizationProblem::SetMaxNumIterations(const int32 max_num_iterations) {
@ -95,11 +95,18 @@ void OptimizationProblem::Solve(const std::vector<Constraint>& constraints) {
ceres::Problem problem(problem_options);
// Set the starting point.
std::vector<std::array<double, 3>> C_submaps(submap_data_.size());
// TODO(hrapp): Move ceres data into SubmapData.
std::deque<std::deque<std::array<double, 3>>> C_submaps;
std::vector<std::array<double, 3>> C_point_clouds(node_data_.size());
for (size_t i = 0; i != submap_data_.size(); ++i) {
C_submaps[i] = FromPose(submap_data_[i].pose);
problem.AddParameterBlock(C_submaps[i].data(), 3);
while (static_cast<int>(C_submaps.size()) <=
submap_data_[i].trajectory_id) {
C_submaps.emplace_back();
}
C_submaps[submap_data_[i].trajectory_id].push_back(
FromPose(submap_data_[i].pose));
problem.AddParameterBlock(
C_submaps[submap_data_[i].trajectory_id].back().data(), 3);
}
for (size_t j = 0; j != node_data_.size(); ++j) {
C_point_clouds[j] = FromPose(node_data_[j].point_cloud_pose);
@ -107,12 +114,11 @@ void OptimizationProblem::Solve(const std::vector<Constraint>& constraints) {
}
// Fix the pose of the first submap.
problem.SetParameterBlockConstant(C_submaps[0].data());
problem.SetParameterBlockConstant(
C_submaps[submap_data_[0].trajectory_id].front().data());
// Add cost functions for intra- and inter-submap constraints.
for (const Constraint& constraint : constraints) {
CHECK_GE(constraint.i, 0);
CHECK_LT(constraint.i, submap_data_.size());
CHECK_GE(constraint.j, 0);
CHECK_LT(constraint.j, node_data_.size());
problem.AddResidualBlock(
@ -122,7 +128,10 @@ void OptimizationProblem::Solve(const std::vector<Constraint>& constraints) {
constraint.tag == Constraint::INTER_SUBMAP
? new ceres::HuberLoss(options_.huber_scale())
: nullptr,
C_submaps[constraint.i].data(), C_point_clouds[constraint.j].data());
C_submaps.at(constraint.i.trajectory_id)
.at(constraint.i.submap_index)
.data(),
C_point_clouds[constraint.j].data());
}
// Add penalties for changes between consecutive scans.
@ -170,9 +179,11 @@ void OptimizationProblem::Solve(const std::vector<Constraint>& constraints) {
}
// Store the result.
for (size_t i = 0; i != submap_data_.size(); ++i) {
submap_data_[i].pose = ToPose(C_submaps[i]);
for (auto& submap_data : submap_data_) {
submap_data.pose = ToPose(C_submaps[submap_data.trajectory_id].front());
C_submaps[submap_data.trajectory_id].pop_front();
}
for (size_t j = 0; j != node_data_.size(); ++j) {
node_data_[j].point_cloud_pose = ToPose(C_point_clouds[j]);
}

5
cartographer/mapping_2d/sparse_pose_graph/optimization_problem.h
View File

@ -44,7 +44,7 @@ struct NodeData {
struct SubmapData {
// TODO(whess): Keep nodes per trajectory instead.
const mapping::Submaps* trajectory;
const int trajectory_id;
transform::Rigid2d pose;
};
@ -67,8 +67,7 @@ class OptimizationProblem {
void AddTrajectoryNode(const mapping::Submaps* trajectory, common::Time time,
const transform::Rigid2d& initial_point_cloud_pose,
const transform::Rigid2d& point_cloud_pose);
void AddSubmap(const mapping::Submaps* trajectory,
const transform::Rigid2d& submap_pose);
void AddSubmap(int trajectory_id, const transform::Rigid2d& submap_pose);
void SetMaxNumIterations(int32 max_num_iterations);

32
cartographer/mapping_3d/sparse_pose_graph.cc
View File

@ -65,7 +65,7 @@ void SparsePoseGraph::GrowSubmapTransformsAsNeeded(
// If we don't already have an entry for this submap, add one.
if (first_submap_index == next_submap_index) {
optimization_problem_.AddSubmap(
submap_states_[first_submap_index].trajectory,
submap_states_[first_submap_index].id.trajectory_id,
transform::Rigid3d::Identity());
}
return;
@ -79,7 +79,7 @@ void SparsePoseGraph::GrowSubmapTransformsAsNeeded(
const auto& first_submap_pose =
optimization_problem_.submap_data().at(first_submap_index).pose;
optimization_problem_.AddSubmap(
submap_states_[second_submap_index].trajectory,
submap_states_[second_submap_index].id.trajectory_id,
first_submap_pose * insertion_submaps[0]->local_pose().inverse() *
insertion_submaps[1]->local_pose());
}
@ -95,6 +95,7 @@ void SparsePoseGraph::AddScan(
GetLocalToGlobalTransform(trajectory) * pose);
common::MutexLocker locker(&mutex_);
trajectory_ids_.emplace(trajectory, trajectory_ids_.size());
const int j = trajectory_nodes_.size();
CHECK_LT(j, std::numeric_limits<int>::max());
@ -112,6 +113,9 @@ void SparsePoseGraph::AddScan(
submap_states_.emplace_back();
submap_states_.back().submap = insertion_submaps.back();
submap_states_.back().trajectory = trajectory;
submap_states_.back().id = mapping::SubmapId{
trajectory_ids_.at(trajectory), num_submaps_in_trajectory_[trajectory]};
++num_submaps_in_trajectory_[trajectory];
CHECK_EQ(submap_states_.size(), submap_indices_.size());
}
const Submap* const finished_submap =
@ -164,11 +168,14 @@ void SparsePoseGraph::ComputeConstraint(const int scan_index,
trajectory_nodes_[scan_index].constant_data->trajectory;
const mapping::Submaps* const submap_trajectory =
submap_states_[submap_index].trajectory;
const mapping::SubmapId submap_id = submap_states_[submap_index].id;
// Only globally match against submaps not in this trajectory.
if (scan_trajectory != submap_trajectory &&
global_localization_samplers_[scan_trajectory]->Pulse()) {
constraint_builder_.MaybeAddGlobalConstraint(
submap_index, submap_states_[submap_index].submap, scan_index,
submap_id, submap_states_[submap_index].submap, scan_index,
scan_trajectory, submap_trajectory, &trajectory_connectivity_,
trajectory_nodes_);
} else {
@ -180,7 +187,7 @@ void SparsePoseGraph::ComputeConstraint(const int scan_index,
if (scan_trajectory == submap_trajectory ||
scan_and_submap_trajectories_connected) {
constraint_builder_.MaybeAddConstraint(
submap_index, submap_states_[submap_index].submap, scan_index,
submap_id, submap_states_[submap_index].submap, scan_index,
trajectory_nodes_, relative_pose);
}
}
@ -222,7 +229,7 @@ void SparsePoseGraph::ComputeConstraintsForScan(
const transform::Rigid3d constraint_transform =
submap->local_pose().inverse() * pose;
constraints_.push_back(
Constraint{submap_index,
Constraint{submap_states_[submap_index].id,
scan_index,
{constraint_transform,
common::ComputeSpdMatrixSqrtInverse(
@ -379,21 +386,6 @@ std::vector<mapping::TrajectoryNode> SparsePoseGraph::GetTrajectoryNodes() {
return trajectory_nodes_;
}
std::vector<mapping::SparsePoseGraph::SubmapState>
SparsePoseGraph::GetSubmapStates() {
std::vector<mapping::SparsePoseGraph::SubmapState> result;
common::MutexLocker locker(&mutex_);
for (const auto& submap_state : submap_states_) {
mapping::SparsePoseGraph::SubmapState entry;
entry.submap = submap_state.submap;
entry.scan_indices = submap_state.scan_indices;
entry.finished = submap_state.finished;
entry.trajectory = submap_state.trajectory;
result.push_back(entry);
}
return result;
}
std::vector<SparsePoseGraph::Constraint> SparsePoseGraph::constraints() {
common::MutexLocker locker(&mutex_);
return constraints_;

11
cartographer/mapping_3d/sparse_pose_graph.h
View File

@ -95,14 +95,11 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
EXCLUDES(mutex_);
protected:
std::vector<SubmapState> GetSubmapStates() EXCLUDES(mutex_) override;
std::vector<Constraint> constraints() EXCLUDES(mutex_) override;
std::vector<Constraint> constraints() override EXCLUDES(mutex_);
const std::unordered_map<const mapping::Submaps*, int>& trajectory_ids()
EXCLUDES(mutex_) override;
override EXCLUDES(mutex_);
private:
// This is 'mapping::SubmapState', but with the 3D versions of 'submap' and
// 'trajectory'.
struct SubmapState {
const Submap* submap = nullptr;
@ -119,6 +116,8 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
// The trajectory to which this SubmapState belongs.
const Submaps* trajectory = nullptr;
mapping::SubmapId id;
};
// Handles a new work item.
@ -197,6 +196,8 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
// before they take part in the background computations.
std::map<const mapping::Submap*, int> submap_indices_ GUARDED_BY(mutex_);
std::vector<SubmapState> submap_states_ GUARDED_BY(mutex_);
std::map<const mapping::Submaps*, int> num_submaps_in_trajectory_
GUARDED_BY(mutex_);
// Connectivity structure of our trajectories.
std::vector<std::vector<const mapping::Submaps*>> connected_components_;

54
cartographer/mapping_3d/sparse_pose_graph/constraint_builder.cc
View File

@ -71,13 +71,14 @@ ConstraintBuilder::ConstraintBuilder(
ConstraintBuilder::~ConstraintBuilder() {
common::MutexLocker locker(&mutex_);
CHECK_EQ(constraints_.size(), 0) << "WhenDone() was not called";
CHECK(pending_computations_.empty());
CHECK_EQ(pending_computations_.size(), 0);
CHECK_EQ(submap_queued_work_items_.size(), 0);
CHECK(when_done_ == nullptr);
}
void ConstraintBuilder::MaybeAddConstraint(
const int submap_index, const Submap* const submap, const int scan_index,
const mapping::SubmapId& submap_id, const Submap* const submap,
const int scan_index,
const std::vector<mapping::TrajectoryNode>& trajectory_nodes,
const transform::Rigid3d& initial_relative_pose) {
if (initial_relative_pose.translation().norm() >
@ -96,9 +97,9 @@ void ConstraintBuilder::MaybeAddConstraint(
const auto* const point_cloud =
&trajectory_nodes[scan_index].constant_data->range_data_3d.returns;
ScheduleSubmapScanMatcherConstructionAndQueueWorkItem(
submap_index, submap_nodes, &submap->high_resolution_hybrid_grid,
submap_id, submap_nodes, &submap->high_resolution_hybrid_grid,
[=]() EXCLUDES(mutex_) {
ComputeConstraint(submap_index, submap, scan_index,
ComputeConstraint(submap_id, submap, scan_index,
nullptr, /* scan_trajectory */
nullptr, /* submap_trajectory */
false, /* match_full_submap */
@ -110,8 +111,8 @@ void ConstraintBuilder::MaybeAddConstraint(
}
void ConstraintBuilder::MaybeAddGlobalConstraint(
const int submap_index, const Submap* const submap, const int scan_index,
const mapping::Submaps* scan_trajectory,
const mapping::SubmapId& submap_id, const Submap* const submap,
const int scan_index, const mapping::Submaps* scan_trajectory,
const mapping::Submaps* submap_trajectory,
mapping::TrajectoryConnectivity* trajectory_connectivity,
const std::vector<mapping::TrajectoryNode>& trajectory_nodes) {
@ -126,9 +127,9 @@ void ConstraintBuilder::MaybeAddGlobalConstraint(
const auto* const point_cloud =
&trajectory_nodes[scan_index].constant_data->range_data_3d.returns;
ScheduleSubmapScanMatcherConstructionAndQueueWorkItem(
submap_index, submap_nodes, &submap->high_resolution_hybrid_grid,
submap_id, submap_nodes, &submap->high_resolution_hybrid_grid,
[=]() EXCLUDES(mutex_) {
ComputeConstraint(submap_index, submap, scan_index, submap_trajectory,
ComputeConstraint(submap_id, submap, scan_index, submap_trajectory,
scan_trajectory, true, /* match_full_submap */
trajectory_connectivity, point_cloud,
transform::Rigid3d::Identity(), constraint);
@ -155,24 +156,24 @@ void ConstraintBuilder::WhenDone(
}
void ConstraintBuilder::ScheduleSubmapScanMatcherConstructionAndQueueWorkItem(
const int submap_index,
const mapping::SubmapId& submap_id,
const std::vector<mapping::TrajectoryNode>& submap_nodes,
const HybridGrid* const submap, const std::function<void()> work_item) {
if (submap_scan_matchers_[submap_index].fast_correlative_scan_matcher !=
if (submap_scan_matchers_[submap_id].fast_correlative_scan_matcher !=
nullptr) {
thread_pool_->Schedule(work_item);
} else {
submap_queued_work_items_[submap_index].push_back(work_item);
if (submap_queued_work_items_[submap_index].size() == 1) {
thread_pool_->Schedule(
std::bind(std::mem_fn(&ConstraintBuilder::ConstructSubmapScanMatcher),
this, submap_index, submap_nodes, submap));
submap_queued_work_items_[submap_id].push_back(work_item);
if (submap_queued_work_items_[submap_id].size() == 1) {
thread_pool_->Schedule([=]() {
ConstructSubmapScanMatcher(submap_id, submap_nodes, submap);
});
}
}
}
void ConstraintBuilder::ConstructSubmapScanMatcher(
const int submap_index,
const mapping::SubmapId& submap_id,
const std::vector<mapping::TrajectoryNode>& submap_nodes,
const HybridGrid* const submap) {
auto submap_scan_matcher =
@ -180,27 +181,26 @@ void ConstraintBuilder::ConstructSubmapScanMatcher(
*submap, submap_nodes,
options_.fast_correlative_scan_matcher_options_3d());
common::MutexLocker locker(&mutex_);
submap_scan_matchers_[submap_index] = {submap,
std::move(submap_scan_matcher)};
submap_scan_matchers_[submap_id] = {submap, std::move(submap_scan_matcher)};
for (const std::function<void()>& work_item :
submap_queued_work_items_[submap_index]) {
submap_queued_work_items_[submap_id]) {
thread_pool_->Schedule(work_item);
}
submap_queued_work_items_.erase(submap_index);
submap_queued_work_items_.erase(submap_id);
}
const ConstraintBuilder::SubmapScanMatcher*
ConstraintBuilder::GetSubmapScanMatcher(const int submap_index) {
ConstraintBuilder::GetSubmapScanMatcher(const mapping::SubmapId& submap_id) {
common::MutexLocker locker(&mutex_);
const SubmapScanMatcher* submap_scan_matcher =
&submap_scan_matchers_[submap_index];
&submap_scan_matchers_[submap_id];
CHECK(submap_scan_matcher->fast_correlative_scan_matcher != nullptr);
return submap_scan_matcher;
}
void ConstraintBuilder::ComputeConstraint(
const int submap_index, const Submap* const submap, const int scan_index,
const mapping::Submaps* scan_trajectory,
const mapping::SubmapId& submap_id, const Submap* const submap,
const int scan_index, const mapping::Submaps* scan_trajectory,
const mapping::Submaps* submap_trajectory, bool match_full_submap,
mapping::TrajectoryConnectivity* trajectory_connectivity,
const sensor::CompressedPointCloud* const compressed_point_cloud,
@ -209,7 +209,7 @@ void ConstraintBuilder::ComputeConstraint(
const transform::Rigid3d initial_pose =
submap->local_pose() * initial_relative_pose;
const SubmapScanMatcher* const submap_scan_matcher =
GetSubmapScanMatcher(submap_index);
GetSubmapScanMatcher(submap_id);
const sensor::PointCloud point_cloud = compressed_point_cloud->Decompress();
const sensor::PointCloud filtered_point_cloud =
adaptive_voxel_filter_.Filter(point_cloud);
@ -258,7 +258,7 @@ void ConstraintBuilder::ComputeConstraint(
const transform::Rigid3d constraint_transform =
submap->local_pose().inverse() * pose_estimate;
constraint->reset(new OptimizationProblem::Constraint{
submap_index,
submap_id,
scan_index,
{constraint_transform,
1. / std::sqrt(options_.lower_covariance_eigenvalue_bound()) *
@ -270,7 +270,7 @@ void ConstraintBuilder::ComputeConstraint(
initial_pose.inverse() * pose_estimate;
std::ostringstream info;
info << "Scan index " << scan_index << " with "
<< filtered_point_cloud.size() << " points on submap " << submap_index
<< filtered_point_cloud.size() << " points on submap " << submap_id
<< " differs by translation " << std::fixed << std::setprecision(2)
<< difference.translation().norm() << " rotation "
<< std::setprecision(3) << transform::GetAngle(difference)

31
cartographer/mapping_3d/sparse_pose_graph/constraint_builder.h
View File

@ -68,18 +68,18 @@ class ConstraintBuilder {
ConstraintBuilder& operator=(const ConstraintBuilder&) = delete;
// Schedules exploring a new constraint between 'submap' identified by
// 'submap_index', and the 'range_data_3d.returns' in 'trajectory_nodes' for
// 'submap_id', and the 'range_data_3d.returns' in 'trajectory_nodes' for
// 'scan_index'. The 'initial_relative_pose' is relative to the 'submap'.
//
// The pointees of 'submap' and 'range_data_3d.returns' must stay valid until
// all computations are finished.
void MaybeAddConstraint(
int submap_index, const Submap* submap, int scan_index,
const mapping::SubmapId& submap_id, const Submap* submap, int scan_index,
const std::vector<mapping::TrajectoryNode>& trajectory_nodes,
const transform::Rigid3d& initial_relative_pose);
// Schedules exploring a new constraint between 'submap' identified by
// 'submap_index' and the 'range_data_3d.returns' in 'trajectory_nodes' for
// 'submap_id' and the 'range_data_3d.returns' in 'trajectory_nodes' for
// 'scan_index'. This performs full-submap matching.
//
// The scan at 'scan_index' should be from trajectory 'scan_trajectory', and
@ -89,7 +89,7 @@ class ConstraintBuilder {
// The pointees of 'submap' and 'range_data_3d.returns' must stay valid until
// all computations are finished.
void MaybeAddGlobalConstraint(
int submap_index, const Submap* submap, int scan_index,
const mapping::SubmapId& submap_id, const Submap* submap, int scan_index,
const mapping::Submaps* scan_trajectory,
const mapping::Submaps* submap_trajectory,
mapping::TrajectoryConnectivity* trajectory_connectivity,
@ -115,20 +115,20 @@ class ConstraintBuilder {
// Either schedules the 'work_item', or if needed, schedules the scan matcher
// construction and queues the 'work_item'.
void ScheduleSubmapScanMatcherConstructionAndQueueWorkItem(
int submap_index,
const mapping::SubmapId& submap_id,
const std::vector<mapping::TrajectoryNode>& submap_nodes,
const HybridGrid* submap, std::function<void()> work_item)
REQUIRES(mutex_);
// Constructs the scan matcher for a 'submap', then schedules its work items.
void ConstructSubmapScanMatcher(
int submap_index,
const mapping::SubmapId& submap_id,
const std::vector<mapping::TrajectoryNode>& submap_nodes,
const HybridGrid* submap) EXCLUDES(mutex_);
// Returns the scan matcher for a submap, which has to exist.
const SubmapScanMatcher* GetSubmapScanMatcher(int submap_index)
EXCLUDES(mutex_);
const SubmapScanMatcher* GetSubmapScanMatcher(
const mapping::SubmapId& submap_id) EXCLUDES(mutex_);
// Runs in a background thread and does computations for an additional
// constraint, assuming 'submap' and 'point_cloud' do not change anymore.
@ -137,7 +137,7 @@ class ConstraintBuilder {
// 'trajectory_connectivity'.
// As output, it may create a new Constraint in 'constraint'.
void ComputeConstraint(
int submap_index, const Submap* submap, int scan_index,
const mapping::SubmapId& submap_id, const Submap* submap, int scan_index,
const mapping::Submaps* scan_trajectory,
const mapping::Submaps* submap_trajectory, bool match_full_submap,
mapping::TrajectoryConnectivity* trajectory_connectivity,
@ -170,14 +170,15 @@ class ConstraintBuilder {
// keep pointers valid when adding more entries.
std::deque<std::unique_ptr<Constraint>> constraints_ GUARDED_BY(mutex_);
// Map of already constructed scan matchers by 'submap_index'.
std::map<int, SubmapScanMatcher> submap_scan_matchers_ GUARDED_BY(mutex_);
// Map by 'submap_index' of scan matchers under construction, and the work
// to do once construction is done.
std::map<int, std::vector<std::function<void()>>> submap_queued_work_items_
// Map of already constructed scan matchers by 'submap_id'.
std::map<mapping::SubmapId, SubmapScanMatcher> submap_scan_matchers_
GUARDED_BY(mutex_);
// Map by 'submap_id' of scan matchers under construction, and the work
// to do once construction is done.
std::map<mapping::SubmapId, std::vector<std::function<void()>>>
submap_queued_work_items_ GUARDED_BY(mutex_);
common::FixedRatioSampler sampler_;
const sensor::AdaptiveVoxelFilter adaptive_voxel_filter_;
scan_matching::CeresScanMatcher ceres_scan_matcher_;

32
cartographer/mapping_3d/sparse_pose_graph/optimization_problem.cc
View File

@ -91,9 +91,9 @@ void OptimizationProblem::AddTrajectoryNode(
node_data_.push_back(NodeData{trajectory, time, point_cloud_pose});
}
void OptimizationProblem::AddSubmap(const mapping::Submaps* const trajectory,
void OptimizationProblem::AddSubmap(const int trajectory_id,
const transform::Rigid3d& submap_pose) {
submap_data_.push_back(SubmapData{trajectory, submap_pose});
submap_data_.push_back(SubmapData{trajectory_id, submap_pose});
}
void OptimizationProblem::SetMaxNumIterations(const int32 max_num_iterations) {
@ -125,19 +125,23 @@ void OptimizationProblem::Solve(const std::vector<Constraint>& constraints) {
};
// Set the starting point.
std::deque<CeresPose> C_submaps;
// TODO(hrapp): Move ceres data into SubmapData.
std::vector<std::deque<CeresPose>> C_submaps(nodes_per_trajectory.size());
std::deque<CeresPose> C_point_clouds;
// Tie the first submap to the origin.
CHECK(!submap_data_.empty());
C_submaps.emplace_back(
C_submaps[submap_data_[0].trajectory_id].emplace_back(
transform::Rigid3d::Identity(), translation_parameterization(),
common::make_unique<ceres::AutoDiffLocalParameterization<
ConstantYawQuaternionPlus, 4, 2>>(),
&problem);
problem.SetParameterBlockConstant(C_submaps.back().translation());
problem.SetParameterBlockConstant(
C_submaps[submap_data_[0].trajectory_id].back().translation());
for (size_t i = 1; i != submap_data_.size(); ++i) {
C_submaps.emplace_back(
C_submaps[submap_data_[i].trajectory_id].emplace_back(
submap_data_[i].pose, translation_parameterization(),
common::make_unique<ceres::QuaternionParameterization>(), &problem);
}
@ -149,8 +153,6 @@ void OptimizationProblem::Solve(const std::vector<Constraint>& constraints) {
// Add cost functions for intra- and inter-submap constraints.
for (const Constraint& constraint : constraints) {
CHECK_GE(constraint.i, 0);
CHECK_LT(constraint.i, submap_data_.size());
CHECK_GE(constraint.j, 0);
CHECK_LT(constraint.j, node_data_.size());
problem.AddResidualBlock(
@ -160,8 +162,12 @@ void OptimizationProblem::Solve(const std::vector<Constraint>& constraints) {
constraint.tag == Constraint::INTER_SUBMAP
? new ceres::HuberLoss(options_.huber_scale())
: nullptr,
C_submaps[constraint.i].rotation(),
C_submaps[constraint.i].translation(),
C_submaps.at(constraint.i.trajectory_id)
.at(constraint.i.submap_index)
.rotation(),
C_submaps.at(constraint.i.trajectory_id)
.at(constraint.i.submap_index)
.translation(),
C_point_clouds[constraint.j].rotation(),
C_point_clouds[constraint.j].translation());
}
@ -242,9 +248,11 @@ void OptimizationProblem::Solve(const std::vector<Constraint>& constraints) {
}
// Store the result.
for (size_t i = 0; i != submap_data_.size(); ++i) {
submap_data_[i].pose = C_submaps[i].ToRigid();
for (auto& submap_data : submap_data_) {
submap_data.pose = C_submaps[submap_data.trajectory_id].front().ToRigid();
C_submaps[submap_data.trajectory_id].pop_front();
}
for (size_t j = 0; j != node_data_.size(); ++j) {
node_data_[j].point_cloud_pose = C_point_clouds[j].ToRigid();
}

5
cartographer/mapping_3d/sparse_pose_graph/optimization_problem.h
View File

@ -44,7 +44,7 @@ struct NodeData {
struct SubmapData {
// TODO(whess): Keep nodes per trajectory instead.
const mapping::Submaps* trajectory;
const int trajectory_id;
transform::Rigid3d pose;
};
@ -69,8 +69,7 @@ class OptimizationProblem {
const Eigen::Vector3d& angular_velocity);
void AddTrajectoryNode(const mapping::Submaps* trajectory, common::Time time,
const transform::Rigid3d& point_cloud_pose);
void AddSubmap(const mapping::Submaps* trajectory,
const transform::Rigid3d& submap_pose);
void AddSubmap(int trajectory_id, const transform::Rigid3d& submap_pose);
void SetMaxNumIterations(int32 max_num_iterations);

13
cartographer/mapping_3d/sparse_pose_graph/optimization_problem_test.cc
View File

@ -107,6 +107,7 @@ TEST_F(OptimizationProblemTest, ReducesNoise) {
const transform::Rigid3d kSubmap2Transform = transform::Rigid3d::Rotation(
Eigen::AngleAxisd(M_PI, Eigen::Vector3d::UnitZ()));
const mapping::Submaps* const kTrajectory = nullptr;
const int kTrajectoryId = 0;
struct NoisyNode {
transform::Rigid3d ground_truth_pose;
@ -132,13 +133,13 @@ TEST_F(OptimizationProblemTest, ReducesNoise) {
std::vector<OptimizationProblem::Constraint> constraints;
for (int j = 0; j != kNumNodes; ++j) {
constraints.push_back(OptimizationProblem::Constraint{
0, j,
mapping::SubmapId{0, 0}, j,
OptimizationProblem::Constraint::Pose{
AddNoise(test_data[j].ground_truth_pose, test_data[j].noise),
Eigen::Matrix<double, 6, 6>::Identity()}});
// We add an additional independent, but equally noisy observation.
constraints.push_back(OptimizationProblem::Constraint{
1, j,
mapping::SubmapId{0, 1}, j,
OptimizationProblem::Constraint::Pose{
AddNoise(test_data[j].ground_truth_pose,
RandomYawOnlyTransform(0.2, 0.3)),
@ -146,7 +147,7 @@ TEST_F(OptimizationProblemTest, ReducesNoise) {
// We add very noisy data with high covariance (i.e. small Lambda) to verify
// it is mostly ignored.
constraints.push_back(OptimizationProblem::Constraint{
2, j,
mapping::SubmapId{0, 2}, j,
OptimizationProblem::Constraint::Pose{
kSubmap2Transform.inverse() * test_data[j].ground_truth_pose *
RandomTransform(1e3, 3.),
@ -165,9 +166,9 @@ TEST_F(OptimizationProblemTest, ReducesNoise) {
node_data[j].point_cloud_pose);
}
optimization_problem_.AddSubmap(kTrajectory, kSubmap0Transform);
optimization_problem_.AddSubmap(kTrajectory, kSubmap0Transform);
optimization_problem_.AddSubmap(kTrajectory, kSubmap2Transform);
optimization_problem_.AddSubmap(kTrajectoryId, kSubmap0Transform);
optimization_problem_.AddSubmap(kTrajectoryId, kSubmap0Transform);
optimization_problem_.AddSubmap(kTrajectoryId, kSubmap2Transform);
optimization_problem_.Solve(constraints);
double translation_error_after = 0.;