[ SLAM ] Ceres Solver를 이용한 Graph Optimization

0. 머리말

SLAM(Simultaneous Localization and Mapping)은 위치 추정(Localization)과 지도 작성(Mapping)을 동시에 수행하는 것을 의미한다. 위치 추정은 로봇이 자신의 위치를 추정하기 위해 관측된 데이터(observation)를 바탕으로 자신의 위치(pose)를 추정하는 것을 말하며, 추정된 자신의 위치들과 해당 위치에서 관측된 데이터를 이용하여 동시에 지도를 그려나가는 것이 SLAM이다. Graph SLAM은 여러 SLAM 방법 중 하나이며, 수학적 개념인 Graph를 기반으로 하여 SLAM을 수행하기 때문에 Graph SLAM이라 한다.

 

A Graph is made up of vertices (also called nodes) which are connected by edges. (Graph Theory, Wikipedia)

 

Graph SLAM은 크게 두 가지 과정으로 나눌 수 있다. 우선, 로봇의 위치를 추정하여 위치(position + orientation)를 나타내는 Node(또는 Vertex), 그리고 이웃한 Node 사이의 관계인 Constraint를 Edge로 하여, Node와 Edge로 이루어진 그래프를 만든다. 그리고, 특정 조건을 만족하는 경우에 그래프를 최적화하게 된다. 많은 SLAM 기술들이 그래프를 최적화하는 조건으로 로봇이 동일한 위치에 돌아온 것을 이용하며, 이를 Loop Closure이라고 부른다.

Loop Closure ( Raúl Mur-Artal, J. M. M. Montiel and Juan D. Tardós.  ORB-SLAM: A Versatile and Accurate Monocular SLAM System .  IEEE Transactions on Robotics,  vol. 31, no. 5, pp. 1147-1163, 2015. )

오늘은 Loop Closure에서 최적화를 계산하는 여러 가지 방법 중 Ceres Solver를 통해 최적화하는 과정에 대해 알아보도록 하자. Ceres solver는 최적화 문제를 계산하는데 사용하는 도구로, Modeling API와 Solver API으로 이루어져있다. 즉, Ceres는 최적화 문제를 모델링한 뒤에 풀게 된다.

 

---

1. pose_graph_3d

Graph SLAM의 Optimization 문제는 non-linear least squares 문제이다. Ceres에서는 non-linear least sqaures 문제를 푸는 것도 당연히 지원하지만, Graph SLAM에 직접적으로 사용할 수 있는 캡슐화된 소스 코드를 지원한다. pose_graph_3d 링크

 

pose_graph_3d.cc의 코드를 상세하게 분석해보자.

 

pose_graph_3d.cc에서는 최적화 문제를 만들고(BuildOptimizationProblem), 푼다(SolveOptimizationProblem).

 

BuildOptimizationProblem()는 cost function를 생성하고(Create), AddResidualBlock()이랑, SetParameterization() 함수를 사용하여 cost function을 구한다. AddResidualBlock()은 problem에 cost function과 residual(=weighted error) block을 추가하는 함수이며, SetParameterization()은 Ceres와 Eigen에서 quaternion을 사용하는 순서가 달라서 이를 변환해주기 위해 사용하는 함수이다. SetParameterLower/UpperBound()는 index에 위치한 파라미터의 lower/upper bound를 지정한 값으로 설정해주는 함수이다.

 

아래의 두 함수에 대해 알아보도록 하자.

BuildOptimizationProblem(constraints, odomConstraints, ratios, &poses, &problem)

SolveOptimizationProblem(&problem, m_param_ceres_maxIteration)

 

---

2. BuildOptimizationProblem(constraints, poses, problem)

