1 of 39

Ceres Solver를 활용한 최적화

Jinyong Jeong

https://github.com/JinyongJeong/helloceres

2 of 39

Jinyong Jeong

Motion2AI CTO

  • Visual SLAM 기반 IOT device 개발
  • 데이터를 이용한 물류센터 데이터 분석
  • 물류센터 생산성 향상 솔루션 개발

Personal Blog

http://jinyongjeong.github.io/about/

This is a sample text. Insert your desired text here. This is a sample text. Insert your desired text here. This is a sample text. This is a sample text. Insert your desired text here.

Company Vision

This is a sample text. Insert your desired text here. This is a sample text. Insert your desired text here. This is a sample text. This is a sample text. Insert your desired text here.

3 of 39

Motion2Ai

Real-time Localization

Data Analysis & Report

Safety Warning & Report

This is a sample text. Insert your desired text here. This is a sample text. Insert your desired text here. This is a sample text. This is a sample text. Insert your desired text here.

Company Vision

This is a sample text. Insert your desired text here. This is a sample text. Insert your desired text here. This is a sample text. This is a sample text. Insert your desired text here.

4 of 39

Linear Algebra

Non-homogeneous System

This is a sample text. Insert your desired text here. This is a sample text. Insert your desired text here. This is a sample text. This is a sample text. Insert your desired text here.

Homogeneous System

5 of 39

Linear Algebra

Plane Fitting Problem

void prepareData(vector<Eigen::Vector3d>& input){

input.push_back(Eigen::Vector3d(1.0, 2.0, -4.777));

input.push_back(Eigen::Vector3d(-3.0, 1.1, 7.477));

input.push_back(Eigen::Vector3d(-3.9, 8.1, 4.6333));

input.push_back(Eigen::Vector3d(-1.9, -8.1, 11.4556));

input.push_back(Eigen::Vector3d(-10.9, -18.1, 45.2333));

input.push_back(Eigen::Vector3d(-20.9, 31.1, 35.855));

input.push_back(Eigen::Vector3d(-21.912, 21.155, 46.514));

input.push_back(Eigen::Vector3d(-27.912, 22.155, 63.0697));

input.push_back(Eigen::Vector3d(-1.912, 30.155, -18.263));

input.push_back(Eigen::Vector3d(-7.912, 20.155, 6.84744));

input.push_back(Eigen::Vector3d(-10.912, 10.155, 23.291));

input.push_back(Eigen::Vector3d(14.912, -51.155, -3.62522));

input.push_back(Eigen::Vector3d(14.912, 91.155, -114.311));

// input.push_back(Eigen::Vector3d(-4.312, 33.135, -16.261)); // large noise

// input.push_back(Eigen::Vector3d(-14.112, 15.152, 28.291)); // large noise

}

​

Ax+By+Cz+D = 0

6 of 39

Linear Algebra

Plane Fitting Problem

void prepareData(vector<Eigen::Vector3d>& input){

input.push_back(Eigen::Vector3d(1.0, 2.0, -4.777));

input.push_back(Eigen::Vector3d(-3.0, 1.1, 7.477));

input.push_back(Eigen::Vector3d(-3.9, 8.1, 4.6333));

input.push_back(Eigen::Vector3d(-1.9, -8.1, 11.4556));

input.push_back(Eigen::Vector3d(-10.9, -18.1, 45.2333));

input.push_back(Eigen::Vector3d(-20.9, 31.1, 35.855));

input.push_back(Eigen::Vector3d(-21.912, 21.155, 46.514));

input.push_back(Eigen::Vector3d(-27.912, 22.155, 63.0697));

input.push_back(Eigen::Vector3d(-1.912, 30.155, -18.263));

input.push_back(Eigen::Vector3d(-7.912, 20.155, 6.84744));

input.push_back(Eigen::Vector3d(-10.912, 10.155, 23.291));

input.push_back(Eigen::Vector3d(14.912, -51.155, -3.62522));

input.push_back(Eigen::Vector3d(14.912, 91.155, -114.311));

// input.push_back(Eigen::Vector3d(-4.312, 33.135, -16.261)); // large noise

// input.push_back(Eigen::Vector3d(-14.112, 15.152, 28.291)); // large noise

}

​

Ax+By+Cz+D = 0

7 of 39

Linear Algebra

