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

Initial pose implementation. (#606)

master
Christoph Schütte 2017-11-08 14:01:38 +01:00 committed by GitHub
parent 55e4338468
commit eb96c91473
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
8 changed files with 158 additions and 21 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
cartographer/mapping/id.h
View File

@ -328,7 +328,8 @@ class MapById {
// trajectory 'trajectory_id' whose time is not considered to go before
// 'time', or EndOfTrajectory(trajectory_id) if all keys are considered to go
// before 'time'.
ConstIterator lower_bound(const int trajectory_id, const common::Time& time) {
ConstIterator lower_bound(const int trajectory_id,
const common::Time time) const {
if (SizeOfTrajectoryOrZero(trajectory_id) == 0) {
return EndOfTrajectory(trajectory_id);
}

9
cartographer/mapping/map_builder.cc
View File

@ -23,6 +23,7 @@
#include <utility>
#include "cartographer/common/make_unique.h"
#include "cartographer/common/time.h"
#include "cartographer/mapping/collated_trajectory_builder.h"
#include "cartographer/mapping/global_trajectory_builder.h"
#include "cartographer/mapping_2d/local_trajectory_builder.h"
@ -100,6 +101,14 @@ int MapBuilder::AddTrajectoryBuilder(
sparse_pose_graph_->AddTrimmer(common::make_unique<PureLocalizationTrimmer>(
trajectory_id, kSubmapsToKeep));
}
if (trajectory_options.has_initial_trajectory_pose()) {
const auto& initial_trajectory_pose =
trajectory_options.initial_trajectory_pose();
sparse_pose_graph_->SetInitialTrajectoryPose(
trajectory_id, initial_trajectory_pose.to_trajectory_id(),
transform::ToRigid3(initial_trajectory_pose.relative_pose()),
common::FromUniversal(initial_trajectory_pose.timestamp()));
}
return trajectory_id;
}

8
cartographer/mapping/proto/trajectory_builder_options.proto
View File

@ -14,15 +14,23 @@
syntax = "proto2";
import "cartographer/transform/proto/transform.proto";
import "cartographer/mapping_2d/proto/local_trajectory_builder_options.proto";
import "cartographer/mapping_3d/proto/local_trajectory_builder_options.proto";
package cartographer.mapping.proto;
message InitialTrajectoryPose {
optional transform.proto.Rigid3d relative_pose = 1;
optional int32 to_trajectory_id = 2;
optional int64 timestamp = 3;
}
message TrajectoryBuilderOptions {
optional mapping_2d.proto.LocalTrajectoryBuilderOptions
trajectory_builder_2d_options = 1;
optional mapping_3d.proto.LocalTrajectoryBuilderOptions
trajectory_builder_3d_options = 2;
optional bool pure_localization = 3;
optional InitialTrajectoryPose initial_trajectory_pose = 4;
}

13
cartographer/mapping/sparse_pose_graph.h
View File

@ -68,6 +68,12 @@ class SparsePoseGraph {
transform::Rigid3d pose;
};
struct InitialTrajectoryPose {
int to_trajectory_id;
transform::Rigid3d relative_pose;
common::Time time;
};
SparsePoseGraph() {}
virtual ~SparsePoseGraph() {}
@ -128,6 +134,13 @@ class SparsePoseGraph {
// Returns the collection of constraints.
virtual std::vector<Constraint> constraints() = 0;
// Sets a relative initial pose 'relative_pose' for 'from_trajectory_id' with
// respect to 'to_trajectory_id' at time 'time'.
virtual void SetInitialTrajectoryPose(int from_trajectory_id,
int to_trajectory_id,
const transform::Rigid3d& pose,
const common::Time time) = 0;
};
std::vector<SparsePoseGraph::Constraint> FromProto(

59
cartographer/mapping_2d/sparse_pose_graph.cc
View File

@ -52,17 +52,24 @@ SparsePoseGraph::~SparsePoseGraph() {
CHECK(work_queue_ == nullptr);
}
std::vector<mapping::SubmapId> SparsePoseGraph::GrowSubmapTransformsAsNeeded(
const int trajectory_id,
std::vector<mapping::SubmapId> SparsePoseGraph::InitializeGlobalSubmapPoses(
const int trajectory_id, const common::Time time,
const std::vector<std::shared_ptr<const Submap>>& insertion_submaps) {
CHECK(!insertion_submaps.empty());
const auto& submap_data = optimization_problem_.submap_data();
if (insertion_submaps.size() == 1) {
// If we don't already have an entry for the first submap, add one.
if (submap_data.SizeOfTrajectoryOrZero(trajectory_id) == 0) {
if (initial_trajectory_poses_.count(trajectory_id) > 0) {
trajectory_connectivity_state_.Connect(
trajectory_id,
initial_trajectory_poses_.at(trajectory_id).to_trajectory_id, time);
}
optimization_problem_.AddSubmap(
trajectory_id,
sparse_pose_graph::ComputeSubmapPose(*insertion_submaps[0]));
trajectory_id, transform::Project2D(
ComputeLocalToGlobalTransform(
optimized_submap_transforms_, trajectory_id) *
insertion_submaps[0]->local_pose()));
}
CHECK_EQ(1, submap_data.SizeOfTrajectoryOrZero(trajectory_id));
const mapping::SubmapId submap_id{trajectory_id, 0};
@ -213,11 +220,11 @@ void SparsePoseGraph::ComputeConstraintsForScan(
const mapping::NodeId& node_id,
std::vector<std::shared_ptr<const Submap>> insertion_submaps,
const bool newly_finished_submap) {
const std::vector<mapping::SubmapId> submap_ids =
GrowSubmapTransformsAsNeeded(node_id.trajectory_id, insertion_submaps);
const auto& constant_data = trajectory_nodes_.at(node_id).constant_data;
const std::vector<mapping::SubmapId> submap_ids = InitializeGlobalSubmapPoses(
node_id.trajectory_id, constant_data->time, insertion_submaps);
CHECK_EQ(submap_ids.size(), insertion_submaps.size());
const mapping::SubmapId matching_id = submap_ids.front();
const auto& constant_data = trajectory_nodes_.at(node_id).constant_data;
const transform::Rigid2d pose = transform::Project2D(
constant_data->local_pose *
transform::Rigid3d::Rotation(constant_data->gravity_alignment.inverse()));
@ -549,6 +556,35 @@ std::vector<SparsePoseGraph::Constraint> SparsePoseGraph::constraints() {
return result;
}
void SparsePoseGraph::SetInitialTrajectoryPose(const int from_trajectory_id,
const int to_trajectory_id,
const transform::Rigid3d& pose,
const common::Time time) {
common::MutexLocker locker(&mutex_);
initial_trajectory_poses_[from_trajectory_id] =
InitialTrajectoryPose{to_trajectory_id, pose, time};
}
transform::Rigid3d SparsePoseGraph::GetInterpolatedGlobalTrajectoryPose(
const int trajectory_id, const common::Time time) const {
CHECK(trajectory_nodes_.SizeOfTrajectoryOrZero(trajectory_id) > 0);
const auto it = trajectory_nodes_.lower_bound(trajectory_id, time);
if (it == trajectory_nodes_.BeginOfTrajectory(trajectory_id)) {
return trajectory_nodes_.BeginOfTrajectory(trajectory_id)->data.global_pose;
}
if (it == trajectory_nodes_.EndOfTrajectory(trajectory_id)) {
return std::prev(trajectory_nodes_.EndOfTrajectory(trajectory_id))
->data.global_pose;
}
return transform::Interpolate(
transform::TimestampedTransform{std::prev(it)->data.time(),
std::prev(it)->data.global_pose},
transform::TimestampedTransform{it->data.time(),
it->data.global_pose},
time)
.transform;
}
transform::Rigid3d SparsePoseGraph::GetLocalToGlobalTransform(
const int trajectory_id) {
common::MutexLocker locker(&mutex_);
@ -585,7 +621,14 @@ transform::Rigid3d SparsePoseGraph::ComputeLocalToGlobalTransform(
auto begin_it = submap_transforms.BeginOfTrajectory(trajectory_id);
auto end_it = submap_transforms.EndOfTrajectory(trajectory_id);
if (begin_it == end_it) {
return transform::Rigid3d::Identity();
const auto it = initial_trajectory_poses_.find(trajectory_id);
if (it != initial_trajectory_poses_.end()) {
return GetInterpolatedGlobalTrajectoryPose(it->second.to_trajectory_id,
it->second.time) *
it->second.relative_pose;
} else {
return transform::Rigid3d::Identity();
}
}
const mapping::SubmapId last_optimized_submap_id = std::prev(end_it)->id;
// Accessing 'local_pose' in Submap is okay, since the member is const.

14
cartographer/mapping_2d/sparse_pose_graph.h
View File

@ -104,6 +104,12 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
mapping::MapById<mapping::NodeId, mapping::TrajectoryNode>
GetTrajectoryNodes() override EXCLUDES(mutex_);
std::vector<Constraint> constraints() override EXCLUDES(mutex_);
void SetInitialTrajectoryPose(int from_trajectory_id, int to_trajectory_id,
const transform::Rigid3d& pose,
const common::Time time) override
EXCLUDES(mutex_);
transform::Rigid3d GetInterpolatedGlobalTrajectoryPose(
int trajectory_id, const common::Time time) const REQUIRES(mutex_);
private:
// The current state of the submap in the background threads. When this
@ -129,8 +135,8 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
// Grows the optimization problem to have an entry for every element of
// 'insertion_submaps'. Returns the IDs for the 'insertion_submaps'.
std::vector<mapping::SubmapId> GrowSubmapTransformsAsNeeded(
int trajectory_id,
std::vector<mapping::SubmapId> InitializeGlobalSubmapPoses(
int trajectory_id, const common::Time time,
const std::vector<std::shared_ptr<const Submap>>& insertion_submaps)
REQUIRES(mutex_);
@ -226,6 +232,10 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
// Set of all frozen trajectories not being optimized.
std::set<int> frozen_trajectories_ GUARDED_BY(mutex_);
// Set of all initial trajectory poses.
std::map<int, InitialTrajectoryPose> initial_trajectory_poses_
GUARDED_BY(mutex_);
// Allows querying and manipulating the pose graph by the 'trimmers_'. The
// 'mutex_' of the pose graph is held while this class is used.
class TrimmingHandle : public mapping::Trimmable {

59
cartographer/mapping_3d/sparse_pose_graph.cc
View File

@ -52,16 +52,23 @@ SparsePoseGraph::~SparsePoseGraph() {
CHECK(work_queue_ == nullptr);
}
std::vector<mapping::SubmapId> SparsePoseGraph::GrowSubmapTransformsAsNeeded(
const int trajectory_id,
std::vector<mapping::SubmapId> SparsePoseGraph::InitializeGlobalSubmapPoses(
const int trajectory_id, const common::Time time,
const std::vector<std::shared_ptr<const Submap>>& insertion_submaps) {
CHECK(!insertion_submaps.empty());
const auto& submap_data = optimization_problem_.submap_data();
if (insertion_submaps.size() == 1) {
// If we don't already have an entry for the first submap, add one.
if (submap_data.SizeOfTrajectoryOrZero(trajectory_id) == 0) {
optimization_problem_.AddSubmap(trajectory_id,
insertion_submaps[0]->local_pose());
if (initial_trajectory_poses_.count(trajectory_id) > 0) {
trajectory_connectivity_state_.Connect(
trajectory_id,
initial_trajectory_poses_.at(trajectory_id).to_trajectory_id, time);
}
optimization_problem_.AddSubmap(
trajectory_id, ComputeLocalToGlobalTransform(
optimized_submap_transforms_, trajectory_id) *
insertion_submaps[0]->local_pose());
}
CHECK_EQ(1, submap_data.SizeOfTrajectoryOrZero(trajectory_id));
const mapping::SubmapId submap_id{trajectory_id, 0};
@ -240,11 +247,11 @@ void SparsePoseGraph::ComputeConstraintsForScan(
const mapping::NodeId& node_id,
std::vector<std::shared_ptr<const Submap>> insertion_submaps,
const bool newly_finished_submap) {
const std::vector<mapping::SubmapId> submap_ids =
GrowSubmapTransformsAsNeeded(node_id.trajectory_id, insertion_submaps);
const auto& constant_data = trajectory_nodes_.at(node_id).constant_data;
const std::vector<mapping::SubmapId> submap_ids = InitializeGlobalSubmapPoses(
node_id.trajectory_id, constant_data->time, insertion_submaps);
CHECK_EQ(submap_ids.size(), insertion_submaps.size());
const mapping::SubmapId matching_id = submap_ids.front();
const auto& constant_data = trajectory_nodes_.at(node_id).constant_data;
const transform::Rigid3d& pose = constant_data->local_pose;
const transform::Rigid3d optimized_pose =
optimization_problem_.submap_data().at(matching_id).pose *
@ -571,6 +578,35 @@ std::vector<SparsePoseGraph::Constraint> SparsePoseGraph::constraints() {
return constraints_;
}
void SparsePoseGraph::SetInitialTrajectoryPose(const int from_trajectory_id,
const int to_trajectory_id,
const transform::Rigid3d& pose,
const common::Time time) {
common::MutexLocker locker(&mutex_);
initial_trajectory_poses_[from_trajectory_id] =
InitialTrajectoryPose{to_trajectory_id, pose, time};
}
transform::Rigid3d SparsePoseGraph::GetInterpolatedGlobalTrajectoryPose(
const int trajectory_id, const common::Time time) const {
CHECK(trajectory_nodes_.SizeOfTrajectoryOrZero(trajectory_id) > 0);
const auto it = trajectory_nodes_.lower_bound(trajectory_id, time);
if (it == trajectory_nodes_.BeginOfTrajectory(trajectory_id)) {
return trajectory_nodes_.BeginOfTrajectory(trajectory_id)->data.global_pose;
}
if (it == trajectory_nodes_.EndOfTrajectory(trajectory_id)) {
return std::prev(trajectory_nodes_.EndOfTrajectory(trajectory_id))
->data.global_pose;
}
return transform::Interpolate(
transform::TimestampedTransform{std::prev(it)->data.time(),
std::prev(it)->data.global_pose},
transform::TimestampedTransform{it->data.time(),
it->data.global_pose},
time)
.transform;
}
transform::Rigid3d SparsePoseGraph::GetLocalToGlobalTransform(
const int trajectory_id) {
common::MutexLocker locker(&mutex_);
@ -607,7 +643,14 @@ transform::Rigid3d SparsePoseGraph::ComputeLocalToGlobalTransform(
auto begin_it = submap_transforms.BeginOfTrajectory(trajectory_id);
auto end_it = submap_transforms.EndOfTrajectory(trajectory_id);
if (begin_it == end_it) {
return transform::Rigid3d::Identity();
const auto it = initial_trajectory_poses_.find(trajectory_id);
if (it != initial_trajectory_poses_.end()) {
return GetInterpolatedGlobalTrajectoryPose(it->second.to_trajectory_id,
it->second.time) *
it->second.relative_pose;
} else {
return transform::Rigid3d::Identity();
}
}
const mapping::SubmapId last_optimized_submap_id = std::prev(end_it)->id;
// Accessing 'local_pose' in Submap is okay, since the member is const.

14
cartographer/mapping_3d/sparse_pose_graph.h
View File

@ -104,6 +104,12 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
mapping::MapById<mapping::NodeId, mapping::TrajectoryNode>
GetTrajectoryNodes() override EXCLUDES(mutex_);
std::vector<Constraint> constraints() override EXCLUDES(mutex_);
void SetInitialTrajectoryPose(int from_trajectory_id, int to_trajectory_id,
const transform::Rigid3d& pose,
const common::Time time) override
EXCLUDES(mutex_);
transform::Rigid3d GetInterpolatedGlobalTrajectoryPose(
int trajectory_id, const common::Time time) const REQUIRES(mutex_);
private:
// The current state of the submap in the background threads. When this
@ -129,8 +135,8 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
// Grows the optimization problem to have an entry for every element of
// 'insertion_submaps'. Returns the IDs for the 'insertion_submaps'.
std::vector<mapping::SubmapId> GrowSubmapTransformsAsNeeded(
int trajectory_id,
std::vector<mapping::SubmapId> InitializeGlobalSubmapPoses(
int trajectory_id, const common::Time time,
const std::vector<std::shared_ptr<const Submap>>& insertion_submaps)
REQUIRES(mutex_);
@ -230,6 +236,10 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
// Set of all frozen trajectories not being optimized.
std::set<int> frozen_trajectories_ GUARDED_BY(mutex_);
// Set of all initial trajectory poses.
std::map<int, InitialTrajectoryPose> initial_trajectory_poses_
GUARDED_BY(mutex_);
// Allows querying and manipulating the pose graph by the 'trimmers_'. The
// 'mutex_' of the pose graph is held while this class is used.
class TrimmingHandle : public mapping::Trimmable {