简介：cartograoher使用位姿图优化来slam，从代码上来看是一股脑的处理所有数据，这样当数据不断增长时就很不利，可以试试因子图的方案，或者是vins的策略。cartographer也不是一点也没有，他使用了frozen_trajectories的策略，就是该路径不需要再优化就设置为frozen，但是何时设置为Frozen呢？这是个问题，所以cartographer是希望大地图是由小地图拼起来的，每次建小地图然后保存，然后再加载时设置为frozen，然后再建新图再一起保存。
虽然功能上还是有些可以优化的地方，但是这段代码写的真优雅。
talk is cheap, show other’s code
void OptimizationProblem2D::Solve(
const std::vector<Constraint>& constraints,
const std::map<int, PoseGraphInterface::TrajectoryState>&
trajectories_state,
const std::map<std::string, LandmarkNode>& landmark_nodes) {
if (node_data_.empty()) {
// Nothing to optimize.
return;
}
//
std::set<int> frozen_trajectories;
std::set<int> idle_trajectories;
for (const auto& it : trajectories_state) {
if (it.second == PoseGraphInterface::TrajectoryState::FROZEN) {
frozen_trajectories.insert(it.first);
}
else if(it.second == PoseGraphInterface::TrajectoryState::IDLE){
idle_trajectories.insert(it.first);
}
}
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;
//遍历每个活跃的子图
for (const auto& submap_id_data : submap_data_) {
//okagv
//查询该子图所在的路径是否需要优化，这个是博主自己添加的
const bool idle =
idle_trajectories.count(submap_id_data.id.trajectory_id) != 0;
if(idle) continue;
//查询该子图所在的路径是否是个固定位姿。
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));
//把要优化的位姿添加进去
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.
//如果是第一张子图或者该子图是frozen，则不优化并设置为常数。
problem.SetParameterBlockConstant(C_submaps.at(submap_id_data.id).data());
}
}
//遍历每一个活跃的路径节点
for (const auto& node_id_data : node_data_) {
//同上
const bool idle =
idle_trajectories.count(node_id_data.id.trajectory_id) != 0;
if(idle) continue;
//同上
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));
//把要优化的路径节点添加进去
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) {
//同上
bool idle =
((idle_trajectories.count(constraint.submap_id.trajectory_id) != 0) ||
(idle_trajectories.count(constraint.node_id.trajectory_id) != 0));
if(idle) continue;
//构建子图位姿与路径位姿的残差方程，ceres传入数据是指针的方式，所以最后优化结果保存在C_submaps和C_nodes里
problem.AddResidualBlock(
CreateAutoDiffSpaCostFunction(constraint.pose),
// 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());
}
// Add cost functions for landmarks.
//LOG(INFO) << "Peak.ding landmark_nodes.size " << landmark_nodes.size();
//构建Landmark与路径节点之间的位姿
AddLandmarkCostFunctions(landmark_nodes, node_data_, &C_nodes, &C_landmarks,
&problem, options_.huber_scale(), trajectories_state);
// Add penalties for violating odometry or changes between consecutive nodes
// if odometry is not available.
//如果有里程计数据（一般指齿轮编码器的数据）和slam pose，则构建路径节点之间的约束，防止优化后的路径失去连续性
for (auto node_it = node_data_.begin(); node_it != node_data_.end();) {
const int trajectory_id = node_it->id.trajectory_id;
const auto trajectory_end = node_data_.EndOfTrajectory(trajectory_id);
if (frozen_trajectories.count(trajectory_id) != 0) {
node_it = trajectory_end;
continue;
}
if (idle_trajectories.count(trajectory_id) != 0){
node_it = trajectory_end;
continue;
}
auto prev_node_it = node_it;
for (++node_it; node_it != trajectory_end; ++node_it) {
const NodeId first_node_id = prev_node_it->id;
const NodeSpec2D& first_node_data = prev_node_it->data;
prev_node_it = node_it;
const NodeId second_node_id = node_it->id;
const NodeSpec2D& second_node_data = node_it->data;
if (second_node_id.node_index != first_node_id.node_index + 1) {
continue;
}
// Add a relative pose constraint based on the odometry (if available).
//如果有齿轮编码器数据
std::unique_ptr<transform::Rigid3d> relative_odometry =
CalculateOdometryBetweenNodes(trajectory_id, first_node_data,
second_node_data);
if (relative_odometry != nullptr) {
problem.AddResidualBlock(
CreateAutoDiffSpaCostFunction(Constraint::Pose{
*relative_odometry, options_.odometry_translation_weight(),
options_.odometry_rotation_weight()}),
nullptr /* loss function */, C_nodes.at(first_node_id).data(),
C_nodes.at(second_node_id).data());
}
// Add a relative pose constraint based on consecutive local SLAM poses.
//使用局部的local SLAM pose
const transform::Rigid3d relative_local_slam_pose =
transform::Embed3D(first_node_data.local_pose_2d.inverse() *
second_node_data.local_pose_2d);
//添加约束
problem.AddResidualBlock(
CreateAutoDiffSpaCostFunction(
Constraint::Pose{relative_local_slam_pose,
options_.local_slam_pose_translation_weight(),
options_.local_slam_pose_rotation_weight()}),
nullptr /* loss function */, C_nodes.at(first_node_id).data(),
C_nodes.at(second_node_id).data());
}
}
//fixed_frame一般指GPS数据，这里我们没有用到
std::map<int, std::array<double, 3>> C_fixed_frames;
for (auto node_it = node_data_.begin(); node_it != node_data_.end();) {
const int trajectory_id = node_it->id.trajectory_id;
const auto trajectory_end = node_data_.EndOfTrajectory(trajectory_id);
if (!fixed_frame_pose_data_.HasTrajectory(trajectory_id)) {
node_it = trajectory_end;
continue;
}
const TrajectoryData& trajectory_data = trajectory_data_.at(trajectory_id);
bool fixed_frame_pose_initialized = false;
for (; node_it != trajectory_end; ++node_it) {
const NodeId node_id = node_it->id;
const NodeSpec2D& node_data = node_it->data;
const std::unique_ptr<transform::Rigid3d> fixed_frame_pose =
Interpolate(fixed_frame_pose_data_, trajectory_id, node_data.time);
if (fixed_frame_pose == nullptr) {
continue;
}
const Constraint::Pose constraint_pose{
*fixed_frame_pose, options_.fixed_frame_pose_translation_weight(),
options_.fixed_frame_pose_rotation_weight()};
if (!fixed_frame_pose_initialized) {
transform::Rigid2d fixed_frame_pose_in_map;
if (trajectory_data.fixed_frame_origin_in_map.has_value()) {
fixed_frame_pose_in_map = transform::Project2D(
trajectory_data.fixed_frame_origin_in_map.value());
} else {
fixed_frame_pose_in_map =
node_data.global_pose_2d *
transform::Project2D(constraint_pose.zbar_ij).inverse();
}
C_fixed_frames.emplace(trajectory_id,
FromPose(fixed_frame_pose_in_map));
fixed_frame_pose_initialized = true;
}
problem.AddResidualBlock(
CreateAutoDiffSpaCostFunction(constraint_pose),
options_.fixed_frame_pose_use_tolerant_loss()
? new ceres::TolerantLoss(
options_.fixed_frame_pose_tolerant_loss_param_a(),
options_.fixed_frame_pose_tolerant_loss_param_b())
: nullptr,
C_fixed_frames.at(trajectory_id).data(), C_nodes.at(node_id).data());
}
}
// Solve.
//方程非线性优化开始
ceres::Solver::Summary summary;
ceres::Solve(
common::CreateCeresSolverOptions(options_.ceres_solver_options()),
&problem, &summary);
if (options_.log_solver_summary()) {
LOG(INFO) << summary.FullReport();
}
// Store the result.
//结果都保存在C_submaps、C_nodes、C_fixed_frames、C_landmarks里
for (const auto& C_submap_id_data : C_submaps) {
submap_data_.at(C_submap_id_data.id).global_pose =
ToPose(C_submap_id_data.data);
}
for (const auto& C_node_id_data : C_nodes) {
node_data_.at(C_node_id_data.id).global_pose_2d =
ToPose(C_node_id_data.data);
}
for (const auto& C_fixed_frame : C_fixed_frames) {
trajectory_data_.at(C_fixed_frame.first).fixed_frame_origin_in_map =
transform::Embed3D(ToPose(C_fixed_frame.second));
}
for (const auto& C_landmark : C_landmarks) {
landmark_data_[C_landmark.first] = C_landmark.second.ToRigid();
}
//LOG(INFO) << "Peak.ding landmark_data_ size " << landmark_data_.size();
}