[SLAM] Bundle Adjustment의 Jacobian 계산

Bundle Adjustment의 Jacobian 계산


Jacobian with respect to Lie Algebra

일반적으로 최적화를 수행할 때 가장 쉬운 방법은 함수의 미분을 계산하고, 현재의 값에서 미분값이 작아지는 방향으로 값을 변경해 가면서 최적화를 수행하는 방법이다. 이러한 방법을 gradient descent 방법이라고 하는데, Jacobian은 이러한 함수가 multi-variable 일때의 미분을 의미한다.

즉 Jacobian은 multi-variable 문제에서 내가 최적화 하고 싶은 parameter들에 대한 편미분을 matrix로 표현한 것이다.

Graph-based SLAM 뿐만 아니라 BA(Bundle Adjustment), Visual SLAM쪽을 공부하다 보면 계속 이러한 Jacobian을 만나게 되는데 정확히 어떻게 계산이 되는지 알고 넘어가는게 좋을 것 같아서 수식을 정리해 보았다.

수식이 너무 많아서 손필기로 정리하였으며, Error function은 일반적인 BA에서 사용하는 reprojection error에 대한 Jacobian을 구하는 방법이다.

(혹시 수식적으로 틀린 부분이 있거나, 더 간단하게 풀수있는 방법이 있으면 댓글로 알려주세요)

Bundle Adjustment 2

Bundle Adjustment 3

Bundle Adjustment 4

Bundle Adjustment 5

Bundle Adjustment 6

Bundle Adjustment 7

Bundle Adjustment 8

Bundle Adjustment 9

최종 rotation의 Jacobian을 보면 상당히 복잡하다.

이 식을 조금 단순화 시키기 위해서 roation matrix를 단순화 시킨다.

Multiple view geometry 책의 Appendix 6의 A6.9.1에 이런 말이 있다.

Lie algebra t가 작으면, rotation matrix는 다음과 같이 근사 가능하다. \[R = I + [t]_{\times}\]

따라서 근사화된 R에 대해서 jacobian을 구해보면 다음과 같다.

Bundle Adjustment 11

즉 근사화된 Rotation matrix를 이용해서 Jacobian을 구해보면 입력 measurement의 skew symmetric matrix가 됨을 알 수 있다.

대부분의 tracking 문제에서는 계산하고자 하는 state는 두 frame 사이의 relative pose이고, 이 relative는 크지 않다.

따라서 R을 위와같이 근사화 해서 풀 수 있다.

Jacobian with respect to Quaternion

그렇다면 state가 quaternion으로 rotation을 표현한다면 어떻게 될까?

visual SLAM 코드를 공부하기 좋은 Pro-SLAM의 코드는 quaternion으로 rotation state를 표현하고 있으며, w를 제외한 3개로 state를 표현한다.

w는 3개의 state로 recovery가 가능하다.

Point를 Quaternion으로 회전 시키는 식을 미분하면 다음과 같다.

quaternion

state를 3개만으로 표현하였기 때문에 최종 matrix에서 뒤의 3 column 즉 \[2 (v^TaI+va^T-av^T-w[a]_{\times})\]

만 해당이 된다.

위에서 설명한 것 처럼 대부분의 tracking 문제에서는 두 frame 사이의 relative를 계산하는 것이며, 이 relative는 크지 않다.

따라서 Jacobian은 회전이 0 (identity) 에서 미분한 값으로 단순화 할 수 있다. (찾고자 하는 relative rotation이 작기 때문에)

회전이 0, identity일 때 quaternion의 w 는 1, 그리고 i,j,k term (위 식에서는 v vector)은 0이 된다.

따라서 quaternion의 jacobian은 다음이 된다. \[-2[a]_{\times}\]

Pro-SLAM 코드에서 rotation state는 quaternion으로 표현되기 때문에 위의 Jacobian을 사용한다.

Rotation의 Jacobian을 계산하는 코드를 보면 일치함을 알 수 있다.

//ds update total error
_total_error += _errors[u];

//ds compute the jacobian of the transformation
Matrix3_6 jacobian_transform;

//ds translation contribution (will be scaled with omega)
// proj(M_i)를 p_cam 으로 미분한 부분에서 translation부분
jacobian_transform.block<3,3>(0,0) = _weights_translation[u]*Matrix3::Identity();

//ds rotation contribution - compensate for inverse depth (far points should have an equally strong contribution as close ones)
// *******************여기가 Rotation의 Jacobian 계산하는 부분, 위에서 설명한대로 넣어주고 있다 **********************
jacobian_transform.block<3,3>(0,3) = -2*srrg_core::skew(sampled_point_in_camera_left);

//ds precompute
// Intrinsic matrix를 곱해주는 부분
const Matrix3_6 camera_matrix_per_jacobian_transform(_camera_calibration_matrix*jacobian_transform);

//ds precompute
const real inverse_sampled_c_left  = 1/sampled_c_left;
const real inverse_sampled_c_right = 1/sampled_c_right;
const real inverse_sampled_c_squared_left  = inverse_sampled_c_left*inverse_sampled_c_left;
const real inverse_sampled_c_squared_right = inverse_sampled_c_right*inverse_sampled_c_right;

//ds jacobian parts of the homogeneous division: left
Matrix2_3 jacobian_left;
// 각 카메라의 coordinate로 이동한 measurement로 만든 matrix (미분 term에서 가장 앞에 곱해지는 matrix)
jacobian_left << inverse_sampled_c_left, 0, -sampled_abc_in_camera_left.x()*inverse_sampled_c_squared_left,
           0, inverse_sampled_c_left, -sampled_abc_in_camera_left.y()*inverse_sampled_c_squared_left;

