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

Use odometry in 2D pose graph optimization. (#456)

master
Moritz Münst 2017-08-21 15:12:00 +02:00 committed by Wolfgang Hess
parent bcde3b45b0
commit 0ef372d584
5 changed files with 49 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
cartographer/mapping_2d/global_trajectory_builder.cc
View File

@ -52,6 +52,8 @@ void GlobalTrajectoryBuilder::AddImuData(const sensor::ImuData& imu_data) {
void GlobalTrajectoryBuilder::AddOdometerData(const common::Time time,
const transform::Rigid3d& pose) {
local_trajectory_builder_.AddOdometerData(time, pose);
sparse_pose_graph_->AddOdometerData(trajectory_id_,
sensor::OdometryData{time, pose});
}
const mapping::GlobalTrajectoryBuilderInterface::PoseEstimate&

8
cartographer/mapping_2d/sparse_pose_graph.cc
View File

@ -164,6 +164,14 @@ void SparsePoseGraph::AddImuData(const int trajectory_id,
});
}
void SparsePoseGraph::AddOdometerData(
const int trajectory_id, const sensor::OdometryData& odometry_data) {
common::MutexLocker locker(&mutex_);
AddWorkItem([=]() REQUIRES(mutex_) {
optimization_problem_.AddOdometerData(trajectory_id, odometry_data);
});
}
void SparsePoseGraph::ComputeConstraint(const mapping::NodeId& node_id,
const mapping::SubmapId& submap_id) {
CHECK(submap_data_.at(submap_id).state == SubmapState::kFinished);

2
cartographer/mapping_2d/sparse_pose_graph.h
View File

@ -78,6 +78,8 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
// Adds new IMU data to be used in the optimization.
void AddImuData(int trajectory_id, const sensor::ImuData& imu_data);
void AddOdometerData(int trajectory_id,
const sensor::OdometryData& odometry_data);
void FreezeTrajectory(int trajectory_id) override;
void AddSubmapFromProto(int trajectory_id,

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

@ -28,7 +28,9 @@
#include "cartographer/common/histogram.h"
#include "cartographer/common/math.h"
#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"
@ -68,6 +70,14 @@ void OptimizationProblem::AddImuData(const int trajectory_id,
imu_data_[trajectory_id].push_back(imu_data);
}
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);
}
void OptimizationProblem::AddTrajectoryNode(
const int trajectory_id, const common::Time time,
const transform::Rigid2d& initial_point_cloud_pose,
@ -170,7 +180,6 @@ void OptimizationProblem::Solve(const std::vector<Constraint>& constraints,
}
}
}
// Add cost functions for intra- and inter-submap constraints.
for (const Constraint& constraint : constraints) {
problem.AddResidualBlock(
@ -192,20 +201,36 @@ void OptimizationProblem::Solve(const std::vector<Constraint>& constraints,
.data());
}
// Add penalties for changes between consecutive scans.
// Add penalties for violating odometry or changes between consecutive scans
// if odometry is not available.
for (size_t trajectory_id = 0; trajectory_id != node_data_.size();
++trajectory_id) {
for (size_t node_data_index = 1;
node_data_index < node_data_[trajectory_id].size();
++node_data_index) {
problem.AddResidualBlock(
new ceres::AutoDiffCostFunction<SpaCostFunction, 3, 3, 3>(
new SpaCostFunction(Constraint::Pose{
transform::Embed3D(
const bool odometry_available =
trajectory_id < odometry_data_.size() &&
odometry_data_[trajectory_id].Has(
node_data_[trajectory_id][node_data_index].time) &&
odometry_data_[trajectory_id].Has(
node_data_[trajectory_id][node_data_index - 1].time);
const transform::Rigid3d relative_pose =
odometry_available
? odometry_data_[trajectory_id]
.Lookup(
node_data_[trajectory_id][node_data_index - 1].time)
.inverse() *
odometry_data_[trajectory_id].Lookup(
node_data_[trajectory_id][node_data_index].time)
: transform::Embed3D(
node_data_[trajectory_id][node_data_index - 1]
.initial_point_cloud_pose.inverse() *
node_data_[trajectory_id][node_data_index]
.initial_point_cloud_pose),
.initial_point_cloud_pose);
problem.AddResidualBlock(
new ceres::AutoDiffCostFunction<SpaCostFunction, 3, 3, 3>(
new SpaCostFunction(Constraint::Pose{
relative_pose,
options_.consecutive_scan_translation_penalty_factor(),
options_.consecutive_scan_rotation_penalty_factor()})),
nullptr /* loss function */,

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

@ -30,6 +30,8 @@
#include "cartographer/mapping/sparse_pose_graph.h"
#include "cartographer/mapping/sparse_pose_graph/proto/optimization_problem_options.pb.h"
#include "cartographer/sensor/imu_data.h"
#include "cartographer/sensor/odometry_data.h"
#include "cartographer/transform/transform_interpolation_buffer.h"
namespace cartographer {
namespace mapping_2d {
@ -59,6 +61,8 @@ class OptimizationProblem {
OptimizationProblem& operator=(const OptimizationProblem&) = delete;
void AddImuData(int trajectory_id, const sensor::ImuData& imu_data);
void AddOdometerData(int trajectory_id,
const sensor::OdometryData& odometry_data);
void AddTrajectoryNode(int trajectory_id, common::Time time,
const transform::Rigid2d& initial_point_cloud_pose,
const transform::Rigid2d& point_cloud_pose);
@ -87,6 +91,7 @@ class OptimizationProblem {
mapping::sparse_pose_graph::proto::OptimizationProblemOptions options_;
std::vector<std::deque<sensor::ImuData>> imu_data_;
std::vector<std::deque<NodeData>> node_data_;
std::vector<transform::TransformInterpolationBuffer> odometry_data_;
std::vector<std::deque<SubmapData>> submap_data_;
std::vector<TrajectoryData> trajectory_data_;
};