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:
Data(int milliseconds) : time_(CreateTime(milliseconds)) {}
const common::Time& time() const {
return time_;
}
const common::Time& time() const { return time_; }
private:
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});
}
void OptimizationProblem::TrimTrajectoryNode(const mapping::NodeId& 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();
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;
for (const mapping::NodeId& submap_node_id :
@ -500,7 +501,7 @@ void SparsePoseGraph::RunOptimization() {
const auto& node_data = optimization_problem_.node_data();
for (const int trajectory_id : node_data.trajectory_ids()) {
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

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

@ -82,20 +82,22 @@ void OptimizationProblem::AddFixedFramePoseData(
void OptimizationProblem::AddTrajectoryNode(
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);
trajectory_data_.resize(std::max(trajectory_data_.size(),
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(
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);
trajectory_data_.resize(std::max(
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) {
@ -197,7 +199,7 @@ void OptimizationProblem::Solve(const std::vector<Constraint>& constraints,
frozen_trajectories.count(node_id_data.id.trajectory_id) != 0;
C_nodes.Insert(
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>(),
&problem));
if (frozen) {
@ -335,8 +337,8 @@ void OptimizationProblem::Solve(const std::vector<Constraint>& constraints,
.inverse() *
odometry_data_[trajectory_id].Lookup(
second_node_data.time)
: first_node_data.initial_pose.inverse() *
second_node_data.initial_pose;
: first_node_data.local_pose.inverse() *
second_node_data.local_pose;
problem.AddResidualBlock(
new ceres::AutoDiffCostFunction<SpaCostFunction, 6, 4, 3, 4, 3>(
new SpaCostFunction(Constraint::Pose{
@ -376,7 +378,7 @@ void OptimizationProblem::Solve(const std::vector<Constraint>& constraints,
if (!fixed_frame_pose_initialized) {
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(
transform::Rigid3d(
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();
}
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 {
common::Time time;
transform::Rigid3d initial_pose;
transform::Rigid3d pose;
transform::Rigid3d local_pose;
transform::Rigid3d global_pose;
};
struct SubmapData {
@ -72,11 +72,11 @@ class OptimizationProblem {
int trajectory_id,
const sensor::FixedFramePoseData& fixed_frame_pose_data);
void AddTrajectoryNode(int trajectory_id, common::Time time,
const transform::Rigid3d& initial_pose,
const transform::Rigid3d& pose);
const transform::Rigid3d& local_pose,
const transform::Rigid3d& global_pose);
void InsertTrajectoryNode(const mapping::NodeId& node_id, common::Time time,
const transform::Rigid3d& initial_pose,
const transform::Rigid3d& pose);
const transform::Rigid3d& local_pose,
const transform::Rigid3d& global_pose);
void TrimTrajectoryNode(const mapping::NodeId& node_id);
void AddSubmap(int trajectory_id, const transform::Rigid3d& submap_pose);
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.;
const auto& node_data = optimization_problem_.node_data();
for (int j = 0; j != kNumNodes; ++j) {
translation_error_before +=
(test_data[j].ground_truth_pose.translation() -
node_data.at(mapping::NodeId{kTrajectoryId, j}).pose.translation())
.norm();
translation_error_before += (test_data[j].ground_truth_pose.translation() -
node_data.at(mapping::NodeId{kTrajectoryId, j})
.global_pose.translation())
.norm();
rotation_error_before += transform::GetAngle(
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);
@ -176,13 +176,13 @@ TEST_F(OptimizationProblemTest, ReducesNoise) {
double translation_error_after = 0.;
double rotation_error_after = 0.;
for (int j = 0; j != kNumNodes; ++j) {
translation_error_after +=
(test_data[j].ground_truth_pose.translation() -
node_data.at(mapping::NodeId{kTrajectoryId, j}).pose.translation())
.norm();
translation_error_after += (test_data[j].ground_truth_pose.translation() -
node_data.at(mapping::NodeId{kTrajectoryId, j})
.global_pose.translation())
.norm();
rotation_error_after += transform::GetAngle(
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);