diff --git a/cartographer/mapping_2d/sparse_pose_graph/optimization_problem.cc b/cartographer/mapping_2d/sparse_pose_graph/optimization_problem.cc index 27984d1..a404ebf 100644 --- a/cartographer/mapping_2d/sparse_pose_graph/optimization_problem.cc +++ b/cartographer/mapping_2d/sparse_pose_graph/optimization_problem.cc @@ -223,10 +223,8 @@ void OptimizationProblem::Solve(const std::vector& 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& 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()); } } diff --git a/cartographer/mapping_3d/sparse_pose_graph/optimization_problem.cc b/cartographer/mapping_3d/sparse_pose_graph/optimization_problem.cc index b2671ad..4027c4b 100644 --- a/cartographer/mapping_3d/sparse_pose_graph/optimization_problem.cc +++ b/cartographer/mapping_3d/sparse_pose_graph/optimization_problem.cc @@ -233,91 +233,143 @@ void OptimizationProblem::Solve(const std::vector& 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& 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& 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 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 result_to_first_center = - IntegrateImu(imu_data, first_time, first_center, &imu_it2); - const IntegrateImuResult 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 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 result_to_first_center = + IntegrateImu(imu_data, first_time, first_center, &imu_it2); + const IntegrateImuResult 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( + 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( - 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( + 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( - 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( + 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()); + } } }