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 = const bool odometry_available =
trajectory_id < odometry_data_.size() && trajectory_id < odometry_data_.size() &&
odometry_data_[trajectory_id].Has( odometry_data_[trajectory_id].Has(next_node_data.time) &&
node_data_[trajectory_id][next_node_index].time) && odometry_data_[trajectory_id].Has(node_data.time);
odometry_data_[trajectory_id].Has(
node_data_[trajectory_id][node_index].time);
const transform::Rigid3d relative_pose = const transform::Rigid3d relative_pose =
odometry_available odometry_available
? transform::Rigid3d::Rotation(node_data.gravity_alignment) * ? 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_translation_penalty_factor(),
options_.consecutive_scan_rotation_penalty_factor()})), options_.consecutive_scan_rotation_penalty_factor()})),
nullptr /* loss function */, nullptr /* loss function */,
C_nodes[trajectory_id][node_index].data(), C_nodes[trajectory_id].at(node_index).data(),
C_nodes[trajectory_id][next_node_index].data()); C_nodes[trajectory_id].at(next_node_index).data());
} }
} }

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

@ -233,91 +233,143 @@ void OptimizationProblem::Solve(const std::vector<Constraint>& constraints,
// Add constraints based on IMU observations of angular velocities and // Add constraints based on IMU observations of angular velocities and
// linear acceleration. // linear acceleration.
trajectory_data_.resize(imu_data_.size()); if (fix_z_ == FixZ::kNo) {
CHECK_GE(trajectory_data_.size(), node_data_.size()); trajectory_data_.resize(imu_data_.size());
for (size_t trajectory_id = 0; trajectory_id != node_data_.size(); CHECK_GE(trajectory_data_.size(), node_data_.size());
++trajectory_id) { for (size_t trajectory_id = 0; trajectory_id != node_data_.size();
if (node_data_[trajectory_id].empty()) { ++trajectory_id) {
// We skip empty trajectories which might not have any IMU data. if (node_data_[trajectory_id].empty()) {
continue; // We skip empty trajectories which might not have any IMU data.
}
TrajectoryData& trajectory_data = trajectory_data_.at(trajectory_id);
problem.AddParameterBlock(trajectory_data.imu_calibration.data(), 4,
new ceres::QuaternionParameterization());
const std::deque<sensor::ImuData>& imu_data = imu_data_.at(trajectory_id);
CHECK(!imu_data.empty());
auto imu_it = imu_data.cbegin();
for (auto node_it = node_data_[trajectory_id].begin();;) {
const int first_node_index = node_it->first;
const NodeData& first_node_data = node_it->second;
++node_it;
if (node_it == node_data_[trajectory_id].end()) {
break;
}
const int second_node_index = node_it->first;
const NodeData& second_node_data = node_it->second;
if (second_node_index != first_node_index + 1) {
continue; continue;
} }
TrajectoryData& trajectory_data = trajectory_data_.at(trajectory_id);
problem.AddParameterBlock(trajectory_data.imu_calibration.data(), 4,
new ceres::QuaternionParameterization());
const std::deque<sensor::ImuData>& imu_data = imu_data_.at(trajectory_id);
CHECK(!imu_data.empty());
// Skip IMU data before the node. auto imu_it = imu_data.cbegin();
while ((imu_it + 1) != imu_data.cend() && for (auto node_it = node_data_[trajectory_id].begin();;) {
(imu_it + 1)->time <= first_node_data.time) { const int first_node_index = node_it->first;
++imu_it; const NodeData& first_node_data = node_it->second;
} ++node_it;
if (node_it == node_data_[trajectory_id].end()) {
break;
}
auto imu_it2 = imu_it; const int second_node_index = node_it->first;
const IntegrateImuResult<double> result = IntegrateImu( const NodeData& second_node_data = node_it->second;
imu_data, first_node_data.time, second_node_data.time, &imu_it);
const auto next_node_it = std::next(node_it); if (second_node_index != first_node_index + 1) {
if (next_node_it != node_data_[trajectory_id].end() && continue;
next_node_it->first == second_node_index + 1) { }
const int third_node_index = next_node_it->first;
const NodeData& third_node_data = next_node_it->second; // Skip IMU data before the node.
const common::Time first_time = first_node_data.time; while ((imu_it + 1) != imu_data.cend() &&
const common::Time second_time = second_node_data.time; (imu_it + 1)->time <= first_node_data.time) {
const common::Time third_time = third_node_data.time; ++imu_it;
const common::Duration first_duration = second_time - first_time; }
const common::Duration second_duration = third_time - second_time;
const common::Time first_center = first_time + first_duration / 2; auto imu_it2 = imu_it;
const common::Time second_center = second_time + second_duration / 2; const IntegrateImuResult<double> result = IntegrateImu(
const IntegrateImuResult<double> result_to_first_center = imu_data, first_node_data.time, second_node_data.time, &imu_it);
IntegrateImu(imu_data, first_time, first_center, &imu_it2); const auto next_node_it = std::next(node_it);
const IntegrateImuResult<double> result_center_to_center = if (next_node_it != node_data_[trajectory_id].end() &&
IntegrateImu(imu_data, first_center, second_center, &imu_it2); next_node_it->first == second_node_index + 1) {
// 'delta_velocity' is the change in velocity from the point in time const int third_node_index = next_node_it->first;
// halfway between the first and second poses to halfway between second const NodeData& third_node_data = next_node_it->second;
// and third pose. It is computed from IMU data and still contains a const common::Time first_time = first_node_data.time;
// delta due to gravity. The orientation of this vector is in the IMU const common::Time second_time = second_node_data.time;
// frame at the second pose. const common::Time third_time = third_node_data.time;
const Eigen::Vector3d delta_velocity = const common::Duration first_duration = second_time - first_time;
(result.delta_rotation.inverse() * const common::Duration second_duration = third_time - second_time;
result_to_first_center.delta_rotation) * const common::Time first_center = first_time + first_duration / 2;
result_center_to_center.delta_velocity; const common::Time second_center = second_time + second_duration / 2;
const IntegrateImuResult<double> result_to_first_center =
IntegrateImu(imu_data, first_time, first_center, &imu_it2);
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.
const Eigen::Vector3d delta_velocity =
(result.delta_rotation.inverse() *
result_to_first_center.delta_rotation) *
result_center_to_center.delta_velocity;
problem.AddResidualBlock(
new ceres::AutoDiffCostFunction<AccelerationCostFunction, 3, 4, 3,
3, 3, 1, 4>(
new AccelerationCostFunction(
options_.acceleration_weight(), delta_velocity,
common::ToSeconds(first_duration),
common::ToSeconds(second_duration))),
nullptr, C_nodes[trajectory_id].at(second_node_index).rotation(),
C_nodes[trajectory_id].at(first_node_index).translation(),
C_nodes[trajectory_id].at(second_node_index).translation(),
C_nodes[trajectory_id].at(third_node_index).translation(),
&trajectory_data.gravity_constant,
trajectory_data.imu_calibration.data());
}
problem.AddResidualBlock( problem.AddResidualBlock(
new ceres::AutoDiffCostFunction<AccelerationCostFunction, 3, 4, 3, new ceres::AutoDiffCostFunction<RotationCostFunction, 3, 4, 4, 4>(
3, 3, 1, 4>( new RotationCostFunction(options_.rotation_weight(),
new AccelerationCostFunction( result.delta_rotation)),
options_.acceleration_weight(), delta_velocity, nullptr, C_nodes[trajectory_id].at(first_node_index).rotation(),
common::ToSeconds(first_duration), C_nodes[trajectory_id].at(second_node_index).rotation(),
common::ToSeconds(second_duration))),
nullptr, C_nodes[trajectory_id].at(second_node_index).rotation(),
C_nodes[trajectory_id].at(first_node_index).translation(),
C_nodes[trajectory_id].at(second_node_index).translation(),
C_nodes[trajectory_id].at(third_node_index).translation(),
&trajectory_data.gravity_constant,
trajectory_data.imu_calibration.data()); trajectory_data.imu_calibration.data());
} }
problem.AddResidualBlock( }
new ceres::AutoDiffCostFunction<RotationCostFunction, 3, 4, 4, 4>( }
new RotationCostFunction(options_.rotation_weight(),
result.delta_rotation)), if (fix_z_ == FixZ::kYes) {
nullptr, C_nodes[trajectory_id].at(first_node_index).rotation(), // Add penalties for violating odometry or changes between consecutive scans
C_nodes[trajectory_id].at(second_node_index).rotation(), // if odometry is not available.
trajectory_data.imu_calibration.data()); 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());
}
} }
} }