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

Pass initial pose of nodes to the 3D optimization problem. (#496) 

Also drops 'point_cloud' from the name since the pose is also
relevant to other sensor data.
master
Wolfgang Hess 2017-09-01 09:34:48 +02:00 committed by GitHub
parent 982f2bd2e0
commit 72bb24e362
7 changed files with 33 additions and 37 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
cartographer/mapping_2d/sparse_pose_graph.cc
View File

@ -194,7 +194,7 @@ void SparsePoseGraph::ComputeConstraint(const mapping::NodeId& node_id,
.at(node_id.trajectory_id)
.at(node_id.node_index - optimization_problem_.num_trimmed_nodes(
node_id.trajectory_id))
.point_cloud_pose;
.pose;
constraint_builder_.MaybeAddConstraint(
submap_id, submap_data_.at(submap_id).submap.get(), node_id,
trajectory_nodes_.at(node_id).constant_data.get(),
@ -444,8 +444,8 @@ void SparsePoseGraph::RunOptimization() {
for (; node_data_index != static_cast<int>(node_data[trajectory_id].size());
++node_data_index, ++node_index) {
const mapping::NodeId node_id{trajectory_id, node_index};
trajectory_nodes_.at(node_id).pose = transform::Embed3D(
node_data[trajectory_id][node_data_index].point_cloud_pose);
trajectory_nodes_.at(node_id).pose =
transform::Embed3D(node_data[trajectory_id][node_data_index].pose);
}
// Extrapolate all point cloud poses that were added later.
const auto local_to_new_global =

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

@ -80,13 +80,11 @@ void OptimizationProblem::AddOdometerData(
void OptimizationProblem::AddTrajectoryNode(
const int trajectory_id, const common::Time time,
const transform::Rigid2d& initial_point_cloud_pose,
const transform::Rigid2d& point_cloud_pose) {
const transform::Rigid2d& initial_pose, const transform::Rigid2d& 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});
node_data_[trajectory_id].push_back(NodeData{time, initial_pose, pose});
trajectory_data_.resize(std::max(trajectory_data_.size(), node_data_.size()));
}
@ -174,7 +172,7 @@ void OptimizationProblem::Solve(const std::vector<Constraint>& constraints,
// stay valid.
C_nodes[trajectory_id].reserve(node_data_[trajectory_id].size());
for (const NodeData& node_data : node_data_[trajectory_id]) {
C_nodes[trajectory_id].push_back(FromPose(node_data.point_cloud_pose));
C_nodes[trajectory_id].push_back(FromPose(node_data.pose));
problem.AddParameterBlock(C_nodes[trajectory_id].back().data(), 3);
if (frozen) {
problem.SetParameterBlockConstant(C_nodes[trajectory_id].back().data());
@ -223,9 +221,8 @@ void OptimizationProblem::Solve(const std::vector<Constraint>& constraints,
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_pose.inverse() *
node_data_[trajectory_id][node_data_index].initial_pose);
problem.AddResidualBlock(
new ceres::AutoDiffCostFunction<SpaCostFunction, 3, 3, 3>(
new SpaCostFunction(Constraint::Pose{
@ -260,7 +257,7 @@ void OptimizationProblem::Solve(const std::vector<Constraint>& constraints,
for (size_t node_data_index = 0;
node_data_index != node_data_[trajectory_id].size();
++node_data_index) {
node_data_[trajectory_id][node_data_index].point_cloud_pose =
node_data_[trajectory_id][node_data_index].pose =
ToPose(C_nodes[trajectory_id][node_data_index]);
}
}

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

@ -39,8 +39,8 @@ namespace sparse_pose_graph {
struct NodeData {
common::Time time;
transform::Rigid2d initial_point_cloud_pose;
transform::Rigid2d point_cloud_pose;
transform::Rigid2d initial_pose;
transform::Rigid2d pose;
};
struct SubmapData {
@ -64,8 +64,8 @@ class OptimizationProblem {
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);
const transform::Rigid2d& initial_pose,
const transform::Rigid2d& pose);
void TrimTrajectoryNode(const mapping::NodeId& node_id);
void AddSubmap(int trajectory_id, const transform::Rigid2d& submap_pose);
void TrimSubmap(const mapping::SubmapId& submap_id);

8
cartographer/mapping_3d/sparse_pose_graph.cc
View File

@ -180,7 +180,7 @@ void SparsePoseGraph::ComputeConstraint(const mapping::NodeId& node_id,
inverse_submap_pose * optimization_problem_.node_data()
.at(node_id.trajectory_id)
.at(node_id.node_index)
.point_cloud_pose;
.pose;
std::vector<mapping::TrajectoryNode> submap_nodes;
for (const mapping::NodeId& submap_node_id :
@ -265,8 +265,8 @@ void SparsePoseGraph::ComputeConstraintsForScan(
.size())
: 0};
const auto& scan_data = trajectory_nodes_.at(node_id).constant_data;
optimization_problem_.AddTrajectoryNode(matching_id.trajectory_id,
scan_data->time, optimized_pose);
optimization_problem_.AddTrajectoryNode(
matching_id.trajectory_id, scan_data->time, pose, optimized_pose);
for (size_t i = 0; i < insertion_submaps.size(); ++i) {
const mapping::SubmapId submap_id = submap_ids[i];
// Even if this was the last scan added to 'submap_id', the submap will only
@ -481,7 +481,7 @@ void SparsePoseGraph::RunOptimization() {
++node_index) {
const mapping::NodeId node_id{trajectory_id, node_index};
trajectory_nodes_.at(node_id).pose =
node_data[trajectory_id][node_index].point_cloud_pose;
node_data[trajectory_id][node_index].pose;
}
// Extrapolate all point cloud poses that were added later.
const auto local_to_new_global = ComputeLocalToGlobalTransform(

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

@ -73,11 +73,11 @@ void OptimizationProblem::AddFixedFramePoseData(
void OptimizationProblem::AddTrajectoryNode(
const int trajectory_id, const common::Time time,
const transform::Rigid3d& point_cloud_pose) {
const transform::Rigid3d& initial_pose, const transform::Rigid3d& 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});
node_data_[trajectory_id].push_back(NodeData{time, initial_pose, pose});
}
void OptimizationProblem::AddSubmap(const int trajectory_id,
@ -151,7 +151,7 @@ void OptimizationProblem::Solve(const std::vector<Constraint>& constraints,
for (size_t node_index = 0; node_index != node_data_[trajectory_id].size();
++node_index) {
C_nodes[trajectory_id].emplace_back(
node_data_[trajectory_id][node_index].point_cloud_pose,
node_data_[trajectory_id][node_index].pose,
translation_parameterization(),
common::make_unique<ceres::QuaternionParameterization>(), &problem);
if (frozen) {
@ -284,8 +284,7 @@ void OptimizationProblem::Solve(const std::vector<Constraint>& constraints,
if (!fixed_frame_pose_initialized) {
const transform::Rigid3d fixed_frame_pose_in_map =
node_data[node_index].point_cloud_pose *
constraint_pose.zbar_ij.inverse();
node_data[node_index].pose * constraint_pose.zbar_ij.inverse();
C_fixed_frames.emplace_back(
transform::Rigid3d(
fixed_frame_pose_in_map.translation(),
@ -347,7 +346,7 @@ void OptimizationProblem::Solve(const std::vector<Constraint>& constraints,
++trajectory_id) {
for (size_t node_index = 0; node_index != node_data_[trajectory_id].size();
++node_index) {
node_data_[trajectory_id][node_index].point_cloud_pose =
node_data_[trajectory_id][node_index].pose =
C_nodes[trajectory_id][node_index].ToRigid();
}
}

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

@ -39,7 +39,8 @@ namespace sparse_pose_graph {
struct NodeData {
common::Time time;
transform::Rigid3d point_cloud_pose;
transform::Rigid3d initial_pose;
transform::Rigid3d pose;
};
struct SubmapData {
@ -67,7 +68,8 @@ class OptimizationProblem {
int trajectory_id,
const sensor::FixedFramePoseData& fixed_frame_pose_data);
void AddTrajectoryNode(int trajectory_id, common::Time time,
const transform::Rigid3d& point_cloud_pose);
const transform::Rigid3d& initial_pose,
const transform::Rigid3d& pose);
void AddSubmap(int trajectory_id, const transform::Rigid3d& submap_pose);
void SetMaxNumIterations(int32 max_num_iterations);

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

@ -127,7 +127,7 @@ TEST_F(OptimizationProblemTest, ReducesNoise) {
optimization_problem_.AddImuData(
kTrajectoryId, sensor::ImuData{now, Eigen::Vector3d::UnitZ() * 9.81,
Eigen::Vector3d::Zero()});
optimization_problem_.AddTrajectoryNode(kTrajectoryId, now, pose);
optimization_problem_.AddTrajectoryNode(kTrajectoryId, now, pose, pose);
now += common::FromSeconds(0.01);
}
@ -159,11 +159,10 @@ TEST_F(OptimizationProblemTest, ReducesNoise) {
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())
node_data[j].pose.translation())
.norm();
rotation_error_before +=
transform::GetAngle(test_data[j].ground_truth_pose.inverse() *
node_data[j].point_cloud_pose);
rotation_error_before += transform::GetAngle(
test_data[j].ground_truth_pose.inverse() * node_data[j].pose);
}
optimization_problem_.AddSubmap(kTrajectoryId, kSubmap0Transform);
@ -176,11 +175,10 @@ TEST_F(OptimizationProblemTest, ReducesNoise) {
double rotation_error_after = 0.;
for (int j = 0; j != kNumNodes; ++j) {
translation_error_after += (test_data[j].ground_truth_pose.translation() -
node_data[j].point_cloud_pose.translation())
node_data[j].pose.translation())
.norm();
rotation_error_after +=
transform::GetAngle(test_data[j].ground_truth_pose.inverse() *
node_data[j].point_cloud_pose);
rotation_error_after += transform::GetAngle(
test_data[j].ground_truth_pose.inverse() * node_data[j].pose);
}
EXPECT_GT(0.8 * translation_error_before, translation_error_after);