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

Change fixed_frame_pose_data_ to MapByTime. (#662)

master
Wolfgang Hess 2017-11-13 17:41:04 +01:00 committed by Wally B. Feed
parent fd5003b69b
commit 3ec583a327
4 changed files with 54 additions and 38 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -47,6 +47,32 @@ namespace cartographer {
namespace mapping_3d {
namespace sparse_pose_graph {
namespace {
// For odometry and fixed frame pose.
template <typename MapByTimeType>
std::unique_ptr<transform::Rigid3d> Interpolate(
const MapByTimeType& map_by_time, const int trajectory_id,
const common::Time time) {
const auto it = map_by_time.lower_bound(trajectory_id, time);
if (it == map_by_time.EndOfTrajectory(trajectory_id)) {
return nullptr;
}
if (it == map_by_time.BeginOfTrajectory(trajectory_id)) {
if (it->time == time) {
return common::make_unique<transform::Rigid3d>(it->pose);
}
return nullptr;
}
const auto prev_it = std::prev(it);
return common::make_unique<transform::Rigid3d>(
Interpolate(transform::TimestampedTransform{prev_it->time, prev_it->pose},
transform::TimestampedTransform{it->time, it->pose}, time)
.transform);
}
} // namespace
OptimizationProblem::OptimizationProblem(
const mapping::sparse_pose_graph::proto::OptimizationProblemOptions&
options,
@ -68,11 +94,7 @@ void OptimizationProblem::AddOdometerData(
void OptimizationProblem::AddFixedFramePoseData(
const int trajectory_id,
const sensor::FixedFramePoseData& fixed_frame_pose_data) {
CHECK_GE(trajectory_id, 0);
fixed_frame_pose_data_.resize(std::max(
fixed_frame_pose_data_.size(), static_cast<size_t>(trajectory_id) + 1));
fixed_frame_pose_data_[trajectory_id].Push(fixed_frame_pose_data.time,
fixed_frame_pose_data.pose);
fixed_frame_pose_data_.Append(trajectory_id, fixed_frame_pose_data);
}
void OptimizationProblem::AddTrajectoryNode(
@ -98,6 +120,7 @@ void OptimizationProblem::InsertTrajectoryNode(
void OptimizationProblem::TrimTrajectoryNode(const mapping::NodeId& node_id) {
imu_data_.Trim(node_data_, node_id);
odometry_data_.Trim(node_data_, node_id);
fixed_frame_pose_data_.Trim(node_data_, node_id);
node_data_.Trim(node_id);
}
@ -346,23 +369,25 @@ void OptimizationProblem::Solve(const std::vector<Constraint>& constraints,
std::deque<CeresPose> C_fixed_frames;
for (auto node_it = node_data_.begin(); node_it != node_data_.end();) {
const int trajectory_id = node_it->id.trajectory_id;
if (trajectory_id >= static_cast<int>(fixed_frame_pose_data_.size())) {
break;
}
bool fixed_frame_pose_initialized = false;
const auto trajectory_end = node_data_.EndOfTrajectory(trajectory_id);
if (!fixed_frame_pose_data_.HasTrajectory(trajectory_id)) {
node_it = trajectory_end;
continue;
}
bool fixed_frame_pose_initialized = false;
for (; node_it != trajectory_end; ++node_it) {
const mapping::NodeId node_id = node_it->id;
const NodeData& node_data = node_it->data;
if (!fixed_frame_pose_data_.at(trajectory_id).Has(node_data.time)) {
const std::unique_ptr<transform::Rigid3d> fixed_frame_pose =
Interpolate(fixed_frame_pose_data_, trajectory_id, node_data.time);
if (fixed_frame_pose == nullptr) {
continue;
}
const mapping::SparsePoseGraph::Constraint::Pose constraint_pose{
fixed_frame_pose_data_.at(trajectory_id).Lookup(node_data.time),
options_.fixed_frame_pose_translation_weight(),
*fixed_frame_pose, options_.fixed_frame_pose_translation_weight(),
options_.fixed_frame_pose_rotation_weight()};
if (!fixed_frame_pose_initialized) {
@ -440,33 +465,14 @@ const sensor::MapByTime<sensor::ImuData>& OptimizationProblem::imu_data()
return imu_data_;
}
std::unique_ptr<transform::Rigid3d> OptimizationProblem::InterpolateOdometry(
const int trajectory_id, const common::Time time) const {
const auto it = odometry_data_.lower_bound(trajectory_id, time);
if (it == odometry_data_.EndOfTrajectory(trajectory_id)) {
return nullptr;
}
if (it == odometry_data_.BeginOfTrajectory(trajectory_id)) {
if (it->time == time) {
return common::make_unique<transform::Rigid3d>(it->pose);
}
return nullptr;
}
const auto prev_it = std::prev(it);
return common::make_unique<transform::Rigid3d>(
Interpolate(transform::TimestampedTransform{prev_it->time, prev_it->pose},
transform::TimestampedTransform{it->time, it->pose}, time)
.transform);
}
transform::Rigid3d OptimizationProblem::ComputeRelativePose(
const int trajectory_id, const NodeData& first_node_data,
const NodeData& second_node_data) const {
if (odometry_data_.HasTrajectory(trajectory_id)) {
const std::unique_ptr<transform::Rigid3d> first_node_odometry =
InterpolateOdometry(trajectory_id, first_node_data.time);
Interpolate(odometry_data_, trajectory_id, first_node_data.time);
const std::unique_ptr<transform::Rigid3d> second_node_odometry =
InterpolateOdometry(trajectory_id, second_node_data.time);
Interpolate(odometry_data_, trajectory_id, second_node_data.time);
if (first_node_odometry != nullptr && second_node_odometry != nullptr) {
return first_node_odometry->inverse() * (*second_node_odometry);
}

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

@ -96,8 +96,6 @@ class OptimizationProblem {
const sensor::MapByTime<sensor::ImuData>& imu_data() const;
private:
std::unique_ptr<transform::Rigid3d> InterpolateOdometry(
int trajectory_id, common::Time time) const;
// Uses odometry if available, otherwise the local SLAM results.
transform::Rigid3d ComputeRelativePose(
int trajectory_id, const NodeData& first_node_data,
@ -114,8 +112,8 @@ class OptimizationProblem {
mapping::MapById<mapping::SubmapId, SubmapData> submap_data_;
sensor::MapByTime<sensor::ImuData> imu_data_;
sensor::MapByTime<sensor::OdometryData> odometry_data_;
sensor::MapByTime<sensor::FixedFramePoseData> fixed_frame_pose_data_;
std::vector<TrajectoryData> trajectory_data_;
std::vector<transform::TransformInterpolationBuffer> fixed_frame_pose_data_;
};
} // namespace sparse_pose_graph

5
cartographer/sensor/map_by_time.h
View File

@ -52,6 +52,9 @@ class MapByTime {
const mapping::NodeId& node_id) {
const int trajectory_id = node_id.trajectory_id;
CHECK_GE(trajectory_id, 0);
if (data_.count(trajectory_id) == 0) {
return;
}
// Data only important between 'gap_start' and 'gap_end' is no longer
// needed. We retain the first and last data of the gap so that
@ -68,7 +71,7 @@ class MapByTime {
: common::Time::max();
CHECK_LT(gap_start, gap_end);
auto& trajectory = data_[trajectory_id];
auto& trajectory = data_.at(trajectory_id);
auto data_it = trajectory.lower_bound(gap_start);
auto data_end = trajectory.upper_bound(gap_end);
if (data_it == data_end) {

9
cartographer/sensor/map_by_time_test.cc
View File

@ -88,6 +88,15 @@ TEST(MapByTimeTest, Trimming) {
EXPECT_FALSE(map_by_time.HasTrajectory(42));
}
TEST(MapByTimeTest, TrimmingDoesNotCreateTrajectory) {
MapByTime<Data> map_by_time;
EXPECT_FALSE(map_by_time.HasTrajectory(42));
mapping::MapById<mapping::NodeId, NodeData> map_by_id;
map_by_id.Append(42, NodeData{CreateTime(42)});
map_by_time.Trim(map_by_id, mapping::NodeId{42, 0});
EXPECT_FALSE(map_by_time.HasTrajectory(42));
}
} // namespace
} // namespace sensor
} // namespace cartographer