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

Optimization problem only deals with trajectory IDs now. (#273) 

Related to #256.

PAIR=wohe
master
Holger Rapp 2017-05-10 17:12:38 +02:00 committed by GitHub
parent 926b0320cb
commit 660eb341ff
7 changed files with 50 additions and 41 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
cartographer/mapping_2d/sparse_pose_graph.cc
View File

@ -153,9 +153,10 @@ void SparsePoseGraph::AddImuData(const mapping::Submaps* trajectory,
const Eigen::Vector3d& linear_acceleration,
const Eigen::Vector3d& angular_velocity) {
common::MutexLocker locker(&mutex_);
trajectory_ids_.emplace(trajectory, trajectory_ids_.size());
AddWorkItem([=]() REQUIRES(mutex_) {
optimization_problem_.AddImuData(trajectory, time, linear_acceleration,
angular_velocity);
optimization_problem_.AddImuData(trajectory_ids_.at(trajectory), time,
linear_acceleration, angular_velocity);
});
}
@ -228,7 +229,8 @@ void SparsePoseGraph::ComputeConstraintsForScan(
const mapping::TrajectoryNode::ConstantData* const scan_data =
trajectory_nodes_[scan_index].constant_data;
optimization_problem_.AddTrajectoryNode(
scan_data->trajectory, scan_data->time, pose, optimized_pose);
trajectory_ids_.at(scan_data->trajectory), scan_data->time, pose,
optimized_pose);
for (const mapping::Submap* submap : insertion_submaps) {
const int submap_index = GetSubmapIndex(submap);
CHECK(!submap_states_[submap_index].finished);

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

@ -60,20 +60,23 @@ OptimizationProblem::OptimizationProblem(
OptimizationProblem::~OptimizationProblem() {}
void OptimizationProblem::AddImuData(const mapping::Submaps* const trajectory,
void OptimizationProblem::AddImuData(const int trajectory_id,
const common::Time time,
const Eigen::Vector3d& linear_acceleration,
const Eigen::Vector3d& angular_velocity) {
imu_data_[trajectory].push_back(
CHECK_GE(trajectory_id, 0);
imu_data_.resize(
std::max(submap_data_.size(), static_cast<size_t>(trajectory_id) + 1));
imu_data_[trajectory_id].push_back(
mapping_3d::ImuData{time, linear_acceleration, angular_velocity});
}
void OptimizationProblem::AddTrajectoryNode(
const mapping::Submaps* const trajectory, const common::Time time,
const int trajectory_id, const common::Time time,
const transform::Rigid2d& initial_point_cloud_pose,
const transform::Rigid2d& point_cloud_pose) {
node_data_.push_back(
NodeData{trajectory, time, initial_point_cloud_pose, point_cloud_pose});
node_data_.push_back(NodeData{trajectory_id, time, initial_point_cloud_pose,
point_cloud_pose});
}
void OptimizationProblem::AddSubmap(const int trajectory_id,
@ -150,15 +153,15 @@ void OptimizationProblem::Solve(const std::vector<Constraint>& constraints) {
// The poses in 'node_data_' are interleaved from multiple trajectories
// (although the points from a given trajectory are in time order).
// 'last_pose_indices[trajectory]' is the index of the most-recent pose on
// 'trajectory'.
std::map<const mapping::Submaps*, int> last_pose_indices;
// 'last_pose_indices[trajectory_id]' is the index of the most-recent pose on
// 'trajectory_id'.
std::map<int, int> last_pose_indices;
for (size_t j = 0; j != node_data_.size(); ++j) {
const mapping::Submaps* trajectory = node_data_[j].trajectory;
const int trajectory_id = node_data_[j].trajectory_id;
// This pose has a predecessor.
if (last_pose_indices.count(trajectory) != 0) {
const int last_pose_index = last_pose_indices[trajectory];
if (last_pose_indices.count(trajectory_id) != 0) {
const int last_pose_index = last_pose_indices[trajectory_id];
constexpr double kUnusedPositionPenalty = 1.;
constexpr double kUnusedOrientationPenalty = 1.;
problem.AddResidualBlock(
@ -173,7 +176,7 @@ void OptimizationProblem::Solve(const std::vector<Constraint>& constraints) {
nullptr /* loss function */, C_point_clouds[last_pose_index].data(),
C_point_clouds[j].data());
}
last_pose_indices[trajectory] = j;
last_pose_indices[trajectory_id] = j;
}
// Solve.

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

@ -36,7 +36,7 @@ namespace sparse_pose_graph {
struct NodeData {
// TODO(whess): Keep nodes per trajectory instead.
const mapping::Submaps* trajectory;
int trajectory_id;
common::Time time;
transform::Rigid2d initial_point_cloud_pose;
transform::Rigid2d point_cloud_pose;
@ -59,10 +59,10 @@ class OptimizationProblem {
OptimizationProblem(const OptimizationProblem&) = delete;
OptimizationProblem& operator=(const OptimizationProblem&) = delete;
void AddImuData(const mapping::Submaps* trajectory, common::Time time,
void AddImuData(int trajectory_id, common::Time time,
const Eigen::Vector3d& linear_acceleration,
const Eigen::Vector3d& angular_velocity);
void AddTrajectoryNode(const mapping::Submaps* trajectory, common::Time time,
void AddTrajectoryNode(int trajectory_id, common::Time time,
const transform::Rigid2d& initial_point_cloud_pose,
const transform::Rigid2d& point_cloud_pose);
void AddSubmap(int trajectory_id, const transform::Rigid2d& submap_pose);
@ -77,7 +77,7 @@ class OptimizationProblem {
private:
mapping::sparse_pose_graph::proto::OptimizationProblemOptions options_;
std::map<const mapping::Submaps*, std::deque<mapping_3d::ImuData>> imu_data_;
std::vector<std::deque<mapping_3d::ImuData>> imu_data_;
std::vector<NodeData> node_data_;
std::vector<std::vector<SubmapData>> submap_data_;
};

10
cartographer/mapping_3d/sparse_pose_graph.cc
View File

@ -153,9 +153,10 @@ void SparsePoseGraph::AddImuData(const mapping::Submaps* trajectory,
const Eigen::Vector3d& linear_acceleration,
const Eigen::Vector3d& angular_velocity) {
common::MutexLocker locker(&mutex_);
trajectory_ids_.emplace(trajectory, trajectory_ids_.size());
AddWorkItem([=]() REQUIRES(mutex_) {
optimization_problem_.AddImuData(trajectory, time, linear_acceleration,
angular_velocity);
optimization_problem_.AddImuData(trajectory_ids_.at(trajectory), time,
linear_acceleration, angular_velocity);
});
}
@ -225,8 +226,9 @@ void SparsePoseGraph::ComputeConstraintsForScan(
CHECK_EQ(scan_index, optimization_problem_.node_data().size());
const mapping::TrajectoryNode::ConstantData* const scan_data =
trajectory_nodes_[scan_index].constant_data;
optimization_problem_.AddTrajectoryNode(scan_data->trajectory,
scan_data->time, optimized_pose);
optimization_problem_.AddTrajectoryNode(
trajectory_ids_.at(scan_data->trajectory), scan_data->time,
optimized_pose);
for (const Submap* submap : insertion_submaps) {
const int submap_index = GetSubmapIndex(submap);
CHECK(!submap_states_[submap_index].finished);

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

@ -78,18 +78,21 @@ OptimizationProblem::OptimizationProblem(
OptimizationProblem::~OptimizationProblem() {}
void OptimizationProblem::AddImuData(const mapping::Submaps* const trajectory,
void OptimizationProblem::AddImuData(const int trajectory_id,
const common::Time time,
const Eigen::Vector3d& linear_acceleration,
const Eigen::Vector3d& angular_velocity) {
imu_data_[trajectory].push_back(
CHECK_GE(trajectory_id, 0);
imu_data_.resize(
std::max(submap_data_.size(), static_cast<size_t>(trajectory_id) + 1));
imu_data_[trajectory_id].push_back(
ImuData{time, linear_acceleration, angular_velocity});
}
void OptimizationProblem::AddTrajectoryNode(
const mapping::Submaps* const trajectory, const common::Time time,
const int trajectory_id, const common::Time time,
const transform::Rigid3d& point_cloud_pose) {
node_data_.push_back(NodeData{trajectory, time, point_cloud_pose});
node_data_.push_back(NodeData{trajectory_id, time, point_cloud_pose});
}
void OptimizationProblem::AddSubmap(const int trajectory_id,
@ -112,9 +115,9 @@ void OptimizationProblem::Solve(const std::vector<Constraint>& constraints) {
}
// Compute the indices of consecutive nodes for each trajectory.
std::map<const mapping::Submaps*, std::vector<size_t>> nodes_per_trajectory;
std::vector<std::vector<size_t>> nodes_per_trajectory(submap_data_.size());
for (size_t j = 0; j != node_data_.size(); ++j) {
nodes_per_trajectory[node_data_[j].trajectory].push_back(j);
nodes_per_trajectory.at(node_data_[j].trajectory_id).push_back(j);
}
ceres::Problem::Options problem_options;
@ -184,10 +187,11 @@ void OptimizationProblem::Solve(const std::vector<Constraint>& constraints) {
// Add constraints based on IMU observations of angular velocities and
// linear acceleration.
for (const auto& trajectory_nodes_pair : nodes_per_trajectory) {
const mapping::Submaps* const trajectory = trajectory_nodes_pair.first;
const std::vector<size_t>& node_indices = trajectory_nodes_pair.second;
const std::deque<ImuData>& imu_data = imu_data_.at(trajectory);
for (size_t trajectory_id = 0; trajectory_id != nodes_per_trajectory.size();
++trajectory_id) {
const std::vector<size_t>& node_indices =
nodes_per_trajectory[trajectory_id];
const std::deque<ImuData>& imu_data = imu_data_.at(trajectory_id);
CHECK(!node_indices.empty());
CHECK(!imu_data.empty());

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

@ -29,7 +29,6 @@
#include "cartographer/mapping/sparse_pose_graph.h"
#include "cartographer/mapping/sparse_pose_graph/proto/optimization_problem_options.pb.h"
#include "cartographer/mapping_3d/imu_integration.h"
#include "cartographer/mapping_3d/submaps.h"
namespace cartographer {
namespace mapping_3d {
@ -37,7 +36,7 @@ namespace sparse_pose_graph {
struct NodeData {
// TODO(whess): Keep nodes per trajectory instead.
const mapping::Submaps* trajectory;
int trajectory_id;
common::Time time;
transform::Rigid3d point_cloud_pose;
};
@ -62,10 +61,10 @@ class OptimizationProblem {
OptimizationProblem(const OptimizationProblem&) = delete;
OptimizationProblem& operator=(const OptimizationProblem&) = delete;
void AddImuData(const mapping::Submaps* trajectory, common::Time time,
void AddImuData(int trajectory_id, common::Time time,
const Eigen::Vector3d& linear_acceleration,
const Eigen::Vector3d& angular_velocity);
void AddTrajectoryNode(const mapping::Submaps* trajectory, common::Time time,
void AddTrajectoryNode(int trajectory_id, common::Time time,
const transform::Rigid3d& point_cloud_pose);
void AddSubmap(int trajectory_id, const transform::Rigid3d& submap_pose);
@ -80,7 +79,7 @@ class OptimizationProblem {
private:
mapping::sparse_pose_graph::proto::OptimizationProblemOptions options_;
FixZ fix_z_;
std::map<const mapping::Submaps*, std::deque<ImuData>> imu_data_;
std::vector<std::deque<ImuData>> imu_data_;
std::vector<NodeData> node_data_;
std::vector<std::vector<SubmapData>> submap_data_;
double gravity_constant_ = 9.8;

5
cartographer/mapping_3d/sparse_pose_graph/optimization_problem_test.cc
View File

@ -106,7 +106,6 @@ TEST_F(OptimizationProblemTest, ReducesNoise) {
const transform::Rigid3d kSubmap0Transform = transform::Rigid3d::Identity();
const transform::Rigid3d kSubmap2Transform = transform::Rigid3d::Rotation(
Eigen::AngleAxisd(M_PI, Eigen::Vector3d::UnitZ()));
const mapping::Submaps* const kTrajectory = nullptr;
const int kTrajectoryId = 0;
struct NoisyNode {
@ -123,10 +122,10 @@ TEST_F(OptimizationProblemTest, ReducesNoise) {
for (const NoisyNode& node : test_data) {
const transform::Rigid3d pose =
AddNoise(node.ground_truth_pose, node.noise);
optimization_problem_.AddImuData(kTrajectory, now,
optimization_problem_.AddImuData(kTrajectoryId, now,
Eigen::Vector3d::UnitZ() * 9.81,
Eigen::Vector3d::Zero());
optimization_problem_.AddTrajectoryNode(kTrajectory, now, pose);
optimization_problem_.AddTrajectoryNode(kTrajectoryId, now, pose);
now += common::FromSeconds(0.01);
}