Initial pose implementation. (#606)
parent
55e4338468
commit
eb96c91473
|
@ -328,7 +328,8 @@ class MapById {
|
||||||
// trajectory 'trajectory_id' whose time is not considered to go before
|
// trajectory 'trajectory_id' whose time is not considered to go before
|
||||||
// 'time', or EndOfTrajectory(trajectory_id) if all keys are considered to go
|
// 'time', or EndOfTrajectory(trajectory_id) if all keys are considered to go
|
||||||
// before 'time'.
|
// 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) {
|
if (SizeOfTrajectoryOrZero(trajectory_id) == 0) {
|
||||||
return EndOfTrajectory(trajectory_id);
|
return EndOfTrajectory(trajectory_id);
|
||||||
}
|
}
|
||||||
|
|
|
@ -23,6 +23,7 @@
|
||||||
#include <utility>
|
#include <utility>
|
||||||
|
|
||||||
#include "cartographer/common/make_unique.h"
|
#include "cartographer/common/make_unique.h"
|
||||||
|
#include "cartographer/common/time.h"
|
||||||
#include "cartographer/mapping/collated_trajectory_builder.h"
|
#include "cartographer/mapping/collated_trajectory_builder.h"
|
||||||
#include "cartographer/mapping/global_trajectory_builder.h"
|
#include "cartographer/mapping/global_trajectory_builder.h"
|
||||||
#include "cartographer/mapping_2d/local_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>(
|
sparse_pose_graph_->AddTrimmer(common::make_unique<PureLocalizationTrimmer>(
|
||||||
trajectory_id, kSubmapsToKeep));
|
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;
|
return trajectory_id;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
|
@ -14,15 +14,23 @@
|
||||||
|
|
||||||
syntax = "proto2";
|
syntax = "proto2";
|
||||||
|
|
||||||
|
import "cartographer/transform/proto/transform.proto";
|
||||||
import "cartographer/mapping_2d/proto/local_trajectory_builder_options.proto";
|
import "cartographer/mapping_2d/proto/local_trajectory_builder_options.proto";
|
||||||
import "cartographer/mapping_3d/proto/local_trajectory_builder_options.proto";
|
import "cartographer/mapping_3d/proto/local_trajectory_builder_options.proto";
|
||||||
|
|
||||||
package cartographer.mapping.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 {
|
message TrajectoryBuilderOptions {
|
||||||
optional mapping_2d.proto.LocalTrajectoryBuilderOptions
|
optional mapping_2d.proto.LocalTrajectoryBuilderOptions
|
||||||
trajectory_builder_2d_options = 1;
|
trajectory_builder_2d_options = 1;
|
||||||
optional mapping_3d.proto.LocalTrajectoryBuilderOptions
|
optional mapping_3d.proto.LocalTrajectoryBuilderOptions
|
||||||
trajectory_builder_3d_options = 2;
|
trajectory_builder_3d_options = 2;
|
||||||
optional bool pure_localization = 3;
|
optional bool pure_localization = 3;
|
||||||
|
optional InitialTrajectoryPose initial_trajectory_pose = 4;
|
||||||
}
|
}
|
||||||
|
|
|
@ -68,6 +68,12 @@ class SparsePoseGraph {
|
||||||
transform::Rigid3d pose;
|
transform::Rigid3d pose;
|
||||||
};
|
};
|
||||||
|
|
||||||
|
struct InitialTrajectoryPose {
|
||||||
|
int to_trajectory_id;
|
||||||
|
transform::Rigid3d relative_pose;
|
||||||
|
common::Time time;
|
||||||
|
};
|
||||||
|
|
||||||
SparsePoseGraph() {}
|
SparsePoseGraph() {}
|
||||||
virtual ~SparsePoseGraph() {}
|
virtual ~SparsePoseGraph() {}
|
||||||
|
|
||||||
|
@ -128,6 +134,13 @@ class SparsePoseGraph {
|
||||||
|
|
||||||
// Returns the collection of constraints.
|
// Returns the collection of constraints.
|
||||||
virtual std::vector<Constraint> constraints() = 0;
|
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(
|
std::vector<SparsePoseGraph::Constraint> FromProto(
|
||||||
|
|
|
@ -52,17 +52,24 @@ SparsePoseGraph::~SparsePoseGraph() {
|
||||||
CHECK(work_queue_ == nullptr);
|
CHECK(work_queue_ == nullptr);
|
||||||
}
|
}
|
||||||
|
|
||||||
std::vector<mapping::SubmapId> SparsePoseGraph::GrowSubmapTransformsAsNeeded(
|
std::vector<mapping::SubmapId> SparsePoseGraph::InitializeGlobalSubmapPoses(
|
||||||
const int trajectory_id,
|
const int trajectory_id, const common::Time time,
|
||||||
const std::vector<std::shared_ptr<const Submap>>& insertion_submaps) {
|
const std::vector<std::shared_ptr<const Submap>>& insertion_submaps) {
|
||||||
CHECK(!insertion_submaps.empty());
|
CHECK(!insertion_submaps.empty());
|
||||||
const auto& submap_data = optimization_problem_.submap_data();
|
const auto& submap_data = optimization_problem_.submap_data();
|
||||||
if (insertion_submaps.size() == 1) {
|
if (insertion_submaps.size() == 1) {
|
||||||
// If we don't already have an entry for the first submap, add one.
|
// If we don't already have an entry for the first submap, add one.
|
||||||
if (submap_data.SizeOfTrajectoryOrZero(trajectory_id) == 0) {
|
if (submap_data.SizeOfTrajectoryOrZero(trajectory_id) == 0) {
|
||||||
optimization_problem_.AddSubmap(
|
if (initial_trajectory_poses_.count(trajectory_id) > 0) {
|
||||||
|
trajectory_connectivity_state_.Connect(
|
||||||
trajectory_id,
|
trajectory_id,
|
||||||
sparse_pose_graph::ComputeSubmapPose(*insertion_submaps[0]));
|
initial_trajectory_poses_.at(trajectory_id).to_trajectory_id, time);
|
||||||
|
}
|
||||||
|
optimization_problem_.AddSubmap(
|
||||||
|
trajectory_id, transform::Project2D(
|
||||||
|
ComputeLocalToGlobalTransform(
|
||||||
|
optimized_submap_transforms_, trajectory_id) *
|
||||||
|
insertion_submaps[0]->local_pose()));
|
||||||
}
|
}
|
||||||
CHECK_EQ(1, submap_data.SizeOfTrajectoryOrZero(trajectory_id));
|
CHECK_EQ(1, submap_data.SizeOfTrajectoryOrZero(trajectory_id));
|
||||||
const mapping::SubmapId submap_id{trajectory_id, 0};
|
const mapping::SubmapId submap_id{trajectory_id, 0};
|
||||||
|
@ -213,11 +220,11 @@ void SparsePoseGraph::ComputeConstraintsForScan(
|
||||||
const mapping::NodeId& node_id,
|
const mapping::NodeId& node_id,
|
||||||
std::vector<std::shared_ptr<const Submap>> insertion_submaps,
|
std::vector<std::shared_ptr<const Submap>> insertion_submaps,
|
||||||
const bool newly_finished_submap) {
|
const bool newly_finished_submap) {
|
||||||
const std::vector<mapping::SubmapId> submap_ids =
|
const auto& constant_data = trajectory_nodes_.at(node_id).constant_data;
|
||||||
GrowSubmapTransformsAsNeeded(node_id.trajectory_id, insertion_submaps);
|
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());
|
CHECK_EQ(submap_ids.size(), insertion_submaps.size());
|
||||||
const mapping::SubmapId matching_id = submap_ids.front();
|
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(
|
const transform::Rigid2d pose = transform::Project2D(
|
||||||
constant_data->local_pose *
|
constant_data->local_pose *
|
||||||
transform::Rigid3d::Rotation(constant_data->gravity_alignment.inverse()));
|
transform::Rigid3d::Rotation(constant_data->gravity_alignment.inverse()));
|
||||||
|
@ -549,6 +556,35 @@ std::vector<SparsePoseGraph::Constraint> SparsePoseGraph::constraints() {
|
||||||
return result;
|
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(
|
transform::Rigid3d SparsePoseGraph::GetLocalToGlobalTransform(
|
||||||
const int trajectory_id) {
|
const int trajectory_id) {
|
||||||
common::MutexLocker locker(&mutex_);
|
common::MutexLocker locker(&mutex_);
|
||||||
|
@ -585,8 +621,15 @@ transform::Rigid3d SparsePoseGraph::ComputeLocalToGlobalTransform(
|
||||||
auto begin_it = submap_transforms.BeginOfTrajectory(trajectory_id);
|
auto begin_it = submap_transforms.BeginOfTrajectory(trajectory_id);
|
||||||
auto end_it = submap_transforms.EndOfTrajectory(trajectory_id);
|
auto end_it = submap_transforms.EndOfTrajectory(trajectory_id);
|
||||||
if (begin_it == end_it) {
|
if (begin_it == end_it) {
|
||||||
|
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();
|
return transform::Rigid3d::Identity();
|
||||||
}
|
}
|
||||||
|
}
|
||||||
const mapping::SubmapId last_optimized_submap_id = std::prev(end_it)->id;
|
const mapping::SubmapId last_optimized_submap_id = std::prev(end_it)->id;
|
||||||
// Accessing 'local_pose' in Submap is okay, since the member is const.
|
// Accessing 'local_pose' in Submap is okay, since the member is const.
|
||||||
return transform::Embed3D(
|
return transform::Embed3D(
|
||||||
|
|
|
@ -104,6 +104,12 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
|
||||||
mapping::MapById<mapping::NodeId, mapping::TrajectoryNode>
|
mapping::MapById<mapping::NodeId, mapping::TrajectoryNode>
|
||||||
GetTrajectoryNodes() override EXCLUDES(mutex_);
|
GetTrajectoryNodes() override EXCLUDES(mutex_);
|
||||||
std::vector<Constraint> constraints() 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:
|
private:
|
||||||
// The current state of the submap in the background threads. When this
|
// 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
|
// Grows the optimization problem to have an entry for every element of
|
||||||
// 'insertion_submaps'. Returns the IDs for the 'insertion_submaps'.
|
// 'insertion_submaps'. Returns the IDs for the 'insertion_submaps'.
|
||||||
std::vector<mapping::SubmapId> GrowSubmapTransformsAsNeeded(
|
std::vector<mapping::SubmapId> InitializeGlobalSubmapPoses(
|
||||||
int trajectory_id,
|
int trajectory_id, const common::Time time,
|
||||||
const std::vector<std::shared_ptr<const Submap>>& insertion_submaps)
|
const std::vector<std::shared_ptr<const Submap>>& insertion_submaps)
|
||||||
REQUIRES(mutex_);
|
REQUIRES(mutex_);
|
||||||
|
|
||||||
|
@ -226,6 +232,10 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
|
||||||
// Set of all frozen trajectories not being optimized.
|
// Set of all frozen trajectories not being optimized.
|
||||||
std::set<int> frozen_trajectories_ GUARDED_BY(mutex_);
|
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
|
// Allows querying and manipulating the pose graph by the 'trimmers_'. The
|
||||||
// 'mutex_' of the pose graph is held while this class is used.
|
// 'mutex_' of the pose graph is held while this class is used.
|
||||||
class TrimmingHandle : public mapping::Trimmable {
|
class TrimmingHandle : public mapping::Trimmable {
|
||||||
|
|
|
@ -52,15 +52,22 @@ SparsePoseGraph::~SparsePoseGraph() {
|
||||||
CHECK(work_queue_ == nullptr);
|
CHECK(work_queue_ == nullptr);
|
||||||
}
|
}
|
||||||
|
|
||||||
std::vector<mapping::SubmapId> SparsePoseGraph::GrowSubmapTransformsAsNeeded(
|
std::vector<mapping::SubmapId> SparsePoseGraph::InitializeGlobalSubmapPoses(
|
||||||
const int trajectory_id,
|
const int trajectory_id, const common::Time time,
|
||||||
const std::vector<std::shared_ptr<const Submap>>& insertion_submaps) {
|
const std::vector<std::shared_ptr<const Submap>>& insertion_submaps) {
|
||||||
CHECK(!insertion_submaps.empty());
|
CHECK(!insertion_submaps.empty());
|
||||||
const auto& submap_data = optimization_problem_.submap_data();
|
const auto& submap_data = optimization_problem_.submap_data();
|
||||||
if (insertion_submaps.size() == 1) {
|
if (insertion_submaps.size() == 1) {
|
||||||
// If we don't already have an entry for the first submap, add one.
|
// If we don't already have an entry for the first submap, add one.
|
||||||
if (submap_data.SizeOfTrajectoryOrZero(trajectory_id) == 0) {
|
if (submap_data.SizeOfTrajectoryOrZero(trajectory_id) == 0) {
|
||||||
optimization_problem_.AddSubmap(trajectory_id,
|
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());
|
insertion_submaps[0]->local_pose());
|
||||||
}
|
}
|
||||||
CHECK_EQ(1, submap_data.SizeOfTrajectoryOrZero(trajectory_id));
|
CHECK_EQ(1, submap_data.SizeOfTrajectoryOrZero(trajectory_id));
|
||||||
|
@ -240,11 +247,11 @@ void SparsePoseGraph::ComputeConstraintsForScan(
|
||||||
const mapping::NodeId& node_id,
|
const mapping::NodeId& node_id,
|
||||||
std::vector<std::shared_ptr<const Submap>> insertion_submaps,
|
std::vector<std::shared_ptr<const Submap>> insertion_submaps,
|
||||||
const bool newly_finished_submap) {
|
const bool newly_finished_submap) {
|
||||||
const std::vector<mapping::SubmapId> submap_ids =
|
const auto& constant_data = trajectory_nodes_.at(node_id).constant_data;
|
||||||
GrowSubmapTransformsAsNeeded(node_id.trajectory_id, insertion_submaps);
|
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());
|
CHECK_EQ(submap_ids.size(), insertion_submaps.size());
|
||||||
const mapping::SubmapId matching_id = submap_ids.front();
|
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& pose = constant_data->local_pose;
|
||||||
const transform::Rigid3d optimized_pose =
|
const transform::Rigid3d optimized_pose =
|
||||||
optimization_problem_.submap_data().at(matching_id).pose *
|
optimization_problem_.submap_data().at(matching_id).pose *
|
||||||
|
@ -571,6 +578,35 @@ std::vector<SparsePoseGraph::Constraint> SparsePoseGraph::constraints() {
|
||||||
return 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(
|
transform::Rigid3d SparsePoseGraph::GetLocalToGlobalTransform(
|
||||||
const int trajectory_id) {
|
const int trajectory_id) {
|
||||||
common::MutexLocker locker(&mutex_);
|
common::MutexLocker locker(&mutex_);
|
||||||
|
@ -607,8 +643,15 @@ transform::Rigid3d SparsePoseGraph::ComputeLocalToGlobalTransform(
|
||||||
auto begin_it = submap_transforms.BeginOfTrajectory(trajectory_id);
|
auto begin_it = submap_transforms.BeginOfTrajectory(trajectory_id);
|
||||||
auto end_it = submap_transforms.EndOfTrajectory(trajectory_id);
|
auto end_it = submap_transforms.EndOfTrajectory(trajectory_id);
|
||||||
if (begin_it == end_it) {
|
if (begin_it == end_it) {
|
||||||
|
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();
|
return transform::Rigid3d::Identity();
|
||||||
}
|
}
|
||||||
|
}
|
||||||
const mapping::SubmapId last_optimized_submap_id = std::prev(end_it)->id;
|
const mapping::SubmapId last_optimized_submap_id = std::prev(end_it)->id;
|
||||||
// Accessing 'local_pose' in Submap is okay, since the member is const.
|
// Accessing 'local_pose' in Submap is okay, since the member is const.
|
||||||
return submap_transforms.at(last_optimized_submap_id).pose *
|
return submap_transforms.at(last_optimized_submap_id).pose *
|
||||||
|
|
|
@ -104,6 +104,12 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
|
||||||
mapping::MapById<mapping::NodeId, mapping::TrajectoryNode>
|
mapping::MapById<mapping::NodeId, mapping::TrajectoryNode>
|
||||||
GetTrajectoryNodes() override EXCLUDES(mutex_);
|
GetTrajectoryNodes() override EXCLUDES(mutex_);
|
||||||
std::vector<Constraint> constraints() 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:
|
private:
|
||||||
// The current state of the submap in the background threads. When this
|
// 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
|
// Grows the optimization problem to have an entry for every element of
|
||||||
// 'insertion_submaps'. Returns the IDs for the 'insertion_submaps'.
|
// 'insertion_submaps'. Returns the IDs for the 'insertion_submaps'.
|
||||||
std::vector<mapping::SubmapId> GrowSubmapTransformsAsNeeded(
|
std::vector<mapping::SubmapId> InitializeGlobalSubmapPoses(
|
||||||
int trajectory_id,
|
int trajectory_id, const common::Time time,
|
||||||
const std::vector<std::shared_ptr<const Submap>>& insertion_submaps)
|
const std::vector<std::shared_ptr<const Submap>>& insertion_submaps)
|
||||||
REQUIRES(mutex_);
|
REQUIRES(mutex_);
|
||||||
|
|
||||||
|
@ -230,6 +236,10 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
|
||||||
// Set of all frozen trajectories not being optimized.
|
// Set of all frozen trajectories not being optimized.
|
||||||
std::set<int> frozen_trajectories_ GUARDED_BY(mutex_);
|
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
|
// Allows querying and manipulating the pose graph by the 'trimmers_'. The
|
||||||
// 'mutex_' of the pose graph is held while this class is used.
|
// 'mutex_' of the pose graph is held while this class is used.
|
||||||
class TrimmingHandle : public mapping::Trimmable {
|
class TrimmingHandle : public mapping::Trimmable {
|
||||||
|
|
Loading…
Reference in New Issue