Plane Fitting Problem

non-trivial solution

U & V 는 orthogonal matrix

8 of 39

Linear Algebra

Plane Fitting Problem

Nx1

Mx1

k 는 smallest singular value (N번째 행)

Mx1

= k

9 of 39

Linear Algebra

Plane Fitting Problem

non-trivial solution

즉 Homogeneous system 에서의 해는 SVD (Singular Value Decomposition)에서 V matrix의 마지막 column이 Trivial solution!

10 of 39

Linear Algebra

Non-homogeneous System

11 of 39

Linear Algebra

Non-homogeneous System

데이터의 양 = 미지수의 양

(유일 해 존재)

데이터의 양 > 미지수의 양

(일반적인 경우)

Exact solution은 존재 하지 않음

근사해 (Approximate solution)

데이터의 양 < 미지수의 양

(무수히 많은 해 존재)

12 of 39

Linear Algebra

Non-homogeneous System (Over-determined)

How to solve?

  • Gradient Descent
  • Gauss-Newton
  • Levenberg-Marquardt

Least square Problem!

13 of 39

Linear Algebra

Non-homogeneous System (Over-determined)

Least square Problem!

제곱오차함수

(squared error function)

Information matrix

14 of 39

Linear Algebra

Non-homogeneous System (Over-determined)

Least square Problem!

제곱오차함수

(squared error function)

Information matrix

x1 = 왼쪽바퀴 weight

x2 = 오른쪽바퀴 weight

왼쪽바퀴 count

오른쪽 바퀴 count

실제 이동거리

15 of 39

SLAM 의 종류

  1. Filter based SLAM
    1. 입력되는 센서 데이터를 기반으로 현재 state를 업데이트 해 나가는 SLAM 기법
    2. Kalman Filter (링크)
    3. EKF(Extended Kalman Filter) SLAM (링크)

​

​

  • Optimization based SLAM (Graph-based SLAM)
    • 센서 데이터를 기반으로 Map과 Pose에 대한 최적의 전체 state를 추정 (링크)

16 of 39

Graph-based SLAM

  • Front-end
    • 다양한 Input을 받았을 때 데이터를 처리하는 부분
      1. 이미지에서 Feature를 추출하고 여러 이미지에서 대응점 추출
      2. 라이다 데이터에서 Line feature 혹은 Plane feature를 추출

​

​

  • Back-end
    • 여러 센서로부터 얻어진 측정값 값들을 이용해서 최적의 state를 계산
      • Ceres를 이용한 최적화, GTAM을 이용한 graph 최적화 등
      • Loop closing

17 of 39

Solver의 종류

  • Ceres solver
    • SLAM과 SfM (Structure From Motion) 쪽에서 많이 활용
    • Error function 에 대한 customization이 자유롭다

​

  • g2o
    • 사용하기 편리
    • 많은 SLAM 연구에서 활용

​

  • GTSAM
    • Matrix Sparsity를 잘 활용하여 높은 계산 효율
    • ISAM 내장 (Incremental Smoothing and Mapping)
    • 최근 많이 활용하는 IMU preintegration 기능이 내장

https://www.cv-learn.com/20210607-solvers/

18 of 39

Ceres Solver

  • 오늘은 Ceres를 활용한 다양한 최적화 문제를 같이 풀어봅시다!

​

http://ceres-solver.org/

https://github.com/ceres-solver/ceres-solver

​

19 of 39

Ceres Solver Tutorial

github: https://github.com/JinyongJeong/helloceres

​

20 of 39

Ceres Solver Tutorial

Visual Studio Code를 활용한 개발 환경 설정

  • Extension 설치
    • Docker
    • Docker compose
    • Remote Development

​

​

​

​

21 of 39

Ceres Solver Tutorial

1_helloworld

​

Ceres solver는 Non-Linear Least Square 문제를 푸는 Tool

​

​

Residual block

Cost Function

CostFunction *cost_function =

new AutoDiffCostFunction<CostFunctor, 1, 1>(new CostFunctor);

Residual의 수, Parameter의 수

22 of 39

Ceres Solver Tutorial

2_add_residual_1 , 3_add_residual_2

​

첫번째 function: 0.5 ( 10 - x ) ^ 2

​

두번째 function: 0.5 ( x ) ^2

​

