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
GitHub
parent
bd8a2e6a92
commit
0053b30cc8
|
|
@ -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());
|
||||
}
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -233,91 +233,143 @@ void OptimizationProblem::Solve(const std::vector<Constraint>& constraints,
|
|||
|
||||
// Add constraints based on IMU observations of angular velocities and
|
||||
// linear acceleration.
|
||||
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();
|
||||
++trajectory_id) {
|
||||
if (node_data_[trajectory_id].empty()) {
|
||||
// We skip empty trajectories which might not have any IMU data.
|
||||
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());
|
||||
|
||||
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) {
|
||||
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();
|
||||
++trajectory_id) {
|
||||
if (node_data_[trajectory_id].empty()) {
|
||||
// We skip empty trajectories which might not have any IMU data.
|
||||
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.
|
||||
while ((imu_it + 1) != imu_data.cend() &&
|
||||
(imu_it + 1)->time <= first_node_data.time) {
|
||||
++imu_it;
|
||||
}
|
||||
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;
|
||||
}
|
||||
|
||||
auto imu_it2 = imu_it;
|
||||
const IntegrateImuResult<double> result = IntegrateImu(
|
||||
imu_data, first_node_data.time, second_node_data.time, &imu_it);
|
||||
const auto next_node_it = std::next(node_it);
|
||||
if (next_node_it != node_data_[trajectory_id].end() &&
|
||||
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;
|
||||
const common::Time first_time = first_node_data.time;
|
||||
const common::Time second_time = second_node_data.time;
|
||||
const common::Time third_time = third_node_data.time;
|
||||
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;
|
||||
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;
|
||||
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;
|
||||
}
|
||||
|
||||
// Skip IMU data before the node.
|
||||
while ((imu_it + 1) != imu_data.cend() &&
|
||||
(imu_it + 1)->time <= first_node_data.time) {
|
||||
++imu_it;
|
||||
}
|
||||
|
||||
auto imu_it2 = imu_it;
|
||||
const IntegrateImuResult<double> result = IntegrateImu(
|
||||
imu_data, first_node_data.time, second_node_data.time, &imu_it);
|
||||
const auto next_node_it = std::next(node_it);
|
||||
if (next_node_it != node_data_[trajectory_id].end() &&
|
||||
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;
|
||||
const common::Time first_time = first_node_data.time;
|
||||
const common::Time second_time = second_node_data.time;
|
||||
const common::Time third_time = third_node_data.time;
|
||||
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;
|
||||
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(
|
||||
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,
|
||||
new ceres::AutoDiffCostFunction<RotationCostFunction, 3, 4, 4, 4>(
|
||||
new RotationCostFunction(options_.rotation_weight(),
|
||||
result.delta_rotation)),
|
||||
nullptr, C_nodes[trajectory_id].at(first_node_index).rotation(),
|
||||
C_nodes[trajectory_id].at(second_node_index).rotation(),
|
||||
trajectory_data.imu_calibration.data());
|
||||
}
|
||||
problem.AddResidualBlock(
|
||||
new ceres::AutoDiffCostFunction<RotationCostFunction, 3, 4, 4, 4>(
|
||||
new RotationCostFunction(options_.rotation_weight(),
|
||||
result.delta_rotation)),
|
||||
nullptr, C_nodes[trajectory_id].at(first_node_index).rotation(),
|
||||
C_nodes[trajectory_id].at(second_node_index).rotation(),
|
||||
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());
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
|
|
|||
Loading…
Reference in New Issue