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

Change odometry_data_ to MapByTime. (#655)

master
Wolfgang Hess 2017-11-13 13:21:53 +01:00 committed by Wally B. Feed
parent d183ab737a
commit 4a8607810e
5 changed files with 108 additions and 42 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -30,7 +30,6 @@
#include "cartographer/mapping_2d/sparse_pose_graph/spa_cost_function.h"
#include "cartographer/sensor/odometry_data.h"
#include "cartographer/transform/transform.h"
#include "cartographer/transform/transform_interpolation_buffer.h"
#include "ceres/ceres.h"
#include "glog/logging.h"
@ -69,10 +68,7 @@ void OptimizationProblem::AddImuData(const int trajectory_id,
void OptimizationProblem::AddOdometerData(
const int trajectory_id, const sensor::OdometryData& odometry_data) {
CHECK_GE(trajectory_id, 0);
odometry_data_.resize(
std::max(odometry_data_.size(), static_cast<size_t>(trajectory_id) + 1));
odometry_data_[trajectory_id].Push(odometry_data.time, odometry_data.pose);
odometry_data_.Append(trajectory_id, odometry_data);
}
void OptimizationProblem::AddTrajectoryNode(
@ -93,6 +89,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);
node_data_.Trim(node_id);
}
@ -184,23 +181,8 @@ void OptimizationProblem::Solve(const std::vector<Constraint>& constraints,
continue;
}
const bool odometry_available =
trajectory_id < static_cast<int>(odometry_data_.size()) &&
odometry_data_[trajectory_id].Has(second_node_data.time) &&
odometry_data_[trajectory_id].Has(first_node_data.time);
const transform::Rigid3d relative_pose =
odometry_available
? transform::Rigid3d::Rotation(
first_node_data.gravity_alignment) *
odometry_data_[trajectory_id]
.Lookup(first_node_data.time)
.inverse() *
odometry_data_[trajectory_id].Lookup(
second_node_data.time) *
transform::Rigid3d::Rotation(
second_node_data.gravity_alignment.inverse())
: transform::Embed3D(first_node_data.initial_pose.inverse() *
second_node_data.initial_pose);
ComputeRelativePose(trajectory_id, first_node_data, second_node_data);
problem.AddResidualBlock(
new ceres::AutoDiffCostFunction<SpaCostFunction, 3, 3, 3>(
new SpaCostFunction(Constraint::Pose{
@ -246,6 +228,44 @@ 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);
const std::unique_ptr<transform::Rigid3d> second_node_odometry =
InterpolateOdometry(trajectory_id, second_node_data.time);
if (first_node_odometry != nullptr && second_node_odometry != nullptr) {
return transform::Rigid3d::Rotation(first_node_data.gravity_alignment) *
first_node_odometry->inverse() * (*second_node_odometry) *
transform::Rigid3d::Rotation(
second_node_data.gravity_alignment.inverse());
}
}
return transform::Embed3D(first_node_data.initial_pose.inverse() *
second_node_data.initial_pose);
}
} // namespace sparse_pose_graph
} // namespace mapping_2d
} // namespace cartographer

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

@ -33,7 +33,7 @@
#include "cartographer/sensor/imu_data.h"
#include "cartographer/sensor/map_by_time.h"
#include "cartographer/sensor/odometry_data.h"
#include "cartographer/transform/transform_interpolation_buffer.h"
#include "cartographer/transform/timestamped_transform.h"
namespace cartographer {
namespace mapping_2d {
@ -92,11 +92,18 @@ 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,
const NodeData& second_node_data) const;
mapping::sparse_pose_graph::proto::OptimizationProblemOptions options_;
sensor::MapByTime<sensor::ImuData> imu_data_;
mapping::MapById<mapping::NodeId, NodeData> node_data_;
std::vector<transform::TransformInterpolationBuffer> odometry_data_;
mapping::MapById<mapping::SubmapId, SubmapData> submap_data_;
sensor::MapByTime<sensor::ImuData> imu_data_;
sensor::MapByTime<sensor::OdometryData> odometry_data_;
};
} // namespace sparse_pose_graph

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

