Visual SLAM 이론에서 실습까지 (Visual SLAM From Theory to Practice) 연습문제 풀이, 3장 3D 강체 변환

1. 회전 행렬이 직교 행렬인지 증명하라.

n차원이라 가정하면, n차원 좌표계에서 n개의 서로 다른 axis를 표현하는 벡터들의 집합 $V$를 다음과 같이 표현할 수 있다.

$$V = \{v_1, v_2, ... , v_n\}$$

$$ v_1^T = [1, 0, 0, ...], v_2^T = [0, 1, 0, ...], ... , v_n^T = [0, 0, ... , 1]$$

위 집합은 axis들의 집합이기 때문에 자명하게 직교집합이다. (직교집합은 모든 원소인 벡터들이 서로 수직 관계인 집합을 말한다)

$V$의 원소벡터들을 임의의 n차원 회전행렬 $R_n$으로 회전시키면 다음과 같은 벡터 집합을 얻을 수있다.

$$V_{rot} = \{R_nv_1, R_nv_2, ... , R_nv_n\}$$

회전 행렬은 정의에 의해 강체 변환 행렬이므로, 강체 내부의 벡터들의 각도들은 변하지 않는다. 따라서 $V_{rot}$ 또한 직교집합이다.

$R_n$은 $n \times n$ 행렬이기 때문에 다음과 같이 표현할 수 있다.

$$R_n = \ [ r_1 r_2 ... r_n \ ]$$

이를 이용해 $V_{rot}$를 다시 표현하면 다음과 같다.

$$V_{rot} = \{ r_1, r_2, ... r_n \}$$

$V_{rot}$은 직교집합이므로, 원소벡터들은 서로 직교한다. 따라서, $R_n$의 모든 열벡터들이 서로 직교하므로 직교행렬의 정의에 의해 $R_n$은 직교행렬이다.

2. 로드리게스 공식의 추론 과정을 찾아 이해하라

회전 과정 $R(v)$을 말로 표현하면 다음과 같다. "3차원 벡터 $v$를 크기가 1인 회전축인 3차원 벡터 $k$를 중심으로 각도 $\theta$만큼 회전한다"

$$ v_{rot} = R(v)$$

먼저, 로드리게스 공식 유도의 메인 아이디어는 $v$를 분해하여 회전 과정에서 변하는 부분과 변하지 않는 부분을 나누어 생각하는 것이다. 회전 과정에서 변하지 않는 부분은 $k$에 대한 평행한 벡터이고, 변하는 부분은 $k$에 대해 수직인 벡터이다.

$$ v_{par} = (v \cdot k) k$$

$$ v_{perp} = v - v_{par} = v - (v \cdot k) k$$

이제, $v_{perp}$를 $\theta$만큼 회전 시킬 것이다. 먼저, 계산을 위해 회전 평면의 두 축을 정의해야 한다. 일단, 축 하나를 $v_{perp} \over {|v_{perp}|}$로 하자. 그러면, 나머지 축은 ${k \times v_{perp}} \over {|v_{perp}|}$이 된다. 이 두 축을 이용해서 $v_{rot, perp}$를 다음과 같이 계산할 수 있다.

$$ \begin{align*} v_{rot, perp} &= |v_{perp}| (cos(\theta) {v_{perp} \over {|v_{perp}|}} + sin(\theta) {{k \times v_{perp}} \over {|v_{perp}|}} ) \\ &= cos(\theta) v_{perp} + sin(\theta) k \times v_{perp} \end{align*} $$

회전축에 평행한 벡터는 회전 이후에도 일정하므로 다음과 같다.

$$v_{rot, par} = v_{par}$$

이제 $v_{rot}$를 계산할 수 있다.