두 function의 합을 최소로 만드는 x를 계산! => 예상되는 최종 결과는?

​

첫번째 cost function 의 중요한 경우 첫번째 function 쪽으로 최적화가 되게 하는 방법은?

​

23 of 39

Ceres Solver Tutorial

3_multivariate

​

다변수의 parameter를 입력으로 받기

​

두 변수의 차가 최소화 되도록 최적화

​

​

​

​

​

​

CostFunction *cost_function =

new AutoDiffCostFunction<CostFunctor, 3, 3, 3>(new CostFunctor);

Residual의 수, Parameter의 수

24 of 39

Ceres Solver Tutorial

Analytic & Numeric derivative

​

  1. Analytic Derivative
    1. 최적화 되는 과정에서 최적화의 방향에 대한 정보를 줄수 있는 Jacobian (미분 식)을 계산해서 넣어줌

​

  • Numeric Derivative
    • Jacobian을 넣어주지 않고 Cost function으로만 자동으로 최적화
      1. NumericDiffCostFunction
      2. AutoDiffCostFunction (추천)

​

25 of 39

Ceres Solver Tutorial

Analytic_diff

​

  • f(x) = 10 - x

​

  • f(x) = x^2 - 30x + 10
    • 왜 15가 아닌 0 근처로 최적화가 될까?
    • 초기값이 5.0가 아닌 20.0이라면 결과가 달라질까?

​

  • f(x) = x^4 - 30x^3 + 10x^2 + x + 10
    • 초기 값이 20일 때 결과는?
    • 초기값이 50일때 결과는?

​

​

