1. SPA殘差
- Constraint與真實位置間的殘差
- Constraint: 只是一個Node j與一個Submap i間最好的匹配,二者匹配得最好,但只關注了局部,並不能保證整個Pose Graph匹配得最好,即與真實環境一致
- SPA優化的目的:使用所有Node與所有的Submap整體上匹配得最好,即關注全局
- SPA優化的方法:使所有殘差之和最小, 殘差爲:constraint 的 pose 與真實pose之差最小
- Ceres的求解參數的數據類型只支持:double*
2.代碼實現
ceres::Problem::Options problem_options;
ceres::Problem problem(problem_options);
// Set the starting point.
// TODO(hrapp): Move ceres data into SubmapSpec.
MapById<SubmapId, std::array<double, 3>> C_submaps;
MapById<NodeId, std::array<double, 3>> C_nodes;
std::map<std::string, CeresPose> C_landmarks;
bool first_submap = true;
bool freeze_landmarks = !frozen_trajectories.empty();
// 提取所有Submap位姿信息,並保存在C_submaps中
for (const auto& submap_id_data : submap_data_) {
const bool frozen =
frozen_trajectories.count(submap_id_data.id.trajectory_id) != 0;
C_submaps.Insert(submap_id_data.id,
FromPose(submap_id_data.data.global_pose));
// 一次性增加所有待優化參數,AddResidualBlock檢查到有此指針,則不再增加
problem.AddParameterBlock(C_submaps.at(submap_id_data.id).data(), 3);
if (first_submap || frozen) {
first_submap = false;
// Fix the pose of the first submap or all submaps of a frozen
// trajectory.
problem.SetParameterBlockConstant(C_submaps.at(submap_id_data.id).data());
}
}
// 提取所有Node位姿信息,並保存在C_nodes中
for (const auto& node_id_data : node_data_) {
const bool frozen =
frozen_trajectories.count(node_id_data.id.trajectory_id) != 0;
C_nodes.Insert(node_id_data.id, FromPose(node_id_data.data.global_pose_2d));
// 一次性增加所有待優化參數,AddResidualBlock檢查到有此指針,則不再增加
problem.AddParameterBlock(C_nodes.at(node_id_data.id).data(), 3);
if (frozen) {
problem.SetParameterBlockConstant(C_nodes.at(node_id_data.id).data());
}
}
// Add cost functions for intra- and inter-submap constraints.
for (const Constraint& constraint : constraints) {
problem.AddResidualBlock(
CreateAutoDiffSpaCostFunction(constraint.pose),
// Only loop closure constraints should have a loss function.
constraint.tag == Constraint::INTER_SUBMAP
? new ceres::HuberLoss(options_.huber_scale())
: nullptr,
C_submaps.at(constraint.submap_id).data(),
C_nodes.at(constraint.node_id).data());
}
ceres::CostFunction* CreateAutoDiffSpaCostFunction(
const PoseGraphInterface::Constraint::Pose& observed_relative_pose) {
return new ceres::AutoDiffCostFunction<SpaCostFunction2D, 3 /* residuals */,
3 /* start pose variables */,
3 /* end pose variables */>(
new SpaCostFunction2D(observed_relative_pose));
}
class SpaCostFunction2D {
public:
explicit SpaCostFunction2D(
const PoseGraphInterface::Constraint::Pose& observed_relative_pose)
: observed_relative_pose_(observed_relative_pose) {}
template <typename T>
bool operator()(const T* const start_pose, const T* const end_pose,
T* e) const {
const std::array<T, 3> error =
ScaleError(ComputeUnscaledError(
transform::Project2D(observed_relative_pose_.zbar_ij),
start_pose, end_pose),
observed_relative_pose_.translation_weight,
observed_relative_pose_.rotation_weight);
std::copy(std::begin(error), std::end(error), e);
return true;
}
private:
const PoseGraphInterface::Constraint::Pose observed_relative_pose_;
};
template <typename T>
static std::array<T, 3> ComputeUnscaledError(
const transform::Rigid2d& relative_pose, const T* const start,
const T* const end) {
const T cos_theta_i = cos(start[2]);
const T sin_theta_i = sin(start[2]);
const T delta_x = end[0] - start[0];
const T delta_y = end[1] - start[1];
const T h[3] = {cos_theta_i * delta_x + sin_theta_i * delta_y,
-sin_theta_i * delta_x + cos_theta_i * delta_y,
end[2] - start[2]};
return {{T(relative_pose.translation().x()) - h[0],
T(relative_pose.translation().y()) - h[1],
common::NormalizeAngleDifference(
T(relative_pose.rotation().angle()) - h[2])}};
}
template <typename T>
std::array<T, 3> ScaleError(const std::array<T, 3>& error,
double translation_weight, double rotation_weight) {
// clang-format off
return {{
error[0] * translation_weight,
error[1] * translation_weight,
error[2] * rotation_weight
}};
// clang-format on
}