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

Add support for odometry to the 3D pose graph optimization. (#570) 

Not used yet. Intended to experiment with the 3D pose graph optimization
in 2D SLAM.
master
Wolfgang Hess 2017-10-05 16:56:31 +02:00 committed by GitHub
parent bd8a2e6a92
commit 0053b30cc8
2 changed files with 134 additions and 84 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -223,10 +223,8 @@ void OptimizationProblem::Solve(const std::vector<Constraint>& constraints,
const bool odometry_available =
trajectory_id < odometry_data_.size() &&
odometry_data_[trajectory_id].Has(
node_data_[trajectory_id][next_node_index].time) &&
odometry_data_[trajectory_id].Has(
node_data_[trajectory_id][node_index].time);
odometry_data_[trajectory_id].Has(next_node_data.time) &&
odometry_data_[trajectory_id].Has(node_data.time);
const transform::Rigid3d relative_pose =
odometry_available
? transform::Rigid3d::Rotation(node_data.gravity_alignment) *
@ -245,8 +243,8 @@ void OptimizationProblem::Solve(const std::vector<Constraint>& constraints,
options_.consecutive_scan_translation_penalty_factor(),
options_.consecutive_scan_rotation_penalty_factor()})),
nullptr /* loss function */,
C_nodes[trajectory_id][node_index].data(),
C_nodes[trajectory_id][next_node_index].data());
C_nodes[trajectory_id].at(node_index).data(),
C_nodes[trajectory_id].at(next_node_index).data());
}
}

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

@ -233,6 +233,7 @@ void OptimizationProblem::Solve(const std::vector<Constraint>& constraints,
// Add constraints based on IMU observations of angular velocities and
// linear acceleration.
if (fix_z_ == FixZ::kNo) {
trajectory_data_.resize(imu_data_.size());
CHECK_GE(trajectory_data_.size(), node_data_.size());
for (size_t trajectory_id = 0; trajectory_id != node_data_.size();
@ -289,10 +290,10 @@ void OptimizationProblem::Solve(const std::vector<Constraint>& constraints,
const IntegrateImuResult<double> result_center_to_center =
IntegrateImu(imu_data, first_center, second_center, &imu_it2);
// 'delta_velocity' is the change in velocity from the point in time
// halfway between the first and second poses to halfway between second
// and third pose. It is computed from IMU data and still contains a
// delta due to gravity. The orientation of this vector is in the IMU
// frame at the second pose.
// halfway between the first and second poses to halfway between
// second and third pose. It is computed from IMU data and still
// contains a delta due to gravity. The orientation of this vector is
// in the IMU frame at the second pose.
const Eigen::Vector3d delta_velocity =
(result.delta_rotation.inverse() *
result_to_first_center.delta_rotation) *
@ -320,6 +321,57 @@ void OptimizationProblem::Solve(const std::vector<Constraint>& constraints,
trajectory_data.imu_calibration.data());
}
}
}
if (fix_z_ == FixZ::kYes) {
// Add penalties for violating odometry or changes between consecutive scans
// if odometry is not available.
for (size_t trajectory_id = 0; trajectory_id != node_data_.size();
++trajectory_id) {
if (node_data_[trajectory_id].empty()) {
continue;
}
for (auto node_it = node_data_[trajectory_id].begin();;) {
const int node_index = node_it->first;
const NodeData& node_data = node_it->second;
++node_it;
if (node_it == node_data_[trajectory_id].end()) {
break;
}
const int next_node_index = node_it->first;
const NodeData& next_node_data = node_it->second;
if (next_node_index != node_index + 1) {
continue;
}
const bool odometry_available =
trajectory_id < odometry_data_.size() &&
odometry_data_[trajectory_id].Has(next_node_data.time) &&
odometry_data_[trajectory_id].Has(node_data.time);
const transform::Rigid3d relative_pose =
odometry_available
? odometry_data_[trajectory_id]
.Lookup(node_data.time)
.inverse() *
odometry_data_[trajectory_id].Lookup(next_node_data.time)
: node_data.initial_pose.inverse() *
next_node_data.initial_pose;
problem.AddResidualBlock(
new ceres::AutoDiffCostFunction<SpaCostFunction, 6, 4, 3, 4, 3>(
new SpaCostFunction(Constraint::Pose{
relative_pose,
options_.consecutive_scan_translation_penalty_factor(),
options_.consecutive_scan_rotation_penalty_factor()})),
nullptr /* loss function */,
C_nodes[trajectory_id].at(node_index).rotation(),
C_nodes[trajectory_id].at(node_index).translation(),
C_nodes[trajectory_id].at(next_node_index).rotation(),
C_nodes[trajectory_id].at(next_node_index).translation());
}
}
}
// Add fixed frame pose constraints.
std::deque<CeresPose> C_fixed_frames;