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

Keep node data per trajectory in the optimization problem. (#282) 

Related to #256.

PAIR=SirVer
master
Wolfgang Hess 2017-05-15 14:09:45 +02:00 committed by GitHub
parent c084624958
commit 7d43eaa08c
7 changed files with 120 additions and 139 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

55
cartographer/mapping_2d/sparse_pose_graph.cc
View File

@ -102,9 +102,6 @@ void SparsePoseGraph::AddScan(
const int trajectory_id = trajectory_ids_.at(trajectory);
const int flat_scan_index = trajectory_nodes_.size();
CHECK_LT(flat_scan_index, std::numeric_limits<int>::max());
scan_index_to_node_id_.push_back(
mapping::NodeId{trajectory_id, num_nodes_in_trajectory_[trajectory_id]});
++num_nodes_in_trajectory_[trajectory_id];
constant_node_data_.push_back(mapping::TrajectoryNode::ConstantData{
time, range_data_in_pose,
@ -167,12 +164,15 @@ void SparsePoseGraph::AddImuData(const mapping::Submaps* trajectory,
void SparsePoseGraph::ComputeConstraint(const int scan_index,
const mapping::SubmapId& submap_id) {
const transform::Rigid2d relative_pose =
optimization_problem_.submap_data()
.at(submap_id.trajectory_id)
.at(submap_id.submap_index)
.pose.inverse() *
optimization_problem_.node_data().at(scan_index).point_cloud_pose;
const mapping::NodeId node_id = scan_index_to_node_id_.at(scan_index);
const transform::Rigid2d relative_pose = optimization_problem_.submap_data()
.at(submap_id.trajectory_id)
.at(submap_id.submap_index)
.pose.inverse() *
optimization_problem_.node_data()
.at(node_id.trajectory_id)
.at(node_id.node_index)
.point_cloud_pose;
const mapping::Submaps* const scan_trajectory =
trajectory_nodes_[scan_index].constant_data->trajectory;
@ -186,8 +186,7 @@ void SparsePoseGraph::ComputeConstraint(const int scan_index,
submap_states_.at(submap_id.trajectory_id)
.at(submap_id.submap_index)
.submap,
scan_index_to_node_id_.at(scan_index), scan_index,
&trajectory_connectivity_,
node_id, scan_index, &trajectory_connectivity_,
&trajectory_nodes_[scan_index].constant_data->range_data_2d.returns);
} else {
const bool scan_and_submap_trajectories_connected =
@ -202,7 +201,7 @@ void SparsePoseGraph::ComputeConstraint(const int scan_index,
submap_states_.at(submap_id.trajectory_id)
.at(submap_id.submap_index)
.submap,
scan_index_to_node_id_.at(scan_index), scan_index,
node_id, scan_index,
&trajectory_nodes_[scan_index].constant_data->range_data_2d.returns,
relative_pose);
}
@ -212,14 +211,11 @@ void SparsePoseGraph::ComputeConstraint(const int scan_index,
void SparsePoseGraph::ComputeConstraintsForOldScans(
const mapping::Submap* submap) {
const auto submap_id = GetSubmapId(submap);
const auto& node_data = optimization_problem_.node_data();
CHECK_GT(node_data.size(), 0);
CHECK_LT(node_data.size(), std::numeric_limits<int>::max());
const int num_nodes = node_data.size();
const auto& submap_state =
submap_states_.at(submap_id.trajectory_id).at(submap_id.submap_index);
const int num_nodes = scan_index_to_node_id_.size();
for (int scan_index = 0; scan_index < num_nodes; ++scan_index) {
if (submap_states_.at(submap_id.trajectory_id)
.at(submap_id.submap_index)
.scan_indices.count(scan_index) == 0) {
if (submap_state.scan_indices.count(scan_index) == 0) {
ComputeConstraint(scan_index, submap_id);
}
}
@ -238,12 +234,17 @@ void SparsePoseGraph::ComputeConstraintsForScan(
.at(matching_id.submap_index)
.pose *
sparse_pose_graph::ComputeSubmapPose(*matching_submap).inverse() * pose;
CHECK_EQ(scan_index, optimization_problem_.node_data().size());
CHECK_EQ(scan_index, scan_index_to_node_id_.size());
scan_index_to_node_id_.push_back(
mapping::NodeId{matching_id.trajectory_id,
num_nodes_in_trajectory_[matching_id.trajectory_id]});
++num_nodes_in_trajectory_[matching_id.trajectory_id];
const mapping::TrajectoryNode::ConstantData* const scan_data =
trajectory_nodes_[scan_index].constant_data;
CHECK_EQ(trajectory_ids_.at(scan_data->trajectory),
matching_id.trajectory_id);
optimization_problem_.AddTrajectoryNode(
trajectory_ids_.at(scan_data->trajectory), scan_data->time, pose,
optimized_pose);
matching_id.trajectory_id, scan_data->time, pose, optimized_pose);
for (const mapping::Submap* submap : insertion_submaps) {
const mapping::SubmapId submap_id = GetSubmapId(submap);
CHECK(!submap_states_.at(submap_id.trajectory_id)
@ -385,10 +386,12 @@ void SparsePoseGraph::RunOptimization() {
common::MutexLocker locker(&mutex_);
const auto& node_data = optimization_problem_.node_data();
const size_t num_optimized_poses = node_data.size();
const size_t num_optimized_poses = scan_index_to_node_id_.size();
for (size_t i = 0; i != num_optimized_poses; ++i) {
trajectory_nodes_[i].pose =
transform::Rigid3d(transform::Embed3D(node_data[i].point_cloud_pose));
transform::Embed3D(node_data.at(scan_index_to_node_id_[i].trajectory_id)
.at(scan_index_to_node_id_[i].node_index)
.point_cloud_pose);
}
// Extrapolate all point cloud poses that were added later.
std::unordered_map<const mapping::Submaps*, transform::Rigid3d>
@ -478,8 +481,8 @@ std::vector<transform::Rigid3d> SparsePoseGraph::ExtrapolateSubmapTransforms(
} else if (result.empty()) {
result.push_back(transform::Rigid3d::Identity());
} else {
// Extrapolate to the remaining submaps. Accessing local_pose() in
// Submaps is okay, since the member is const.
// Extrapolate to the remaining submaps. Accessing local_pose() in Submaps
// is okay, since the member is const.
result.push_back(result.back() *
submap_states_.at(trajectory_id)
.at(result.size() - 1)

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

@ -66,7 +66,7 @@ void OptimizationProblem::AddImuData(const int trajectory_id,
const Eigen::Vector3d& angular_velocity) {
CHECK_GE(trajectory_id, 0);
imu_data_.resize(
std::max(submap_data_.size(), static_cast<size_t>(trajectory_id) + 1));
std::max(imu_data_.size(), static_cast<size_t>(trajectory_id) + 1));
imu_data_[trajectory_id].push_back(
mapping_3d::ImuData{time, linear_acceleration, angular_velocity});
}
@ -75,8 +75,11 @@ void OptimizationProblem::AddTrajectoryNode(
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_id, time, initial_point_cloud_pose,
point_cloud_pose});
CHECK_GE(trajectory_id, 0);
node_data_.resize(
std::max(node_data_.size(), static_cast<size_t>(trajectory_id) + 1));
node_data_[trajectory_id].push_back(
NodeData{time, initial_point_cloud_pose, point_cloud_pose});
}
void OptimizationProblem::AddSubmap(const int trajectory_id,
@ -98,20 +101,13 @@ void OptimizationProblem::Solve(const std::vector<Constraint>& constraints) {
return;
}
// Compute the indices of consecutive nodes for each 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.at(node_data_[j].trajectory_id).push_back(j);
}
ceres::Problem::Options problem_options;
ceres::Problem problem(problem_options);
// Set the starting point.
// TODO(hrapp): Move ceres data into SubmapData.
std::vector<std::deque<std::array<double, 3>>> C_submaps(submap_data_.size());
std::vector<std::deque<std::array<double, 3>>> C_nodes(
nodes_per_trajectory.size());
std::vector<std::deque<std::array<double, 3>>> C_nodes(node_data_.size());
for (size_t trajectory_id = 0; trajectory_id != submap_data_.size();
++trajectory_id) {
for (size_t submap_index = 0;
@ -130,15 +126,13 @@ void OptimizationProblem::Solve(const std::vector<Constraint>& constraints) {
}
}
}
for (size_t trajectory_id = 0; trajectory_id != nodes_per_trajectory.size();
for (size_t trajectory_id = 0; trajectory_id != node_data_.size();
++trajectory_id) {
C_nodes[trajectory_id].resize(nodes_per_trajectory[trajectory_id].size());
for (size_t node_index = 0;
node_index != nodes_per_trajectory[trajectory_id].size();
C_nodes[trajectory_id].resize(node_data_[trajectory_id].size());
for (size_t node_index = 0; node_index != node_data_[trajectory_id].size();
++node_index) {
C_nodes[trajectory_id][node_index] =
FromPose(node_data_[nodes_per_trajectory[trajectory_id][node_index]]
.point_cloud_pose);
FromPose(node_data_[trajectory_id][node_index].point_cloud_pose);
problem.AddParameterBlock(C_nodes[trajectory_id][node_index].data(), 3);
}
}
@ -166,28 +160,22 @@ void OptimizationProblem::Solve(const std::vector<Constraint>& constraints) {
options_.consecutive_scan_translation_penalty_factor(),
options_.consecutive_scan_rotation_penalty_factor());
for (size_t trajectory_id = 0; trajectory_id != nodes_per_trajectory.size();
for (size_t trajectory_id = 0; trajectory_id != node_data_.size();
++trajectory_id) {
if (nodes_per_trajectory[trajectory_id].empty()) {
continue;
}
for (size_t node_index = 1;
node_index != nodes_per_trajectory[trajectory_id].size();
for (size_t node_index = 1; node_index < node_data_[trajectory_id].size();
++node_index) {
constexpr double kUnusedPositionPenalty = 1.;
constexpr double kUnusedOrientationPenalty = 1.;
problem.AddResidualBlock(
new ceres::AutoDiffCostFunction<
SpaCostFunction, 3, 3, 3>(new SpaCostFunction(Constraint::Pose{
transform::Embed3D(
node_data_[nodes_per_trajectory[trajectory_id]
[node_index - 1]]
.initial_point_cloud_pose.inverse() *
node_data_[nodes_per_trajectory[trajectory_id][node_index]]
.initial_point_cloud_pose),
kalman_filter::Embed3D(consecutive_pose_change_penalty_matrix,
kUnusedPositionPenalty,
kUnusedOrientationPenalty)})),
new ceres::AutoDiffCostFunction<SpaCostFunction, 3, 3, 3>(
new SpaCostFunction(Constraint::Pose{
transform::Embed3D(node_data_[trajectory_id][node_index - 1]
.initial_point_cloud_pose.inverse() *
node_data_[trajectory_id][node_index]
.initial_point_cloud_pose),
kalman_filter::Embed3D(consecutive_pose_change_penalty_matrix,
kUnusedPositionPenalty,
kUnusedOrientationPenalty)})),
nullptr /* loss function */,
C_nodes[trajectory_id][node_index - 1].data(),
C_nodes[trajectory_id][node_index].data());
@ -213,18 +201,18 @@ void OptimizationProblem::Solve(const std::vector<Constraint>& constraints) {
ToPose(C_submaps[trajectory_id][submap_index]);
}
}
for (size_t trajectory_id = 0; trajectory_id != nodes_per_trajectory.size();
for (size_t trajectory_id = 0; trajectory_id != node_data_.size();
++trajectory_id) {
for (size_t node_index = 0;
node_index != nodes_per_trajectory[trajectory_id].size();
for (size_t node_index = 0; node_index != node_data_[trajectory_id].size();
++node_index) {
node_data_[nodes_per_trajectory[trajectory_id][node_index]]
.point_cloud_pose = ToPose(C_nodes[trajectory_id][node_index]);
node_data_[trajectory_id][node_index].point_cloud_pose =
ToPose(C_nodes[trajectory_id][node_index]);
}
}
}
const std::vector<NodeData>& OptimizationProblem::node_data() const {
const std::vector<std::vector<NodeData>>& OptimizationProblem::node_data()
const {
return node_data_;
}

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

@ -18,6 +18,7 @@
#define CARTOGRAPHER_MAPPING_2D_SPARSE_POSE_GRAPH_OPTIMIZATION_PROBLEM_H_
#include <array>
#include <deque>
#include <map>
#include <vector>
@ -27,7 +28,6 @@
#include "cartographer/common/time.h"
#include "cartographer/mapping/sparse_pose_graph.h"
#include "cartographer/mapping/sparse_pose_graph/proto/optimization_problem_options.pb.h"
#include "cartographer/mapping_2d/submaps.h"
#include "cartographer/mapping_3d/imu_integration.h"
namespace cartographer {
@ -35,8 +35,6 @@ namespace mapping_2d {
namespace sparse_pose_graph {
struct NodeData {
// TODO(whess): Keep nodes per trajectory instead.
int trajectory_id;
common::Time time;
transform::Rigid2d initial_point_cloud_pose;
transform::Rigid2d point_cloud_pose;
@ -72,13 +70,13 @@ class OptimizationProblem {
// Computes the optimized poses.
void Solve(const std::vector<Constraint>& constraints);
const std::vector<NodeData>& node_data() const;
const std::vector<std::vector<NodeData>>& node_data() const;
const std::vector<std::vector<SubmapData>>& submap_data() const;
private:
mapping::sparse_pose_graph::proto::OptimizationProblemOptions options_;
std::vector<std::deque<mapping_3d::ImuData>> imu_data_;
std::vector<NodeData> node_data_;
std::vector<std::vector<NodeData>> node_data_;
std::vector<std::vector<SubmapData>> submap_data_;
};

56
cartographer/mapping_3d/sparse_pose_graph.cc
View File

@ -100,9 +100,6 @@ void SparsePoseGraph::AddScan(
const int trajectory_id = trajectory_ids_.at(trajectory);
const int flat_scan_index = trajectory_nodes_.size();
CHECK_LT(flat_scan_index, std::numeric_limits<int>::max());
scan_index_to_node_id_.push_back(
mapping::NodeId{trajectory_id, num_nodes_in_trajectory_[trajectory_id]});
++num_nodes_in_trajectory_[trajectory_id];
constant_node_data_.push_back(mapping::TrajectoryNode::ConstantData{
time, sensor::RangeData{Eigen::Vector3f::Zero(), {}, {}},
@ -167,12 +164,15 @@ void SparsePoseGraph::AddImuData(const mapping::Submaps* trajectory,
void SparsePoseGraph::ComputeConstraint(const int scan_index,
const mapping::SubmapId& submap_id) {
const transform::Rigid3d relative_pose =
optimization_problem_.submap_data()
.at(submap_id.trajectory_id)
.at(submap_id.submap_index)
.pose.inverse() *
optimization_problem_.node_data().at(scan_index).point_cloud_pose;
const mapping::NodeId node_id = scan_index_to_node_id_.at(scan_index);
const transform::Rigid3d relative_pose = optimization_problem_.submap_data()
.at(submap_id.trajectory_id)
.at(submap_id.submap_index)
.pose.inverse() *
optimization_problem_.node_data()
.at(node_id.trajectory_id)
.at(node_id.node_index)
.point_cloud_pose;
const mapping::Submaps* const scan_trajectory =
trajectory_nodes_[scan_index].constant_data->trajectory;
@ -186,8 +186,7 @@ void SparsePoseGraph::ComputeConstraint(const int scan_index,
submap_states_.at(submap_id.trajectory_id)
.at(submap_id.submap_index)
.submap,
scan_index_to_node_id_.at(scan_index), scan_index,
&trajectory_connectivity_, trajectory_nodes_);
node_id, scan_index, &trajectory_connectivity_, trajectory_nodes_);
} else {
const bool scan_and_submap_trajectories_connected =
reverse_connected_components_.count(scan_trajectory_id) > 0 &&
@ -201,22 +200,18 @@ void SparsePoseGraph::ComputeConstraint(const int scan_index,
submap_states_.at(submap_id.trajectory_id)
.at(submap_id.submap_index)
.submap,
scan_index_to_node_id_.at(scan_index), scan_index, trajectory_nodes_,
relative_pose);
node_id, scan_index, trajectory_nodes_, relative_pose);
}
}
}
void SparsePoseGraph::ComputeConstraintsForOldScans(const Submap* submap) {
const auto submap_id = GetSubmapId(submap);
const auto& node_data = optimization_problem_.node_data();
CHECK_GT(node_data.size(), 0);
CHECK_LT(node_data.size(), std::numeric_limits<int>::max());
const int num_nodes = node_data.size();
const auto& submap_state =
submap_states_.at(submap_id.trajectory_id).at(submap_id.submap_index);
const int num_nodes = scan_index_to_node_id_.size();
for (int scan_index = 0; scan_index < num_nodes; ++scan_index) {
if (submap_states_.at(submap_id.trajectory_id)
.at(submap_id.submap_index)
.scan_indices.count(scan_index) == 0) {
if (submap_state.scan_indices.count(scan_index) == 0) {
ComputeConstraint(scan_index, submap_id);
}
}
@ -235,12 +230,17 @@ void SparsePoseGraph::ComputeConstraintsForScan(
.at(matching_id.submap_index)
.pose *
matching_submap->local_pose().inverse() * pose;
CHECK_EQ(scan_index, optimization_problem_.node_data().size());
CHECK_EQ(scan_index, scan_index_to_node_id_.size());
scan_index_to_node_id_.push_back(
mapping::NodeId{matching_id.trajectory_id,
num_nodes_in_trajectory_[matching_id.trajectory_id]});
++num_nodes_in_trajectory_[matching_id.trajectory_id];
const mapping::TrajectoryNode::ConstantData* const scan_data =
trajectory_nodes_[scan_index].constant_data;
optimization_problem_.AddTrajectoryNode(
trajectory_ids_.at(scan_data->trajectory), scan_data->time,
optimized_pose);
CHECK_EQ(trajectory_ids_.at(scan_data->trajectory),
matching_id.trajectory_id);
optimization_problem_.AddTrajectoryNode(matching_id.trajectory_id,
scan_data->time, optimized_pose);
for (const Submap* submap : insertion_submaps) {
const mapping::SubmapId submap_id = GetSubmapId(submap);
CHECK(!submap_states_.at(submap_id.trajectory_id)
@ -378,9 +378,12 @@ void SparsePoseGraph::RunOptimization() {
common::MutexLocker locker(&mutex_);
const auto& node_data = optimization_problem_.node_data();
const size_t num_optimized_poses = node_data.size();
const size_t num_optimized_poses = scan_index_to_node_id_.size();
for (size_t i = 0; i != num_optimized_poses; ++i) {
trajectory_nodes_[i].pose = node_data[i].point_cloud_pose;
trajectory_nodes_[i].pose =
node_data.at(scan_index_to_node_id_[i].trajectory_id)
.at(scan_index_to_node_id_[i].node_index)
.point_cloud_pose;
}
// Extrapolate all point cloud poses that were added later.
std::unordered_map<const mapping::Submaps*, transform::Rigid3d>
@ -459,6 +462,7 @@ std::vector<transform::Rigid3d> SparsePoseGraph::ExtrapolateSubmapTransforms(
return {transform::Rigid3d::Identity()};
}
// Submaps for which we have optimized poses.
std::vector<transform::Rigid3d> result;
for (const auto& state : submap_states_.at(trajectory_id)) {
if (trajectory_id < submap_transforms.size() &&

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

@ -84,7 +84,7 @@ void OptimizationProblem::AddImuData(const int trajectory_id,
const Eigen::Vector3d& angular_velocity) {
CHECK_GE(trajectory_id, 0);
imu_data_.resize(
std::max(submap_data_.size(), static_cast<size_t>(trajectory_id) + 1));
std::max(imu_data_.size(), static_cast<size_t>(trajectory_id) + 1));
imu_data_[trajectory_id].push_back(
ImuData{time, linear_acceleration, angular_velocity});
}
@ -92,7 +92,10 @@ void OptimizationProblem::AddImuData(const int trajectory_id,
void OptimizationProblem::AddTrajectoryNode(
const int trajectory_id, const common::Time time,
const transform::Rigid3d& point_cloud_pose) {
node_data_.push_back(NodeData{trajectory_id, time, point_cloud_pose});
CHECK_GE(trajectory_id, 0);
node_data_.resize(
std::max(node_data_.size(), static_cast<size_t>(trajectory_id) + 1));
node_data_[trajectory_id].push_back(NodeData{time, point_cloud_pose});
}
void OptimizationProblem::AddSubmap(const int trajectory_id,
@ -114,12 +117,6 @@ void OptimizationProblem::Solve(const std::vector<Constraint>& constraints) {
return;
}
// Compute the indices of consecutive nodes for each 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.at(node_data_[j].trajectory_id).push_back(j);
}
ceres::Problem::Options problem_options;
ceres::Problem problem(problem_options);
@ -136,7 +133,7 @@ void OptimizationProblem::Solve(const std::vector<Constraint>& constraints) {
CHECK(!submap_data_[0].empty());
// TODO(hrapp): Move ceres data into SubmapData.
std::vector<std::deque<CeresPose>> C_submaps(submap_data_.size());
std::vector<std::deque<CeresPose>> C_nodes(nodes_per_trajectory.size());
std::vector<std::deque<CeresPose>> C_nodes(node_data_.size());
for (size_t trajectory_id = 0; trajectory_id != submap_data_.size();
++trajectory_id) {
for (size_t submap_index = 0;
@ -158,14 +155,12 @@ void OptimizationProblem::Solve(const std::vector<Constraint>& constraints) {
}
}
}
for (size_t trajectory_id = 0; trajectory_id != nodes_per_trajectory.size();
for (size_t trajectory_id = 0; trajectory_id != node_data_.size();
++trajectory_id) {
for (size_t node_index = 0;
node_index != nodes_per_trajectory[trajectory_id].size();
for (size_t node_index = 0; node_index != node_data_[trajectory_id].size();
++node_index) {
C_nodes[trajectory_id].emplace_back(
node_data_[nodes_per_trajectory[trajectory_id][node_index]]
.point_cloud_pose,
node_data_[trajectory_id][node_index].point_cloud_pose,
translation_parameterization(),
common::make_unique<ceres::QuaternionParameterization>(), &problem);
}
@ -196,34 +191,29 @@ void OptimizationProblem::Solve(const std::vector<Constraint>& constraints) {
// Add constraints based on IMU observations of angular velocities and
// linear acceleration.
for (size_t trajectory_id = 0; trajectory_id != nodes_per_trajectory.size();
for (size_t trajectory_id = 0; trajectory_id != node_data_.size();
++trajectory_id) {
const std::vector<size_t>& flat_indices =
nodes_per_trajectory[trajectory_id];
const std::deque<ImuData>& imu_data = imu_data_.at(trajectory_id);
CHECK(!flat_indices.empty());
CHECK(!imu_data.empty());
// TODO(whess): Add support for empty trajectories.
const auto& node_data = node_data_[trajectory_id];
CHECK(!node_data.empty());
// Skip IMU data before the first node of this trajectory.
auto it = imu_data.cbegin();
while ((it + 1) != imu_data.cend() &&
(it + 1)->time <= node_data_[flat_indices[0]].time) {
while ((it + 1) != imu_data.cend() && (it + 1)->time <= node_data[0].time) {
++it;
}
for (size_t node_index = 1; node_index < flat_indices.size();
++node_index) {
for (size_t node_index = 1; node_index < node_data.size(); ++node_index) {
auto it2 = it;
const IntegrateImuResult<double> result =
IntegrateImu(imu_data, node_data_[flat_indices[node_index - 1]].time,
node_data_[flat_indices[node_index]].time, &it);
if (node_index + 1 < flat_indices.size()) {
const common::Time first_time =
node_data_[flat_indices[node_index - 1]].time;
const common::Time second_time =
node_data_[flat_indices[node_index]].time;
const common::Time third_time =
node_data_[flat_indices[node_index + 1]].time;
IntegrateImu(imu_data, node_data[node_index - 1].time,
node_data[node_index].time, &it);
if (node_index + 1 < node_data.size()) {
const common::Time first_time = node_data[node_index - 1].time;
const common::Time second_time = node_data[node_index].time;
const common::Time third_time = node_data[node_index + 1].time;
const common::Duration first_duration = second_time - first_time;
const common::Duration second_duration = third_time - second_time;
const common::Time first_center = first_time + first_duration / 2;
@ -283,18 +273,18 @@ void OptimizationProblem::Solve(const std::vector<Constraint>& constraints) {
C_submaps[trajectory_id][submap_index].ToRigid();
}
}
for (size_t trajectory_id = 0; trajectory_id != nodes_per_trajectory.size();
for (size_t trajectory_id = 0; trajectory_id != node_data_.size();
++trajectory_id) {
for (size_t node_index = 0;
node_index != nodes_per_trajectory[trajectory_id].size();
for (size_t node_index = 0; node_index != node_data_[trajectory_id].size();
++node_index) {
node_data_[nodes_per_trajectory[trajectory_id][node_index]]
.point_cloud_pose = C_nodes[trajectory_id][node_index].ToRigid();
node_data_[trajectory_id][node_index].point_cloud_pose =
C_nodes[trajectory_id][node_index].ToRigid();
}
}
}
const std::vector<NodeData>& OptimizationProblem::node_data() const {
const std::vector<std::vector<NodeData>>& OptimizationProblem::node_data()
const {
return node_data_;
}

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

@ -35,8 +35,6 @@ namespace mapping_3d {
namespace sparse_pose_graph {
struct NodeData {
// TODO(whess): Keep nodes per trajectory instead.
int trajectory_id;
common::Time time;
transform::Rigid3d point_cloud_pose;
};
@ -73,14 +71,14 @@ class OptimizationProblem {
// Computes the optimized poses.
void Solve(const std::vector<Constraint>& constraints);
const std::vector<NodeData>& node_data() const;
const std::vector<std::vector<NodeData>>& node_data() const;
const std::vector<std::vector<SubmapData>>& submap_data() const;
private:
mapping::sparse_pose_graph::proto::OptimizationProblemOptions options_;
FixZ fix_z_;
std::vector<std::deque<ImuData>> imu_data_;
std::vector<NodeData> node_data_;
std::vector<std::vector<NodeData>> node_data_;
std::vector<std::vector<SubmapData>> submap_data_;
double gravity_constant_ = 9.8;
};

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

@ -155,7 +155,7 @@ TEST_F(OptimizationProblemTest, ReducesNoise) {
double translation_error_before = 0.;
double rotation_error_before = 0.;
const auto& node_data = optimization_problem_.node_data();
const auto& node_data = optimization_problem_.node_data().at(0);
for (int j = 0; j != kNumNodes; ++j) {
translation_error_before += (test_data[j].ground_truth_pose.translation() -
node_data[j].point_cloud_pose.translation())