Change fixed_frame_pose_data_ to MapByTime. (#662)
parent
fd5003b69b
commit
3ec583a327
|
@ -47,6 +47,32 @@ namespace cartographer {
|
||||||
namespace mapping_3d {
|
namespace mapping_3d {
|
||||||
namespace sparse_pose_graph {
|
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(
|
OptimizationProblem::OptimizationProblem(
|
||||||
const mapping::sparse_pose_graph::proto::OptimizationProblemOptions&
|
const mapping::sparse_pose_graph::proto::OptimizationProblemOptions&
|
||||||
options,
|
options,
|
||||||
|
@ -68,11 +94,7 @@ void OptimizationProblem::AddOdometerData(
|
||||||
void OptimizationProblem::AddFixedFramePoseData(
|
void OptimizationProblem::AddFixedFramePoseData(
|
||||||
const int trajectory_id,
|
const int trajectory_id,
|
||||||
const sensor::FixedFramePoseData& fixed_frame_pose_data) {
|
const sensor::FixedFramePoseData& fixed_frame_pose_data) {
|
||||||
CHECK_GE(trajectory_id, 0);
|
fixed_frame_pose_data_.Append(trajectory_id, fixed_frame_pose_data);
|
||||||
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);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
void OptimizationProblem::AddTrajectoryNode(
|
void OptimizationProblem::AddTrajectoryNode(
|
||||||
|
@ -98,6 +120,7 @@ void OptimizationProblem::InsertTrajectoryNode(
|
||||||
void OptimizationProblem::TrimTrajectoryNode(const mapping::NodeId& node_id) {
|
void OptimizationProblem::TrimTrajectoryNode(const mapping::NodeId& node_id) {
|
||||||
imu_data_.Trim(node_data_, node_id);
|
imu_data_.Trim(node_data_, node_id);
|
||||||
odometry_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);
|
node_data_.Trim(node_id);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -346,23 +369,25 @@ void OptimizationProblem::Solve(const std::vector<Constraint>& constraints,
|
||||||
std::deque<CeresPose> C_fixed_frames;
|
std::deque<CeresPose> C_fixed_frames;
|
||||||
for (auto node_it = node_data_.begin(); node_it != node_data_.end();) {
|
for (auto node_it = node_data_.begin(); node_it != node_data_.end();) {
|
||||||
const int trajectory_id = node_it->id.trajectory_id;
|
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);
|
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) {
|
for (; node_it != trajectory_end; ++node_it) {
|
||||||
const mapping::NodeId node_id = node_it->id;
|
const mapping::NodeId node_id = node_it->id;
|
||||||
const NodeData& node_data = node_it->data;
|
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;
|
continue;
|
||||||
}
|
}
|
||||||
|
|
||||||
const mapping::SparsePoseGraph::Constraint::Pose constraint_pose{
|
const mapping::SparsePoseGraph::Constraint::Pose constraint_pose{
|
||||||
fixed_frame_pose_data_.at(trajectory_id).Lookup(node_data.time),
|
*fixed_frame_pose, options_.fixed_frame_pose_translation_weight(),
|
||||||
options_.fixed_frame_pose_translation_weight(),
|
|
||||||
options_.fixed_frame_pose_rotation_weight()};
|
options_.fixed_frame_pose_rotation_weight()};
|
||||||
|
|
||||||
if (!fixed_frame_pose_initialized) {
|
if (!fixed_frame_pose_initialized) {
|
||||||
|
@ -440,33 +465,14 @@ const sensor::MapByTime<sensor::ImuData>& OptimizationProblem::imu_data()
|
||||||
return 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(
|
transform::Rigid3d OptimizationProblem::ComputeRelativePose(
|
||||||
const int trajectory_id, const NodeData& first_node_data,
|
const int trajectory_id, const NodeData& first_node_data,
|
||||||
const NodeData& second_node_data) const {
|
const NodeData& second_node_data) const {
|
||||||
if (odometry_data_.HasTrajectory(trajectory_id)) {
|
if (odometry_data_.HasTrajectory(trajectory_id)) {
|
||||||
const std::unique_ptr<transform::Rigid3d> first_node_odometry =
|
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 =
|
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) {
|
if (first_node_odometry != nullptr && second_node_odometry != nullptr) {
|
||||||
return first_node_odometry->inverse() * (*second_node_odometry);
|
return first_node_odometry->inverse() * (*second_node_odometry);
|
||||||
}
|
}
|
||||||
|
|
|
@ -96,8 +96,6 @@ class OptimizationProblem {
|
||||||
const sensor::MapByTime<sensor::ImuData>& imu_data() const;
|
const sensor::MapByTime<sensor::ImuData>& imu_data() const;
|
||||||
|
|
||||||
private:
|
private:
|
||||||
std::unique_ptr<transform::Rigid3d> InterpolateOdometry(
|
|
||||||
int trajectory_id, common::Time time) const;
|
|
||||||
// Uses odometry if available, otherwise the local SLAM results.
|
// Uses odometry if available, otherwise the local SLAM results.
|
||||||
transform::Rigid3d ComputeRelativePose(
|
transform::Rigid3d ComputeRelativePose(
|
||||||
int trajectory_id, const NodeData& first_node_data,
|
int trajectory_id, const NodeData& first_node_data,
|
||||||
|
@ -114,8 +112,8 @@ class OptimizationProblem {
|
||||||
mapping::MapById<mapping::SubmapId, SubmapData> submap_data_;
|
mapping::MapById<mapping::SubmapId, SubmapData> submap_data_;
|
||||||
sensor::MapByTime<sensor::ImuData> imu_data_;
|
sensor::MapByTime<sensor::ImuData> imu_data_;
|
||||||
sensor::MapByTime<sensor::OdometryData> odometry_data_;
|
sensor::MapByTime<sensor::OdometryData> odometry_data_;
|
||||||
|
sensor::MapByTime<sensor::FixedFramePoseData> fixed_frame_pose_data_;
|
||||||
std::vector<TrajectoryData> trajectory_data_;
|
std::vector<TrajectoryData> trajectory_data_;
|
||||||
std::vector<transform::TransformInterpolationBuffer> fixed_frame_pose_data_;
|
|
||||||
};
|
};
|
||||||
|
|
||||||
} // namespace sparse_pose_graph
|
} // namespace sparse_pose_graph
|
||||||
|
|
|
@ -52,6 +52,9 @@ class MapByTime {
|
||||||
const mapping::NodeId& node_id) {
|
const mapping::NodeId& node_id) {
|
||||||
const int trajectory_id = node_id.trajectory_id;
|
const int trajectory_id = node_id.trajectory_id;
|
||||||
CHECK_GE(trajectory_id, 0);
|
CHECK_GE(trajectory_id, 0);
|
||||||
|
if (data_.count(trajectory_id) == 0) {
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
// Data only important between 'gap_start' and 'gap_end' is no longer
|
// 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
|
// needed. We retain the first and last data of the gap so that
|
||||||
|
@ -68,7 +71,7 @@ class MapByTime {
|
||||||
: common::Time::max();
|
: common::Time::max();
|
||||||
CHECK_LT(gap_start, gap_end);
|
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_it = trajectory.lower_bound(gap_start);
|
||||||
auto data_end = trajectory.upper_bound(gap_end);
|
auto data_end = trajectory.upper_bound(gap_end);
|
||||||
if (data_it == data_end) {
|
if (data_it == data_end) {
|
||||||
|
|
|
@ -88,6 +88,15 @@ TEST(MapByTimeTest, Trimming) {
|
||||||
EXPECT_FALSE(map_by_time.HasTrajectory(42));
|
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
|
||||||
} // namespace sensor
|
} // namespace sensor
|
||||||
} // namespace cartographer
|
} // namespace cartographer
|
||||||
|
|
Loading…
Reference in New Issue