cartographer OptimizationProblem2D

所处：

MapBuilder cartographer/mapping/inter/optimization/optimization_problem_2d.cc /.h中

 Pose_graph_2d.cc 中 RunOptimization（）

optimization_problem_->Solve(data_.constraints, GetTrajectoryStates(), data_.landmark_nodes);

代码：

1、Solve

void OptimizationProblem2D::Solve(
    const std::vector<Constraint>& constraints,
    const std::map<int, PoseGraphInterface::TrajectoryState>&
        trajectories_state,
    const std::map<std::string, LandmarkNode>& landmark_nodes) 

1.定义ceres非线性优化的变量  

ceres::Problem::Options problem_options;
ceres::Problem problem(problem_options);

2.设置起点，将ceres数据移动到SubmapSpec中。

  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));
    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());
    }
  }
  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));
    problem.AddParameterBlock(C_nodes.at(node_id_data.id).data(), 3);
    if (frozen) {
      problem.SetParameterBlockConstant(C_nodes.at(node_id_data.id).data());
    }
  }

3.建立子图数据的參差问题  建立Node节点的參差问题

4.为子图内和子图间约束添加成本函数。

5.为landmark添加代价函数。 

  AddLandmarkCostFunctions(landmark_nodes, freeze_landmarks, node_data_,
                           &C_nodes, &C_landmarks, &problem,
                           options_.huber_scale());

6.  如果没有里程计，添加违反测距或连续节点之间更改的代价

  for (auto node_it = node_data_.begin(); node_it != node_data_.end();) {

7.    for (++node_it; node_it != trajectory_end; ++node_it) 

8.根据里程计（如果可用）添加相对姿势约束。     

std::unique_ptr<transform::Rigid3d> relative_odometry =
          CalculateOdometryBetweenNodes(trajectory_id, first_node_data, second_node_data);

9.基于连续的本地SLAM姿势添加相对姿势约束。++1结束

10.优化   ceres::Solve   存储结果

2、AddLandmarkCostFunctions

void AddLandmarkCostFunctions(
    const std::map<std::string, LandmarkNode>& landmark_nodes,
    bool freeze_landmarks, const MapById<NodeId, NodeSpec2D>& node_data,
    MapById<NodeId, std::array<double, 3>>* C_nodes,
    std::map<std::string, CeresPose>* C_landmarks, ceres::Problem* problem,
    double huber_scale) 

1.for (const auto& landmark_node : landmark_nodes)   //所有landmark

2.如果landmark数据为空或者 不优化landmark  continue

    if (!landmark_node.second.global_landmark_pose.has_value() && freeze_landmarks) {
      continue; }

3.for (const auto& observation : landmark_node.second.landmark_observations)  //本次视眼中观察到的landmark

4.landmark的观察时间应该比创建轨迹的时间大，否则 continue

5.在landmark观测之前和之后找到轨迹节点。

6.进行了地标观测，但下一个轨迹节点有还没有添加。

7.增加参数模块为landmark Id 如果他们还没添加之前

8.      problem->AddResidualBlock( LandmarkCostFunction2D::CreateAutoDiffCostFunction(
           observation, prev->data, next->data),new ceres::HuberLoss(huber_scale), prev_node_pose->data(),
          next_node_pose->data(), C_landmarks->at(landmark_id).rotation(), C_landmarks->at(landmark_id).translation());

3.CalculateOdometryBetweenNodes

std::unique_ptr<transform::Rigid3d>
OptimizationProblem2D::CalculateOdometryBetweenNodes(
    const int trajectory_id, const NodeSpec2D& first_node_data,
    const NodeSpec2D& second_node_data) const

1.如果有轨迹则执行，否则返回nullptr

2.计算   first_node_odometry  second_node_odometry 位姿

const std::unique_ptr<transform::Rigid3d> first_node_odometry =
        InterpolateOdometry(trajectory_id, first_node_data.time);

3.   计算里程计的相对位姿

 transform::Rigid3d relative_odometry =
          transform::Rigid3d::Rotation(first_node_data.gravity_alignment) *
          first_node_odometry->inverse() * (*second_node_odometry) *
          transform::Rigid3d::Rotation(
              second_node_data.gravity_alignment.inverse());

4.return relative_odometry

4、InterpolateOdometry

std::unique_ptr<transform::Rigid3d> OptimizationProblem2D::InterpolateOdometry(
    const int trajectory_id, const common::Time time) const

1.  如果是开始返回刚开始的位姿

2. return absl::make_unique<transform::Rigid3d>(
      Interpolate(transform::TimestampedTransform{prev_it->time, prev_it->pose},
                  transform::TimestampedTransform{it->time, it->pose}, time) .transform);