//ds we compute only the contribution for the horizontal error: right
Matrix2_3 jacobian_right;
jacobian_right << inverse_sampled_c_right, 0, -sampled_abc_in_camera_right.x()*inverse_sampled_c_squared_right,
            0, inverse_sampled_c_right, -sampled_abc_in_camera_right.y()*inverse_sampled_c_squared_right;

//ds assemble final jacobian
_jacobian.setZero();

//ds we have to compute the full block
// 최종 p_cam으로 미분 한 matrix, 여기에선 left, right의 reporjection error를 concat해서 사용
_jacobian.block<2,6>(0,0) = jacobian_left*camera_matrix_per_jacobian_transform;

//ds we only have to compute the horizontal block
_jacobian.block<2,6>(2,0) = jacobian_right*camera_matrix_per_jacobian_transform;

//ds precompute transposed
const Matrix6_4 jacobian_transposed(_jacobian.transpose());

//ds update H and b
// 최적화 문제를 풀기 위해 계산된 jacobian으로 hessian 계산 (이 부분은 이전 Graph SLAM post 참고)
_H += jacobian_transposed*_omega*_jacobian;
_b += jacobian_transposed*_omega*error;

아래의 github 링크는 Pro-SLAM 에 대해서 한글로 주석을 달아놓은 code이다.

https://github.com/JinyongJeong/proSLAM.git


[Ubuntu] Ubuntu CPU Frequency 최적화 하기

Ubuntu CPU Frequency 최적화 하기


Ubuntu CPU Frequency

일반적으로 전력 관리를 위해 CPU의 frequency는 가변적으로 설정되어 있다. 일반적으로 800MHz-4200MHz정도로 설정되어 있다. 이런 CPU의 Frequency는 OS가 자동적으로 load을 보고 조절하는 것으로 알고 있으나, 실제 Ubuntu에서 구현된 code를 돌려보면 CPU의 frequency의 변화에 의해서 함수 구동 속도가 변하는 현상을 볼 수 있다. 일반적으로 Code가 최대 performance로 돌아야 하는 경우는 CPU frequency가 일정하게 최대 속도인것이 좋다.

현재 frequency 및 min/max frequency는 아래 명령어로 확인 가능하다

watch -n 0.1 "lscpu | grep MHz"

Ubuntu CPU Frequency 조절

다음 package를 설치한다.

sudo apt-get install cpufrequtils

CPU frequnecy min 설정

sudo cpufreq-set -d 0.8Ghz

CPU frequnecy max 설정

sudo cpufreq-set -u 4.2Ghz

아래 명령어를 통해 현재 CPU의 max frequency를 확인 후 약간 아래값으로 minimum 값을 설정해주면 일정한 속도의 code 성능을 확인할 수 있다.

watch -n 0.1 "lscpu | grep MHz"

이 방법을 통해 minimum frequency를 높이게 되면 노트북의 경우 배터리 성능에 영향이 있을 수도 있으니, 적절히 사용하면 좋을 것 같다.


[SLAM] IMU Filter (AHRS)

IMU Filter 관련 정리


IMU는 Angular velocity를 측정하는 Gyroscope, 가속도를 측정하는 Accelerometer, 자기장을 측정하는 Magnetometer 센서를 포함하고 있다. IMU Filter (AHRS: Attitude Heading Reference System)는 이 데이터를 이용해서 센서의 Global 회전 (Orientation)을 계산한다.

관련 ROS package http://wiki.ros.org/imu_filter_madgwick

관련 Report https://www.x-io.co.uk/res/doc/madgwick_internal_report.pdf

IMU Filter (AHRS) from Jinyong Jeong Steve



[SLAM survey] 2019년 11월 Arxiv SLAM 논문 정리

2019년 11월 Arxiv SLAM 논문 정리


A Survey of Simultaneous Localization and Mapping

LiDAR-enhanced Structure-from-Motion




[SLAM survey] IROS2019 SLAM 관련 논문 정리

IROS2019 SLAM 관련 논문 정리


SuMa++Efficient LiDAR-based Semantic SLAM

SuMa (Efficient Surfel-Based SLAM using 3D Laser Range Data in Urban Environments) - RSS2018

RangeNet++: Fast and Accurate LiDAR Semantic Segmentation

Robot Localization in Floor Plans Using a Room Layout Edge Extraction Network

Free-Space Features: Global Localization in 2D Laser SLAM Using Distance Function Maps

FLAME: Feature-Likelihood Based Mapping and Localization for Autonomous Vehicles

ORBSLAM-Atlas: a robust and accurate multi-map system

Exploiting Sparse Semantic HD Maps for Self-Driving Vehicle Localization

Lane Marking Learning based on Crowdsourced Data

Fusing Lidar Data and Aerial Imagery with Perspective Correction for Precise Localization in Urban Canyons

Automatic Spatial Template Generation for Realistic 3D Modeling of Large-Scale Indoor Spaces

A Robust Laser-Inertial Odometry and Mapping Method for Large-Scale Highway Environments

Visual-Inertial Localization with Prior LiDAR Map Constraints

Degeneracy-Aware Factors with Applications to Underwater SLAM

Visual-Inertial Odometry with Point and Line Features

LIC-Fusion: LiDAR-Inertial-Camera Odometry

Stereo Visual Inertial LiDAR Simultaneous Localization and Mapping




Pagination