@ -36,6 +36,7 @@
#include "cartographer/mapping_3d/rotation_cost_function.h"
#include "cartographer/mapping_3d/rotation_parameterization.h"
#include "cartographer/mapping_3d/sparse_pose_graph/spa_cost_function.h"
#include "cartographer/transform/timestamped_transform.h"
#include "cartographer/transform/transform.h"
#include "ceres/ceres.h"
#include "ceres/jet.h"
@ -61,10 +62,7 @@ void OptimizationProblem::AddImuData(const int trajectory_id,
void OptimizationProblem::AddOdometerData(
const int trajectory_id, const sensor::OdometryData& odometry_data) {
CHECK_GE(trajectory_id, 0);
odometry_data_.resize(
std::max(odometry_data_.size(), static_cast<size_t>(trajectory_id) + 1));
odometry_data_[trajectory_id].Push(odometry_data.time, odometry_data.pose);
odometry_data_.Append(trajectory_id, odometry_data);
}
void OptimizationProblem::AddFixedFramePoseData(
@ -99,6 +97,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);
node_data_.Trim(node_id);
}
@ -323,18 +322,8 @@ void OptimizationProblem::Solve(const std::vector<Constraint>& constraints,
continue;
}
const bool odometry_available =
trajectory_id < static_cast<int>(odometry_data_.size()) &&
odometry_data_[trajectory_id].Has(second_node_data.time) &&
odometry_data_[trajectory_id].Has(first_node_data.time);
const transform::Rigid3d relative_pose =
odometry_available ? odometry_data_[trajectory_id]
.Lookup(first_node_data.time)
.inverse() *
odometry_data_[trajectory_id].Lookup(
second_node_data.time)
: first_node_data.local_pose.inverse() *
second_node_data.local_pose;
const transform::Rigid3d relative_pose = ComputeRelativePose(
trajectory_id, first_node_data, second_node_data);
problem.AddResidualBlock(
new ceres::AutoDiffCostFunction<SpaCostFunction, 6, 4, 3, 4, 3>(
new SpaCostFunction(Constraint::Pose{
@ -447,6 +436,40 @@ 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);
const std::unique_ptr<transform::Rigid3d> second_node_odometry =
InterpolateOdometry(trajectory_id, second_node_data.time);
if (first_node_odometry != nullptr && second_node_odometry != nullptr) {
return first_node_odometry->inverse() * (*second_node_odometry);
}
}
return first_node_data.local_pose.inverse() * second_node_data.local_pose;
}
} // namespace sparse_pose_graph
} // namespace mapping_3d
} // namespace cartographer

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

@ -96,6 +96,13 @@ 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,
const NodeData& second_node_data) const;
struct TrajectoryData {
double gravity_constant = 9.8;
std::array<double, 4> imu_calibration{{1., 0., 0., 0.}};
@ -103,10 +110,10 @@ class OptimizationProblem {
mapping::sparse_pose_graph::proto::OptimizationProblemOptions options_;
FixZ fix_z_;
sensor::MapByTime<sensor::ImuData> imu_data_;
mapping::MapById<mapping::NodeId, NodeData> node_data_;
std::vector<transform::TransformInterpolationBuffer> odometry_data_;
mapping::MapById<mapping::SubmapId, SubmapData> submap_data_;
sensor::MapByTime<sensor::ImuData> imu_data_;
sensor::MapByTime<sensor::OdometryData> odometry_data_;
std::vector<TrajectoryData> trajectory_data_;
std::vector<transform::TransformInterpolationBuffer> fixed_frame_pose_data_;
};

9
cartographer/sensor/map_by_time.h
View File

@ -191,6 +191,15 @@ class MapByTime {
EndOfTrajectory(trajectory_id));
}
// Returns an iterator to the the first element in the container belonging to
// 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'. 'trajectory_id' must refer to an existing trajectory.
ConstIterator lower_bound(const int trajectory_id,
const common::Time time) const {
return ConstIterator(data_.at(trajectory_id).lower_bound(time));
}
private:
std::map<int, std::map<common::Time, DataType>> data_;
};