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

Follow #591 terminology in 3D optimization. (#612)

master
Wolfgang Hess 2017-10-27 15:48:51 +02:00 committed by GitHub
parent dacc962399
commit 82a4b2f171
6 changed files with 32 additions and 31 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
cartographer/mapping/id_test.cc
View File

@ -37,9 +37,7 @@ class Data {
public: public:
Data(int milliseconds) : time_(CreateTime(milliseconds)) {} Data(int milliseconds) : time_(CreateTime(milliseconds)) {}
const common::Time& time() const { const common::Time& time() const { return time_; }
return time_;
}
private: private:
const common::Time time_; const common::Time time_;

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

@ -94,7 +94,6 @@ void OptimizationProblem::InsertTrajectoryNode(
NodeData{time, initial_pose, pose, gravity_alignment}); NodeData{time, initial_pose, pose, gravity_alignment});
} }
void OptimizationProblem::TrimTrajectoryNode(const mapping::NodeId& node_id) { void OptimizationProblem::TrimTrajectoryNode(const mapping::NodeId& node_id) {
node_data_.Trim(node_id); node_data_.Trim(node_id);

5
cartographer/mapping_3d/sparse_pose_graph.cc
View File

@ -175,7 +175,8 @@ void SparsePoseGraph::ComputeConstraint(const mapping::NodeId& node_id,
optimization_problem_.submap_data().at(submap_id).pose.inverse(); optimization_problem_.submap_data().at(submap_id).pose.inverse();
const transform::Rigid3d initial_relative_pose = const transform::Rigid3d initial_relative_pose =
inverse_submap_pose * optimization_problem_.node_data().at(node_id).pose; inverse_submap_pose *
optimization_problem_.node_data().at(node_id).global_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 :
@ -500,7 +501,7 @@ void SparsePoseGraph::RunOptimization() {
const auto& node_data = optimization_problem_.node_data(); const auto& node_data = optimization_problem_.node_data();
for (const int trajectory_id : node_data.trajectory_ids()) { for (const int trajectory_id : node_data.trajectory_ids()) {
for (const auto& node : node_data.trajectory(trajectory_id)) { for (const auto& node : node_data.trajectory(trajectory_id)) {
trajectory_nodes_.at(node.id).global_pose = node.data.pose; trajectory_nodes_.at(node.id).global_pose = node.data.global_pose;
} }
// Extrapolate all point cloud poses that were not included in the // Extrapolate all point cloud poses that were not included in the

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

@ -82,20 +82,22 @@ 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& initial_pose, const transform::Rigid3d& pose) { const transform::Rigid3d& local_pose,
const transform::Rigid3d& global_pose) {
CHECK_GE(trajectory_id, 0); CHECK_GE(trajectory_id, 0);
trajectory_data_.resize(std::max(trajectory_data_.size(), trajectory_data_.resize(std::max(trajectory_data_.size(),
static_cast<size_t>(trajectory_id) + 1)); static_cast<size_t>(trajectory_id) + 1));
node_data_.Append(trajectory_id, NodeData{time, initial_pose, pose}); node_data_.Append(trajectory_id, NodeData{time, local_pose, global_pose});
} }
void OptimizationProblem::InsertTrajectoryNode( void OptimizationProblem::InsertTrajectoryNode(
const mapping::NodeId& node_id, const common::Time time, const mapping::NodeId& node_id, const common::Time time,
const transform::Rigid3d& initial_pose, const transform::Rigid3d& pose) { const transform::Rigid3d& local_pose,
const transform::Rigid3d& global_pose) {
CHECK_GE(node_id.trajectory_id, 0); CHECK_GE(node_id.trajectory_id, 0);
trajectory_data_.resize(std::max( trajectory_data_.resize(std::max(
trajectory_data_.size(), static_cast<size_t>(node_id.trajectory_id) + 1)); trajectory_data_.size(), static_cast<size_t>(node_id.trajectory_id) + 1));
node_data_.Insert(node_id, NodeData{time, initial_pose, pose}); node_data_.Insert(node_id, NodeData{time, local_pose, global_pose});
} }
void OptimizationProblem::TrimTrajectoryNode(const mapping::NodeId& node_id) { void OptimizationProblem::TrimTrajectoryNode(const mapping::NodeId& node_id) {
@ -197,7 +199,7 @@ void OptimizationProblem::Solve(const std::vector<Constraint>& constraints,
frozen_trajectories.count(node_id_data.id.trajectory_id) != 0; frozen_trajectories.count(node_id_data.id.trajectory_id) != 0;
C_nodes.Insert( C_nodes.Insert(
node_id_data.id, node_id_data.id,
CeresPose(node_id_data.data.pose, translation_parameterization(), CeresPose(node_id_data.data.global_pose, translation_parameterization(),
common::make_unique<ceres::QuaternionParameterization>(), common::make_unique<ceres::QuaternionParameterization>(),
&problem)); &problem));
if (frozen) { if (frozen) {
@ -335,8 +337,8 @@ void OptimizationProblem::Solve(const std::vector<Constraint>& constraints,
.inverse() * .inverse() *
odometry_data_[trajectory_id].Lookup( odometry_data_[trajectory_id].Lookup(
second_node_data.time) second_node_data.time)
: first_node_data.initial_pose.inverse() * : first_node_data.local_pose.inverse() *
second_node_data.initial_pose; second_node_data.local_pose;
problem.AddResidualBlock( problem.AddResidualBlock(
new ceres::AutoDiffCostFunction<SpaCostFunction, 6, 4, 3, 4, 3>( new ceres::AutoDiffCostFunction<SpaCostFunction, 6, 4, 3, 4, 3>(
new SpaCostFunction(Constraint::Pose{ new SpaCostFunction(Constraint::Pose{
@ -376,7 +378,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.pose * constraint_pose.zbar_ij.inverse(); node_data.global_pose * 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(),
@ -428,7 +430,8 @@ void OptimizationProblem::Solve(const std::vector<Constraint>& constraints,
submap_data_.at(C_submap_id_data.id).pose = C_submap_id_data.data.ToRigid(); submap_data_.at(C_submap_id_data.id).pose = C_submap_id_data.data.ToRigid();
} }
for (const auto& C_node_id_data : C_nodes) { for (const auto& C_node_id_data : C_nodes) {
node_data_.at(C_node_id_data.id).pose = C_node_id_data.data.ToRigid(); node_data_.at(C_node_id_data.id).global_pose =
C_node_id_data.data.ToRigid();
} }
} }

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

@ -41,8 +41,8 @@ namespace sparse_pose_graph {
struct NodeData { struct NodeData {
common::Time time; common::Time time;
transform::Rigid3d initial_pose; transform::Rigid3d local_pose;
transform::Rigid3d pose; transform::Rigid3d global_pose;
}; };
struct SubmapData { struct SubmapData {
@ -72,11 +72,11 @@ 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& initial_pose, const transform::Rigid3d& local_pose,
const transform::Rigid3d& pose); const transform::Rigid3d& global_pose);
void InsertTrajectoryNode(const mapping::NodeId& node_id, common::Time time, void InsertTrajectoryNode(const mapping::NodeId& node_id, common::Time time,
const transform::Rigid3d& initial_pose, const transform::Rigid3d& local_pose,
const transform::Rigid3d& pose); const transform::Rigid3d& global_pose);
void TrimTrajectoryNode(const mapping::NodeId& node_id); void TrimTrajectoryNode(const mapping::NodeId& node_id);
void AddSubmap(int trajectory_id, const transform::Rigid3d& submap_pose); void AddSubmap(int trajectory_id, const transform::Rigid3d& submap_pose);
void InsertSubmap(const mapping::SubmapId& submap_id, void InsertSubmap(const mapping::SubmapId& submap_id,

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

@ -158,13 +158,13 @@ TEST_F(OptimizationProblemTest, ReducesNoise) {
double rotation_error_before = 0.; double rotation_error_before = 0.;
const auto& node_data = optimization_problem_.node_data(); const auto& node_data = optimization_problem_.node_data();
for (int j = 0; j != kNumNodes; ++j) { for (int j = 0; j != kNumNodes; ++j) {
translation_error_before += translation_error_before += (test_data[j].ground_truth_pose.translation() -
(test_data[j].ground_truth_pose.translation() - node_data.at(mapping::NodeId{kTrajectoryId, j})
node_data.at(mapping::NodeId{kTrajectoryId, j}).pose.translation()) .global_pose.translation())
.norm(); .norm();
rotation_error_before += transform::GetAngle( rotation_error_before += transform::GetAngle(
test_data[j].ground_truth_pose.inverse() * test_data[j].ground_truth_pose.inverse() *
node_data.at(mapping::NodeId{kTrajectoryId, j}).pose); node_data.at(mapping::NodeId{kTrajectoryId, j}).global_pose);
} }
optimization_problem_.AddSubmap(kTrajectoryId, kSubmap0Transform); optimization_problem_.AddSubmap(kTrajectoryId, kSubmap0Transform);
@ -176,13 +176,13 @@ TEST_F(OptimizationProblemTest, ReducesNoise) {
double translation_error_after = 0.; double translation_error_after = 0.;
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 += translation_error_after += (test_data[j].ground_truth_pose.translation() -
(test_data[j].ground_truth_pose.translation() - node_data.at(mapping::NodeId{kTrajectoryId, j})
node_data.at(mapping::NodeId{kTrajectoryId, j}).pose.translation()) .global_pose.translation())
.norm(); .norm();
rotation_error_after += transform::GetAngle( rotation_error_after += transform::GetAngle(
test_data[j].ground_truth_pose.inverse() * test_data[j].ground_truth_pose.inverse() *
node_data.at(mapping::NodeId{kTrajectoryId, j}).pose); node_data.at(mapping::NodeId{kTrajectoryId, j}).global_pose);
} }
EXPECT_GT(0.8 * translation_error_before, translation_error_after); EXPECT_GT(0.8 * translation_error_before, translation_error_after);