$$ \begin{align*} v_{rot} &= v_{rot, par} + v_{rot, perp} \\ &= (v \cdot k) k + cos(\theta) v_{perp} + sin(\theta) k \times v_{perp} \\ &= (v \cdot k) k +cos(\theta)(v - (v \cdot k) k) + sin(\theta) k \times(v - (v \cdot k) k) \\ &= (v \cdot k) k + cos(\theta)(v - (v \cdot k) k) + sin(\theta)(k \times v - (v \cdot k) k \times k) \\ &= (v \cdot k) k + cos(\theta)(v - (v \cdot k) k) + sin(\theta)k \times v \\ &= cos(\theta)v + (1 - cos(\theta))(v \cdot k) k + sin(\theta)k \times v \end{align*} $$

이로써 $v_{rot} = R(v)$는 유도가 됐다. 추가로 $v_{rot} = Rv$ 형태로 $v$에 대해 선형으로 표현할 수 있는 회전행렬을 계산할 수 있다. 유도한 결과에서 세 개의 항을 3x3행렬과 $v$의 곱으로 표현하면 된다.

$$ \begin{align*} cos(\theta)v &= cos(\theta) \begin{pmatrix} 1 & 0 & 0 \\ 0 & 1 & 0 \\ 0 & 0 & 1 \end{pmatrix}v \end{align*} $$
$$ \begin{align*} (1 - cos(\theta))(v \cdot k) k &= (1 - cos(\theta)) (v_1k_1+ v_2k_2 + v_3k_3) \begin{pmatrix} k_1 \\ k_2 \\ k_3 \end{pmatrix} \\ &= (1 - cos(\theta)) \begin{pmatrix} v_1k_1k_1 + v_2k_2k_1 + v_3k_3k_1 \\ v_1k_1k_2 + v_2k_2k_2 + v_3k_3k_2 \\v_1k_1k_3 + v_2k_2k_3 + v_3k_3k_3 \end{pmatrix} \\ &= (1 - cos(\theta)) \begin{pmatrix} k_1^2 & k_1k_2 & k_1k_3 \\ k_1k_2 & k_2^2 & k_2k_3 \\ k_1k_3 & k_2k_3 & k_3^2 \end{pmatrix} v \end{align*} $$
$$ \begin{align*} sin(\theta)k \times v &= sin(\theta) \begin{pmatrix} 0 & -k_3 & k_2 \\ k_3 & 0 & -k_1 \\ -k_2 & k_1 & 0 \end{pmatrix}v \end{align*} $$

이제 위 세 개의 항을 합쳐주면 회전행렬 $R$을 구할 수 있다.

$$ R = cos(\theta) \begin{pmatrix} 1 & 0 & 0 \\ 0 & 1 & 0 \\ 0 & 0 & 1 \end{pmatrix} + (1 - cos(\theta)) \begin{pmatrix} k_1^2 & k_1k_2 & k_1k_3 \\ k_1k_2 & k_2^2 & k_2k_3 \\ k_1k_3 & k_2k_3 & k_3^2 \end{pmatrix} + sin(\theta) \begin{pmatrix} 0 & -k_3 & k_2 \\ k_3 & 0 & -k_1 \\ -k_2 & k_1 & 0 \end{pmatrix} $$

3. 쿼터니언이 점에 대한 회전임을 확인하고 결과는 허수부 쿼터니언(실수부가 0)이므로 여전히 공간 점에 해당함을 보여라. 식 (3.33)을 참조하시오

여기서는 책에 있는 식 (3.37)을 참고하면 된다.

$$ \begin{align*} p^{'} &= q^+{q^{-1}}^{\oplus}p \\ &= \begin{pmatrix} 1 & 0 \\ 0^T & R \end{pmatrix} \begin{pmatrix} 0 \\ v \end{pmatrix} \\ &= \begin{pmatrix} 0 \\ v^{'} \end{pmatrix} \end{align*} $$

따라서, 주어진 쿼터니언을 이용한 회전이 점에 대한 회전이고 실수부가 0임을 알 수 있다.

4. 테이블을 그려 회전 행렬, 회전 각도, 오일러 각도 및 Quaternion의 변환 관계를 요약하라.