// Constructs the nonlinear least squares optimization problem from the pose
// graph constraints.
void BuildOptimizationProblem(const VectorOfConstraints& constraints,
                              MapOfPoses* poses,
                              ceres::Problem* problem) {
  CHECK(poses != NULL);
  CHECK(problem != NULL);
  if (constraints.empty()) {
    LOG(INFO) << "No constraints, no problem to optimize.";
    return;
  }

  ceres::LossFunction* loss_function = NULL;
  ceres::LocalParameterization* quaternion_local_parameterization =
      new EigenQuaternionParameterization;

  for (VectorOfConstraints::const_iterator constraints_iter =
           constraints.begin();
       constraints_iter != constraints.end();
       ++constraints_iter) {
    const Constraint3d& constraint = *constraints_iter;

    MapOfPoses::iterator pose_begin_iter = poses->find(constraint.id_begin);
    CHECK(pose_begin_iter != poses->end())
        << "Pose with ID: " << constraint.id_begin << " not found.";
    MapOfPoses::iterator pose_end_iter = poses->find(constraint.id_end);
    CHECK(pose_end_iter != poses->end())
        << "Pose with ID: " << constraint.id_end << " not found.";

    const Eigen::Matrix<double, 6, 6> sqrt_information =
        constraint.information.llt().matrixL();
    // Ceres will take ownership of the pointer.
    ceres::CostFunction* cost_function =
        PoseGraph3dErrorTerm::Create(constraint.t_be, sqrt_information);

    problem->AddResidualBlock(cost_function,
                              loss_function,
                              pose_begin_iter->second.p.data(),
                              pose_begin_iter->second.q.coeffs().data(),
                              pose_end_iter->second.p.data(),
                              pose_end_iter->second.q.coeffs().data());

    problem->SetParameterization(pose_begin_iter->second.q.coeffs().data(),
                                 quaternion_local_parameterization);
    problem->SetParameterization(pose_end_iter->second.q.coeffs().data(),
                                 quaternion_local_parameterization);
  }

  // The pose graph optimization problem has six DOFs that are not fully
  // constrained. This is typically referred to as gauge freedom. You can apply
  // a rigid body transformation to all the nodes and the optimization problem
  // will still have the exact same cost. The Levenberg-Marquardt algorithm has
  // internal damping which mitigates this issue, but it is better to properly
  // constrain the gauge freedom. This can be done by setting one of the poses
  // as constant so the optimizer cannot change it.
  
  MapOfPoses::iterator pose_start_iter = poses->begin();
  CHECK(pose_start_iter != poses->end()) << "There are no poses.";
  problem->SetParameterBlockConstant(pose_start_iter->second.p.data());
  problem->SetParameterBlockConstant(pose_start_iter->second.q.coeffs().data());
}

 

2-1. poseGraph3dErrorTerm::Create( constraint.t_be, sqrt_information )

여기서, constraint.t_be는 이전 위치에 대한 현재 위치의 Transformation이고, sqrt_information은 covariance의 역수로 이루어진 information matrix의 cholesky decomposed된 lower triangular matrix이다. sqrt_information에 사용된 covariance는 odometry에 따라서 계산된 값이다.

static ceres::CostFunction* Create(
      const Pose3d& t_ab_measured,
      const Eigen::Matrix<double, 6, 6>& sqrt_information) {
    return new ceres::AutoDiffCostFunction<PoseGraph3dErrorTerm, 6, 3, 4, 3, 4>(
        new PoseGraph3dErrorTerm(t_ab_measured, sqrt_information));
  }

Create() 함수 내에서는 ceres::AutoDiffCostFunction<poseGraph3dErrorTerm, 6, 3, 4, 3, 4> (new poseGraph3dErrorTerm (t_ab_measured, sqrt_information))로 ceres::CostFunction*를 계산하여 이를 return 한다.

ceres::AutoDiffCostFunction에서는 poseGraph3dErrorTerm를 cost function으로 하여 poseGraph3dErrorTerm의 operator()를 사용하고, residual의 크기는 6, 각각의 파라미터 블록의 크기는 3, 4, 3, 4이다. 여기서 residual의 크기가 6인 이유는 residual이 position의 차이(delta_p, 3)과 orientation의 차이(delta_q, 3)로 만들어진 matrix<6, 1>에 sqrt_information을 앞에 곱해서 만들어지기 때문이고, quaternion인 orientation은 imaginary 부분만 들어가게 하는 vec()를 거쳐 크기가 4가 아니라 3이다. 그래서, residual의 크기가 6인 것이다. 그리고, 사용되는 파라미터가 a점에서의 position(p_a)과 orientation(q_a), b점에서의 position(p_b), quaternion(q_b)이므로 파라미터 블록의 크기가 3, 4, 3, 4이다. 따라서, residual은 position과 orientation의 오차를 계산하고, 여기에 covariance의 역수(Information matrix)를 곱해 각 오차의 신뢰성을 고려한다고 볼 수 있다.

 

residuals = (Information matrix)^(1/2) * error

 

AutoDiffConstFunction()는 미리 정의해놓은 operator()에서 cost function을 계산한다. 즉, residual의 계산은 operator() 함수에서 확인할 수 있다. AutoDiffCostFunction()를 사용하는 이유는 Jet 객체와 함께 미분을 계산하기에 용이롭다고 한다.

