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& linear_acceleration,
const Eigen::Vector3d& angular_velocity) { const Eigen::Vector3d& angular_velocity) {
common::MutexLocker locker(&mutex_); common::MutexLocker locker(&mutex_);
trajectory_ids_.emplace(trajectory, trajectory_ids_.size());
AddWorkItem([=]() REQUIRES(mutex_) { AddWorkItem([=]() REQUIRES(mutex_) {
optimization_problem_.AddImuData(trajectory, time, linear_acceleration, optimization_problem_.AddImuData(trajectory_ids_.at(trajectory), time,
angular_velocity); linear_acceleration, angular_velocity);
}); });
} }
@ -228,7 +229,8 @@ void SparsePoseGraph::ComputeConstraintsForScan(
const mapping::TrajectoryNode::ConstantData* const scan_data = const mapping::TrajectoryNode::ConstantData* const scan_data =
trajectory_nodes_[scan_index].constant_data; trajectory_nodes_[scan_index].constant_data;
optimization_problem_.AddTrajectoryNode( 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) { for (const mapping::Submap* submap : insertion_submaps) {
const int submap_index = GetSubmapIndex(submap); const int submap_index = GetSubmapIndex(submap);
CHECK(!submap_states_[submap_index].finished); 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() {} OptimizationProblem::~OptimizationProblem() {}
void OptimizationProblem::AddImuData(const mapping::Submaps* const trajectory, void OptimizationProblem::AddImuData(const int trajectory_id,
const common::Time time, const common::Time time,
const Eigen::Vector3d& linear_acceleration, const Eigen::Vector3d& linear_acceleration,
const Eigen::Vector3d& angular_velocity) { 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}); mapping_3d::ImuData{time, linear_acceleration, angular_velocity});
} }
void OptimizationProblem::AddTrajectoryNode( 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& initial_point_cloud_pose,
const transform::Rigid2d& point_cloud_pose) { const transform::Rigid2d& point_cloud_pose) {
node_data_.push_back( node_data_.push_back(NodeData{trajectory_id, time, initial_point_cloud_pose,
NodeData{trajectory, time, initial_point_cloud_pose, point_cloud_pose}); point_cloud_pose});
} }
void OptimizationProblem::AddSubmap(const int trajectory_id, 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 // The poses in 'node_data_' are interleaved from multiple trajectories
// (although the points from a given trajectory are in time order). // (although the points from a given trajectory are in time order).
// 'last_pose_indices[trajectory]' is the index of the most-recent pose on // 'last_pose_indices[trajectory_id]' is the index of the most-recent pose on
// 'trajectory'. // 'trajectory_id'.
std::map<const mapping::Submaps*, int> last_pose_indices; std::map<int, int> last_pose_indices;
for (size_t j = 0; j != node_data_.size(); ++j) { 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. // This pose has a predecessor.
if (last_pose_indices.count(trajectory) != 0) { if (last_pose_indices.count(trajectory_id) != 0) {
const int last_pose_index = last_pose_indices[trajectory]; const int last_pose_index = last_pose_indices[trajectory_id];
constexpr double kUnusedPositionPenalty = 1.; constexpr double kUnusedPositionPenalty = 1.;
constexpr double kUnusedOrientationPenalty = 1.; constexpr double kUnusedOrientationPenalty = 1.;
problem.AddResidualBlock( problem.AddResidualBlock(
@ -173,7 +176,7 @@ void OptimizationProblem::Solve(const std::vector<Constraint>& constraints) {
nullptr /* loss function */, C_point_clouds[last_pose_index].data(), nullptr /* loss function */, C_point_clouds[last_pose_index].data(),
C_point_clouds[j].data()); C_point_clouds[j].data());
} }
last_pose_indices[trajectory] = j; last_pose_indices[trajectory_id] = j;
} }
// Solve. // Solve.

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

@ -36,7 +36,7 @@ namespace sparse_pose_graph {
struct NodeData { struct NodeData {
// TODO(whess): Keep nodes per trajectory instead. // TODO(whess): Keep nodes per trajectory instead.
const mapping::Submaps* trajectory; int trajectory_id;
common::Time time; common::Time time;
transform::Rigid2d initial_point_cloud_pose; transform::Rigid2d initial_point_cloud_pose;
transform::Rigid2d point_cloud_pose; transform::Rigid2d point_cloud_pose;
@ -59,10 +59,10 @@ class OptimizationProblem {
OptimizationProblem(const OptimizationProblem&) = delete; OptimizationProblem(const OptimizationProblem&) = delete;
OptimizationProblem& operator=(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& linear_acceleration,
const Eigen::Vector3d& angular_velocity); 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& initial_point_cloud_pose,
const transform::Rigid2d& point_cloud_pose); const transform::Rigid2d& point_cloud_pose);
void AddSubmap(int trajectory_id, const transform::Rigid2d& submap_pose); void AddSubmap(int trajectory_id, const transform::Rigid2d& submap_pose);
@ -77,7 +77,7 @@ class OptimizationProblem {
private: private:
mapping::sparse_pose_graph::proto::OptimizationProblemOptions options_; 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<NodeData> node_data_;
std::vector<std::vector<SubmapData>> submap_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& linear_acceleration,
const Eigen::Vector3d& angular_velocity) { const Eigen::Vector3d& angular_velocity) {
common::MutexLocker locker(&mutex_); common::MutexLocker locker(&mutex_);
trajectory_ids_.emplace(trajectory, trajectory_ids_.size());
AddWorkItem([=]() REQUIRES(mutex_) { AddWorkItem([=]() REQUIRES(mutex_) {
optimization_problem_.AddImuData(trajectory, time, linear_acceleration, optimization_problem_.AddImuData(trajectory_ids_.at(trajectory), time,
angular_velocity); linear_acceleration, angular_velocity);
}); });
} }
@ -225,8 +226,9 @@ void SparsePoseGraph::ComputeConstraintsForScan(
CHECK_EQ(scan_index, optimization_problem_.node_data().size()); CHECK_EQ(scan_index, optimization_problem_.node_data().size());
const mapping::TrajectoryNode::ConstantData* const scan_data = const mapping::TrajectoryNode::ConstantData* const scan_data =
trajectory_nodes_[scan_index].constant_data; trajectory_nodes_[scan_index].constant_data;
optimization_problem_.AddTrajectoryNode(scan_data->trajectory, optimization_problem_.AddTrajectoryNode(
scan_data->time, optimized_pose); trajectory_ids_.at(scan_data->trajectory), scan_data->time,
optimized_pose);
for (const Submap* submap : insertion_submaps) { for (const Submap* submap : insertion_submaps) {
const int submap_index = GetSubmapIndex(submap); const int submap_index = GetSubmapIndex(submap);
CHECK(!submap_states_[submap_index].finished); 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() {} OptimizationProblem::~OptimizationProblem() {}
void OptimizationProblem::AddImuData(const mapping::Submaps* const trajectory, void OptimizationProblem::AddImuData(const int trajectory_id,
const common::Time time, const common::Time time,
const Eigen::Vector3d& linear_acceleration, const Eigen::Vector3d& linear_acceleration,
const Eigen::Vector3d& angular_velocity) { 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}); ImuData{time, linear_acceleration, angular_velocity});
} }
void OptimizationProblem::AddTrajectoryNode( 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) { 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, 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. // 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) { 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; 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 // Add constraints based on IMU observations of angular velocities and
// linear acceleration. // linear acceleration.
for (const auto& trajectory_nodes_pair : nodes_per_trajectory) { for (size_t trajectory_id = 0; trajectory_id != nodes_per_trajectory.size();
const mapping::Submaps* const trajectory = trajectory_nodes_pair.first; ++trajectory_id) {
const std::vector<size_t>& node_indices = trajectory_nodes_pair.second; const std::vector<size_t>& node_indices =
const std::deque<ImuData>& imu_data = imu_data_.at(trajectory); nodes_per_trajectory[trajectory_id];
const std::deque<ImuData>& imu_data = imu_data_.at(trajectory_id);
CHECK(!node_indices.empty()); CHECK(!node_indices.empty());
CHECK(!imu_data.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.h"
#include "cartographer/mapping/sparse_pose_graph/proto/optimization_problem_options.pb.h" #include "cartographer/mapping/sparse_pose_graph/proto/optimization_problem_options.pb.h"
#include "cartographer/mapping_3d/imu_integration.h" #include "cartographer/mapping_3d/imu_integration.h"
#include "cartographer/mapping_3d/submaps.h"
namespace cartographer { namespace cartographer {
namespace mapping_3d { namespace mapping_3d {
@ -37,7 +36,7 @@ namespace sparse_pose_graph {
struct NodeData { struct NodeData {
// TODO(whess): Keep nodes per trajectory instead. // TODO(whess): Keep nodes per trajectory instead.
const mapping::Submaps* trajectory; int trajectory_id;
common::Time time; common::Time time;
transform::Rigid3d point_cloud_pose; transform::Rigid3d point_cloud_pose;
}; };
@ -62,10 +61,10 @@ class OptimizationProblem {
OptimizationProblem(const OptimizationProblem&) = delete; OptimizationProblem(const OptimizationProblem&) = delete;
OptimizationProblem& operator=(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& linear_acceleration,
const Eigen::Vector3d& angular_velocity); 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); const transform::Rigid3d& point_cloud_pose);
void AddSubmap(int trajectory_id, const transform::Rigid3d& submap_pose); void AddSubmap(int trajectory_id, const transform::Rigid3d& submap_pose);
@ -80,7 +79,7 @@ class OptimizationProblem {
private: private:
mapping::sparse_pose_graph::proto::OptimizationProblemOptions options_; mapping::sparse_pose_graph::proto::OptimizationProblemOptions options_;
FixZ fix_z_; 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<NodeData> node_data_;
std::vector<std::vector<SubmapData>> submap_data_; std::vector<std::vector<SubmapData>> submap_data_;
double gravity_constant_ = 9.8; 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 kSubmap0Transform = transform::Rigid3d::Identity();
const transform::Rigid3d kSubmap2Transform = transform::Rigid3d::Rotation( const transform::Rigid3d kSubmap2Transform = transform::Rigid3d::Rotation(
Eigen::AngleAxisd(M_PI, Eigen::Vector3d::UnitZ())); Eigen::AngleAxisd(M_PI, Eigen::Vector3d::UnitZ()));
const mapping::Submaps* const kTrajectory = nullptr;
const int kTrajectoryId = 0; const int kTrajectoryId = 0;
struct NoisyNode { struct NoisyNode {
@ -123,10 +122,10 @@ TEST_F(OptimizationProblemTest, ReducesNoise) {
for (const NoisyNode& node : test_data) { for (const NoisyNode& node : test_data) {
const transform::Rigid3d pose = const transform::Rigid3d pose =
AddNoise(node.ground_truth_pose, node.noise); AddNoise(node.ground_truth_pose, node.noise);
optimization_problem_.AddImuData(kTrajectory, now, optimization_problem_.AddImuData(kTrajectoryId, now,
Eigen::Vector3d::UnitZ() * 9.81, Eigen::Vector3d::UnitZ() * 9.81,
Eigen::Vector3d::Zero()); Eigen::Vector3d::Zero());
optimization_problem_.AddTrajectoryNode(kTrajectory, now, pose); optimization_problem_.AddTrajectoryNode(kTrajectoryId, now, pose);
now += common::FromSeconds(0.01); now += common::FromSeconds(0.01);
} }