영어 번역본도 확인해봤는데, rotation angle이라는 말을 쓰고 있었다. 아마 중국어에서 영어로 번역하면서 axis-angle 번역에 오류가 있었을 것으로 생각된다. 회전 행렬은 서로 모두 변환이 가능하며, 나머지 중에서는 axis-angle과 Quaternion만이 서로 변환이 가능하다.
(이거는 조사는 안하고 기억나는 대로 적었다. 혹시나 틀렸으면 지적부탁드린다)

5. 큰 Eigen 행렬이 있다고 가정하고 왼쪽 위 모서리 3x3의 블록을 제거하고 3x3의 단위 행렬을 할당하려고 한다. C++ 프로그래밍으로 구현하시오.

eigen 라이브러리에 Eigen::MatrixXd::block()과 Eigen::MatrixXd::setIdentity() 메소드를 이용하면 된다. 직접 assign하는 방법도 있다.

Eigen::MatrixXd big_matrix;

// method call
big_matrix.block(0, 0, 3, 3).setIdentity();

// assigning
big_matrix.block(0, 0, 3, 3) = Eigen::Matrix3d::Identity();

6. 일반 선형 방정식 $Ax=b$가 $x$의 고유한 해를 갖는 경우는 언제인가? 수치상으로 해결하는 방법은? Eigen에서 구현해보시오.

그리고 $Ax=b$가 $x$의 고유한 해를 가지는 충분조건은 rank($A$) = rank($A|b$)이면서 rank($A$)가 full column rank를 가질 경우이다. 이는 고유한 해라는 말을 해를 가지는 경우와 해를 하나만 가지는 경우들의 충분조건의 교집합 표현한 것이다. 따라서 증명은, 해를 가지는 경우와 해를 하나만 가지는 경우들 각각의 충분조건임을 증명한다.(사실 모두 필요충분조건이지만 충분조건임만을 증명한다)

 

1. Augmented matrix $A|b$에 대해서 rank($A$) = rank($A|b$)이면 해를 가진다.

이는 귀류법으로 쉽게 증명할 수 있다.

해를 가지지 않는다는 것은 $A$와 $A|b$을 reduced row echelon form으로 만들었을 때, 어떤 row가 왼쪽 n개의 column들에선 0 값으로 채워져있지만, 마지막 column에선 0이 아닌 값을 가진 다는 것과 같다. 이 경우에는, rank($A$) < rank($A|b$)가 된다. 그리고 rank($A$) > rank($A|b$)는 존재할 수 없는 경우이므로, 해를 가지지 않으면 rank($A$) != rank($A|b$)이다.
따라서, rank($A$) = rank($A|b$)이면, 해를 가진다.

2. rank($A$)가 full column rank일 경우, 유일한 해를 가지거나 해가 없다.

full column rank라는 것은 모든 column들이 pivot을 가지고 있다는 것이므로 free variable이 없다는 것과 같다. 따라서, free variable이 없으면 해가 유일하거나 해를 가지지 않는다.

위 두가지를 합치면, rank($A$) = rank($A|b$)이면서 rank($A$)가 full column rank이면 $Ax=b$가 $x$는 고유한 해를 가진다.

int m, n;
Eigen::MatrixXd A;
Eigen::VectorXd b;
Eigen::MatrixXd aug(A.rows(), A.cols() + 1);
aug << A, b;

Eigen::FullPivLU<Eigen::MatrixXd> lu_decomp(A);
Eigen::FullPivLU<Eigen::MatrixXd> lu_decomp_aug(aug);

if (lu_decomp.rank() == lu_decomp_aug.rank() && lu_decomp.rank() == A.cols()) {
  printf("A unique solution!");
}

나는 decomposition을 두 번 사용해서 구현했는데, decomposition을 $A|b$에 대해서 한 번만 하고서 계산할 수 있을 것 같은데 방법을 아는 분은 댓글로 남겨주시면 감사하겠다.