bool operator()(const T* const p_a_ptr,
                  const T* const q_a_ptr,
                  const T* const p_b_ptr,
                  const T* const q_b_ptr,
                  T* residuals_ptr) const {
    Eigen::Map<const Eigen::Matrix<T, 3, 1>> p_a(p_a_ptr);
    Eigen::Map<const Eigen::Quaternion<T>> q_a(q_a_ptr);

    Eigen::Map<const Eigen::Matrix<T, 3, 1>> p_b(p_b_ptr);
    Eigen::Map<const Eigen::Quaternion<T>> q_b(q_b_ptr);

    // Compute the relative transformation between the two frames.
    Eigen::Quaternion<T> q_a_inverse = q_a.conjugate();
    Eigen::Quaternion<T> q_ab_estimated = q_a_inverse * q_b;

    // Represent the displacement between the two frames in the A frame.
    Eigen::Matrix<T, 3, 1> p_ab_estimated = q_a_inverse * (p_b - p_a);

    // Compute the error between the two orientation estimates.
    Eigen::Quaternion<T> delta_q =
        t_ab_measured_.q.template cast<T>() * q_ab_estimated.conjugate();

    // Compute the residuals.
    // [ position         ]   [ delta_p          ]
    // [ orientation (3x1)] = [ 2 * delta_q(0:2) ]
    Eigen::Map<Eigen::Matrix<T, 6, 1>> residuals(residuals_ptr);
    residuals.template block<3, 1>(0, 0) =
        p_ab_estimated - t_ab_measured_.p.template cast<T>();
    residuals.template block<3, 1>(3, 0) = T(2.0) * delta_q.vec();

    // Scale the residuals by the measurement uncertainty.
    residuals.applyOnTheLeft(sqrt_information_.template cast<T>());

    return true;
  }

 

2-2. AddResidualBlock( cost_function, loss_function, ~ )

Residual을 계산하기 위해 파라미터 블록들을 cost function에 추가하는 함수이다. loss_function = NULL 로 설정되어있다. 파라미터 블록으로는 MapOfPoses의 iterator인 pose_begin_iter, pose_end_iter를 통해, MapOfPoses의 두번째 요소인 pose3d에서 position, quaternion를 각각 가져온다. ( 3, 4, 3, 4 = 파라미터 블록 )

 

2-3. SetParameterization( pose_begin/end_iter->second.q.coeffs().data(), quaternion_local_parameterization)

이건 Ceres와 Eigen에서 다루는 quaternion 요소의 순서가 다르기 때문에 quaternion 파라미터를 설정해주는 것이다. 따라서, quaternion_local_parametrization은 ceres::EigenQuaternionParametrization으로 설정하여, Eigen이 (x,y,z,w) 순서로 quaternion을 다루는 것을 Ceres가 (w,x,y,z) 순서로 다룰 수 있도록 한다.

 

2-4. SetParameterLower/UpperBound(ratios, index, value)

index번째 Lower/UpperBound를 value 값으로 설정한다.

 

2-5. SetParameterBlockConstant( pose_start_iter->second.p.data() / pose_start_iter->second.q.coeffs().data() )

Constant 값으로는 첫번째 pose의 position과 quaternion을 설정한다.

 

---

3. SolvingOptimizationProblem(problem)

// Returns true if the solve was successful.
bool SolveOptimizationProblem(ceres::Problem* problem) {
  CHECK(problem != NULL);

  ceres::Solver::Options options;
  options.max_num_iterations = 200;
  options.linear_solver_type = ceres::SPARSE_NORMAL_CHOLESKY;

  ceres::Solver::Summary summary;
  ceres::Solve(options, problem, &summary);

  std::cout << summary.FullReport() << '\n';

  return summary.IsSolutionUsable();
}

 

3-1. ceres::Solve(options, problem, &summary);

options에 maxIteration을 max_num_iterations로 설정해주고, linear_solver_type은 ceres::SPARSE_NORMAL_CHOLESKY로 설정해준다. 그리고 ceres::Solver::Summary summary로 solve 내용을 저장한다. return 값은 summary.isSolutionUsable().

SPARSE_NORMAL_CHOLESKY로 설정해주면 QR factorization을 통해 구한 upper triangular matrix를 구하여 optimization을 하게 된다.

---

4. References

[1] en.wikipedia.org/wiki/Graph_theory

[2] github.com/ceres-solver/ceres-solver

[3] ceres-solver.org/

[4] Raúl Mur-Artal, J. M. M. Montiel and Juan D. Tardós. ORB-SLAM: A Versatile and Accurate Monocular SLAM System. IEEE Transactions on Robotics, vol. 31, no. 5, pp. 1147-1163, 2015.