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.trajectory_id)
.at(node_id.node_index - optimization_problem_.num_trimmed_nodes( .at(node_id.node_index - optimization_problem_.num_trimmed_nodes(
node_id.trajectory_id)) node_id.trajectory_id))
.point_cloud_pose; .pose;
constraint_builder_.MaybeAddConstraint( constraint_builder_.MaybeAddConstraint(
submap_id, submap_data_.at(submap_id).submap.get(), node_id, submap_id, submap_data_.at(submap_id).submap.get(), node_id,
trajectory_nodes_.at(node_id).constant_data.get(), 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()); for (; node_data_index != static_cast<int>(node_data[trajectory_id].size());
++node_data_index, ++node_index) { ++node_data_index, ++node_index) {
const mapping::NodeId node_id{trajectory_id, node_index}; const mapping::NodeId node_id{trajectory_id, node_index};
trajectory_nodes_.at(node_id).pose = transform::Embed3D( trajectory_nodes_.at(node_id).pose =
node_data[trajectory_id][node_data_index].point_cloud_pose); transform::Embed3D(node_data[trajectory_id][node_data_index].pose);
} }
// Extrapolate all point cloud poses that were added later. // Extrapolate all point cloud poses that were added later.
const auto local_to_new_global = 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( void OptimizationProblem::AddTrajectoryNode(
const int trajectory_id, const common::Time time, const int trajectory_id, const common::Time time,
const transform::Rigid2d& initial_point_cloud_pose, const transform::Rigid2d& initial_pose, const transform::Rigid2d& pose) {
const transform::Rigid2d& point_cloud_pose) {
CHECK_GE(trajectory_id, 0); CHECK_GE(trajectory_id, 0);
node_data_.resize( node_data_.resize(
std::max(node_data_.size(), static_cast<size_t>(trajectory_id) + 1)); std::max(node_data_.size(), static_cast<size_t>(trajectory_id) + 1));
node_data_[trajectory_id].push_back( node_data_[trajectory_id].push_back(NodeData{time, initial_pose, pose});
NodeData{time, initial_point_cloud_pose, point_cloud_pose});
trajectory_data_.resize(std::max(trajectory_data_.size(), node_data_.size())); 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. // stay valid.
C_nodes[trajectory_id].reserve(node_data_[trajectory_id].size()); C_nodes[trajectory_id].reserve(node_data_[trajectory_id].size());
for (const NodeData& node_data : node_data_[trajectory_id]) { 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); problem.AddParameterBlock(C_nodes[trajectory_id].back().data(), 3);
if (frozen) { if (frozen) {
problem.SetParameterBlockConstant(C_nodes[trajectory_id].back().data()); 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) node_data_[trajectory_id][node_data_index].time)
: transform::Embed3D( : transform::Embed3D(
node_data_[trajectory_id][node_data_index - 1] node_data_[trajectory_id][node_data_index - 1]
.initial_point_cloud_pose.inverse() * .initial_pose.inverse() *
node_data_[trajectory_id][node_data_index] node_data_[trajectory_id][node_data_index].initial_pose);
.initial_point_cloud_pose);
problem.AddResidualBlock( problem.AddResidualBlock(
new ceres::AutoDiffCostFunction<SpaCostFunction, 3, 3, 3>( new ceres::AutoDiffCostFunction<SpaCostFunction, 3, 3, 3>(
new SpaCostFunction(Constraint::Pose{ new SpaCostFunction(Constraint::Pose{
@ -260,7 +257,7 @@ void OptimizationProblem::Solve(const std::vector<Constraint>& constraints,
for (size_t node_data_index = 0; for (size_t node_data_index = 0;
node_data_index != node_data_[trajectory_id].size(); node_data_index != node_data_[trajectory_id].size();
++node_data_index) { ++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]); 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 { struct NodeData {
common::Time time; common::Time time;
transform::Rigid2d initial_point_cloud_pose; transform::Rigid2d initial_pose;
transform::Rigid2d point_cloud_pose; transform::Rigid2d pose;
}; };
struct SubmapData { struct SubmapData {
@ -64,8 +64,8 @@ class OptimizationProblem {
void AddOdometerData(int trajectory_id, void AddOdometerData(int trajectory_id,
const sensor::OdometryData& odometry_data); const sensor::OdometryData& odometry_data);
void AddTrajectoryNode(int trajectory_id, common::Time time, void AddTrajectoryNode(int trajectory_id, common::Time time,
const transform::Rigid2d& initial_point_cloud_pose, const transform::Rigid2d& initial_pose,
const transform::Rigid2d& point_cloud_pose); const transform::Rigid2d& pose);
void TrimTrajectoryNode(const mapping::NodeId& node_id); void TrimTrajectoryNode(const mapping::NodeId& node_id);
void AddSubmap(int trajectory_id, const transform::Rigid2d& submap_pose); void AddSubmap(int trajectory_id, const transform::Rigid2d& submap_pose);
void TrimSubmap(const mapping::SubmapId& submap_id); 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() inverse_submap_pose * optimization_problem_.node_data()
.at(node_id.trajectory_id) .at(node_id.trajectory_id)
.at(node_id.node_index) .at(node_id.node_index)
.point_cloud_pose; .pose;
std::vector<mapping::TrajectoryNode> submap_nodes; std::vector<mapping::TrajectoryNode> submap_nodes;
for (const mapping::NodeId& submap_node_id : for (const mapping::NodeId& submap_node_id :
@ -265,8 +265,8 @@ void SparsePoseGraph::ComputeConstraintsForScan(
.size()) .size())
: 0}; : 0};
const auto& scan_data = trajectory_nodes_.at(node_id).constant_data; const auto& scan_data = trajectory_nodes_.at(node_id).constant_data;
optimization_problem_.AddTrajectoryNode(matching_id.trajectory_id, optimization_problem_.AddTrajectoryNode(
scan_data->time, optimized_pose); matching_id.trajectory_id, scan_data->time, pose, optimized_pose);
for (size_t i = 0; i < insertion_submaps.size(); ++i) { for (size_t i = 0; i < insertion_submaps.size(); ++i) {
const mapping::SubmapId submap_id = submap_ids[i]; const mapping::SubmapId submap_id = submap_ids[i];
// Even if this was the last scan added to 'submap_id', the submap will only // Even if this was the last scan added to 'submap_id', the submap will only
@ -481,7 +481,7 @@ void SparsePoseGraph::RunOptimization() {
++node_index) { ++node_index) {
const mapping::NodeId node_id{trajectory_id, node_index}; const mapping::NodeId node_id{trajectory_id, node_index};
trajectory_nodes_.at(node_id).pose = 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. // Extrapolate all point cloud poses that were added later.
const auto local_to_new_global = ComputeLocalToGlobalTransform( 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( void OptimizationProblem::AddTrajectoryNode(
const int trajectory_id, const common::Time time, 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); CHECK_GE(trajectory_id, 0);
node_data_.resize( node_data_.resize(
std::max(node_data_.size(), static_cast<size_t>(trajectory_id) + 1)); 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, 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(); for (size_t node_index = 0; node_index != node_data_[trajectory_id].size();
++node_index) { ++node_index) {
C_nodes[trajectory_id].emplace_back( C_nodes[trajectory_id].emplace_back(
node_data_[trajectory_id][node_index].point_cloud_pose, node_data_[trajectory_id][node_index].pose,
translation_parameterization(), translation_parameterization(),
common::make_unique<ceres::QuaternionParameterization>(), &problem); common::make_unique<ceres::QuaternionParameterization>(), &problem);
if (frozen) { if (frozen) {
@ -284,8 +284,7 @@ void OptimizationProblem::Solve(const std::vector<Constraint>& constraints,
if (!fixed_frame_pose_initialized) { if (!fixed_frame_pose_initialized) {
const transform::Rigid3d fixed_frame_pose_in_map = const transform::Rigid3d fixed_frame_pose_in_map =
node_data[node_index].point_cloud_pose * node_data[node_index].pose * constraint_pose.zbar_ij.inverse();
constraint_pose.zbar_ij.inverse();
C_fixed_frames.emplace_back( C_fixed_frames.emplace_back(
transform::Rigid3d( transform::Rigid3d(
fixed_frame_pose_in_map.translation(), fixed_frame_pose_in_map.translation(),
@ -347,7 +346,7 @@ void OptimizationProblem::Solve(const std::vector<Constraint>& constraints,
++trajectory_id) { ++trajectory_id) {
for (size_t node_index = 0; node_index != node_data_[trajectory_id].size(); for (size_t node_index = 0; node_index != node_data_[trajectory_id].size();
++node_index) { ++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(); 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 { struct NodeData {
common::Time time; common::Time time;
transform::Rigid3d point_cloud_pose; transform::Rigid3d initial_pose;
transform::Rigid3d pose;
}; };
struct SubmapData { struct SubmapData {
@ -67,7 +68,8 @@ class OptimizationProblem {
int trajectory_id, int trajectory_id,
const sensor::FixedFramePoseData& fixed_frame_pose_data); const sensor::FixedFramePoseData& fixed_frame_pose_data);
void AddTrajectoryNode(int trajectory_id, common::Time time, 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 AddSubmap(int trajectory_id, const transform::Rigid3d& submap_pose);
void SetMaxNumIterations(int32 max_num_iterations); 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( optimization_problem_.AddImuData(
kTrajectoryId, sensor::ImuData{now, Eigen::Vector3d::UnitZ() * 9.81, kTrajectoryId, sensor::ImuData{now, Eigen::Vector3d::UnitZ() * 9.81,
Eigen::Vector3d::Zero()}); Eigen::Vector3d::Zero()});
optimization_problem_.AddTrajectoryNode(kTrajectoryId, now, pose); optimization_problem_.AddTrajectoryNode(kTrajectoryId, now, pose, pose);
now += common::FromSeconds(0.01); now += common::FromSeconds(0.01);
} }
@ -159,11 +159,10 @@ TEST_F(OptimizationProblemTest, ReducesNoise) {
const auto& node_data = optimization_problem_.node_data().at(0); const auto& node_data = optimization_problem_.node_data().at(0);
for (int j = 0; j != kNumNodes; ++j) { for (int j = 0; j != kNumNodes; ++j) {
translation_error_before += (test_data[j].ground_truth_pose.translation() - translation_error_before += (test_data[j].ground_truth_pose.translation() -
node_data[j].point_cloud_pose.translation()) node_data[j].pose.translation())
.norm(); .norm();
rotation_error_before += rotation_error_before += transform::GetAngle(
transform::GetAngle(test_data[j].ground_truth_pose.inverse() * test_data[j].ground_truth_pose.inverse() * node_data[j].pose);
node_data[j].point_cloud_pose);
} }
optimization_problem_.AddSubmap(kTrajectoryId, kSubmap0Transform); optimization_problem_.AddSubmap(kTrajectoryId, kSubmap0Transform);
@ -176,11 +175,10 @@ TEST_F(OptimizationProblemTest, ReducesNoise) {
double rotation_error_after = 0.; double rotation_error_after = 0.;
for (int j = 0; j != kNumNodes; ++j) { for (int j = 0; j != kNumNodes; ++j) {
translation_error_after += (test_data[j].ground_truth_pose.translation() - translation_error_after += (test_data[j].ground_truth_pose.translation() -
node_data[j].point_cloud_pose.translation()) node_data[j].pose.translation())
.norm(); .norm();
rotation_error_after += rotation_error_after += transform::GetAngle(
transform::GetAngle(test_data[j].ground_truth_pose.inverse() * test_data[j].ground_truth_pose.inverse() * node_data[j].pose);
node_data[j].point_cloud_pose);
} }
EXPECT_GT(0.8 * translation_error_before, translation_error_after); EXPECT_GT(0.8 * translation_error_before, translation_error_after);