Let’s draw the function (https://www.wolframalpha.com/)

​

26 of 39

Ceres Solver Tutorial

Analytic_diff vs Auto Diff (Numeric)

​

  • f(x) = x^4 - 30x^3 + 10x^2 + x + 10

​

​

​

=========== Analytic solver ===========

iter cost cost_change |gradient| |step| tr_ratio tr_radius ls_iter iter_time total_time

0 4.089800e+06 0.00e+00 4.72e+06 0.00e+00 0.00e+00 1.00e+04 0 3.10e-05 1.35e-04

1 3.290684e+05 3.76e+06 6.12e+05 1.73e+00 9.20e-01 2.44e+04 1 5.01e-05 2.25e-04

2 2.692007e+04 3.02e+05 8.00e+04 1.08e+00 9.18e-01 5.89e+04 1 1.19e-05 2.51e-04

…

7 1.109799e-06 2.48e-02 6.71e-02 4.87e-03 1.00e+00 1.16e+07 1 8.11e-06 3.48e-04

8 2.342133e-15 1.11e-06 3.08e-06 3.31e-05 1.00e+00 3.48e+07 1 7.87e-06 3.65e-04

Ceres Solver Report: Iterations: 9, Initial cost: 4.089800e+06, Final cost: 2.342133e-15, Termination: CONVERGENCE

x : 5 -> 0.853867

=========== Numaric solver ===========

iter cost cost_change |gradient| |step| tr_ratio tr_radius ls_iter iter_time total_time

0 4.089800e+06 0.00e+00 4.72e+06 0.00e+00 0.00e+00 1.00e+04 0 1.91e-05 4.20e-05

1 3.290684e+05 3.76e+06 6.12e+05 1.73e+00 9.20e-01 2.44e+04 1 3.10e-05 9.11e-05

2 2.692007e+04 3.02e+05 8.00e+04 1.08e+00 9.18e-01 5.89e+04 1 2.00e-05 1.23e-04

…

7 1.109799e-06 2.48e-02 6.71e-02 4.87e-03 1.00e+00 1.16e+07 1 2.00e-05 2.77e-04

8 2.342133e-15 1.11e-06 3.08e-06 3.31e-05 1.00e+00 3.48e+07 1 1.88e-05 3.06e-04

Ceres Solver Report: Iterations: 9, Initial cost: 4.089800e+06, Final cost: 2.342133e-15, Termination: CONVERGENCE

x : 5 -> 0.853867

​

27 of 39

Ceres Solver Tutorial

9_curve_fitting

​

Let’s draw the raw data with scatter plot

(https://www.rapidtables.com/tools/scatter-plot.html)

​

28 of 39

Ceres Solver Tutorial

10_curve_fitting2

​

f(x) = 2x^2 + 3 x + 1

​

Let’s draw the raw data with scatter plot

(https://www.rapidtables.com/tools/scatter-plot.html)

​

​

  • 데이터를 생성할 때 노이즈를 크게 주면 어떻게 될까?

​

  • 최적화 결과가 더욱 정확해 지기 위해서는 어떻게 해야될까?
    • Data의 범위?
    • Data의 갯수?

​

  • Source noise가 클 때 더욱 정확하게 최적화 하는 방법은? (Outlier가 많을 때)

29 of 39

Ceres Solver Tutorial

10_curve_fitting2

​

  • RANSAC (Random Sample Consensus)
    • Outlier에 강하다

30 of 39

Ceres Solver Tutorial

Pose_2d_fixed

​

Graph Optimization 어떤 경우에 Node를 Fix 해야 하는가?

​

  • 기본적으로 첫번째 Node는 반드시 fix 해줘야 한다

​

31 of 39

Ceres Solver Tutorial

Pose_2d_fixed

​

첫번째 Node의 위치를 어떻게 Fix 시킬수 있을까?

(x: 3.0, y: 1.0, yaw: 0.523)

(x: 5.1, y: 2.9, yaw:0.79)

(x: 2.73, y:0.732, yaw:0.26)

32 of 39

Ceres Solver Tutorial

Pose Graph

​

g2o data format

  • Graph SLAM의 raw 데이터 format으로 많이 사용됨
  • 각 Node들의 위치 정보, constraint 의 값 및 Information matrix
  • Information matrix는 Upper triangular value (Symmetric)

​

VERTEX_SE2 0 0.000000 0.000000 0.000000

VERTEX_SE2 1 1.030390 0.011350 -0.012958

VERTEX_SE2 2 2.043445 -0.060422 -0.026183

VERTEX_SE2 3 3.070548 -0.094779 -0.021350

VERTEX_SE2 4 3.079976 0.909609 1.545440

VERTEX_SE2 5 3.091176 1.925681 1.529136

EDGE_SE2 73 74 0.985691 0.011144 -0.001664 44.721360 0.000000 0.000000 44.721360 0.000000 44.721360

EDGE_SE2 74 75 0.981205 -0.005965 0.022669 44.721360 0.000000 0.000000 44.721360 0.000000 44.721360

EDGE_SE2 75 76 -0.008260 0.981841 1.564555 44.721360 0.000000 0.000000 44.721360 0.000000 44.721360

​

​

x y yaw

x y yaw [ Information Matix ]

33 of 39

Ceres Solver Tutorial

Pose Graph

​

Manifold은 무엇인가?

  • 해당 군이 가지는 제약조건 공간 (Constraint Space)

​

  • Quaternion
    • Unit quaternion 성질을 만족해야만 함

​

​

  • SO(3)
    • SO(3) : Special Orthogonal Group (Rotation Matrix)
      • Det(R) = 1
      • Inverse of R == Transpose of R
      • Orthogonal Matrix (inner product = 0)

​

34 of 39

Ceres Solver Tutorial

Pose Graph

​

Loss Function (Robust Kernel)

  • Outlier의 영향을 줄이기 위해 weight를 조절하는 함수

35 of 39

Ceres Solver Tutorial

Pose Graph 2D

​

​

36 of 39

Ceres Solver Tutorial

Pose Graph 3D

​

​

37 of 39

Ceres Solver Summary

  • 사용자가 Error function 을 자유롭게 수정 가능하다

​

  • Graph Optimization 문제 뿐만 아니라 다양한 최적화 문제에 적용 가능하다

​

  • 데이터 양이 많은 대형 non-linear least square 문제에도 잘 동작한다

​

  • 특별한 경우가 아니라면 AutoDiff 최적화 방법을 사용하자 (Analytic < AutoDiff)

​

​

​

​

​

38 of 39

Reference

https://github.com/JinyongJeong/helloceres

https://notes.andrewtorgesen.com/doku.php?id=public:ceres-slides

https://www.cvl.isy.liu.se/education/graduate/opencv/ceres-presentation.pdf

http://ceres-solver.org/features.html

https://www.cv-learn.com/20210607-solvers/

https://velog.io/@nabi4622/Ceres-Solver-%ED%8A%9C%ED%86%A0%EB%A6%AC%EC%96%BC

​

39 of 39