[3D CV] se3, SE3, so3, SO3

[Blog]

본 포스트는 위 블로그 내용을 참조하여 정리한 내용임을 밝힙니다.

Lie Group SO(3), SE(3)

Lie Algebra so(3), se(3)

결론부터 말하자면 SE3에서 회전 및 이동변환을 구할때 한두번 계산할때는 큰 문제가 없습니다. 하지만 SLAM에서와 같이 매우 빠른시간에 연속적으로 R|T를 변수로 하여 pose를 최적화를 통해 구하는 경우에, SE3상에서는 constraints로 인해 최적화 할때마다 이 조건들이 맞는지 확인해야 합니다. 하지만 se3에서는 이런 constraints가 없고 linear space이기 때문에 부담없이 미분을 적용하여 최적화를 수행할 수 없습니다. 즉, 최적화 변수를 se3(w,v)로 놓고 계산된 증분량을 exp(zeta)를 통해 SE3로 변환하면 됩니다. constrained optimization이 unconstrained optimization으로 변환되는거죠. 또한 최근에 learning based SLAM같은 경우 뉴럴넷에서 R|T를 편미분해야하니 Lie pytorch를 이용하여 Lie group을 활용합니다.

SE(3)는 SO(3)를 포함하는 관계입니다.

SO(3)는 순수한 회전만을 나타내고, SE(3)는 SO(3)의 회전과 추가적인 이동 벡터를 포함합니다.

즉, SE(3)는 강체의 회전과 이동을 모두 표현할 수 있습니다. (Rigid-body transform)

SE(3)와 SO(3)는 각각 공간에서의 운동을 설명하는 데 사용되는 수학적 그룹입니다. 이 두 그룹은 로봇공학, 컴퓨터 비전, 그리고 인공지능 분야에서 특히 중요합니다.

SO(3)는 3차원 공간에서의 회전을 표현하는 그룹으로, ‘Special Orthogonal Group of degree 3’의 약어입니다. 이 그룹의 원소는 3x3 직교 행렬로, 행렬식이 1인 행렬을 말합니다. 이는 순수한 회전을 나타냅니다.

SE(3)는 3차원 공간에서의 회전과 이동을 함께 표현할 수 있는 그룹으로, ‘Special Euclidean Group of degree 3’의 약어입니다. SE(3)는 SO(3)의 회전과 추가적인 이동 벡터를 포함합니다. 즉, SE(3)는 강체의 회전과 이동을 모두 표현할 수 있습니다.

결론적으로, SE(3)는 SO(3)를 포함하는 개념이며, 이동(translations)을 추가로 다룹니다. SO(3)는 SE(3) 내에서 회전 부분에 해당합니다.

정리하면 se(3)는 회전인 so(3)를 포함하는 관계이고, SE(3)는 회전인 SO(3)를 포함하는 관계이므로, se(3) to SE(3) 변환 관계에서 순수한 회전인 so(3), SO(3)에 추가적으로 이동 벡터를 표현할 수 있습니다.

코드: Lie Algebra인 se(3)는 linear space 여서 Lie Group인 SE(3)에 존재하는 constraints가 없기 때문에, Lie Algebra인 se(3)에 대해서 미분을 계산하고 Lie Group인 SE(3)로 변환하는 과정을 거쳐 강체의 회전과 이동을 모두 표현할 수 있습니다.

3DGS에서 코드로 se(3)에 대한 파라미터로 최적화하는 과정은 w, v 로 nn.Parameter로 정의하고, 이를 se3_to_SE3 함수로 w,v의 변수로 SE(3)를 정의하는 방식을 사용함을 확인할 수 있습니다.

# gaussian_splatting/scene/cameras.py
import torch
from torch import nn
import numpy as np
from utils.graphics_utils import getWorld2View2, getProjectionMatrix, se3_to_SE3

# Define a class named Camera_Pose. The code is based on the camera_transf class in iNeRF. You can refer to iNeRF at https://github.com/salykovaa/inerf.
class Camera_Pose(nn.Module):
    def __init__(self,start_pose_w2c, FoVx, FoVy, image_width, image_height,
             trans=torch.tensor([0.0, 0.0, 0.0]), scale=1.0,
             ):
        super(Camera_Pose, self).__init__()

        self.FoVx = FoVx
        self.FoVy = FoVy

        self.image_width = image_width
        self.image_height = image_height

        self.zfar = 100.0
        self.znear = 0.01

        self.trans = trans
        self.scale = scale
        self.cov_offset = 0
        
        self.w = nn.Parameter(torch.normal(0., 1e-6, size=(3,)).to(start_pose_w2c.device))
        self.v = nn.Parameter(torch.normal(0., 1e-6, size=(3,)).to(start_pose_w2c.device))
        
        self.forward(start_pose_w2c)
    
    def forward(self, start_pose_w2c):
        deltaT=se3_to_SE3(self.w,self.v)
        self.pose_w2c = torch.matmul(deltaT, start_pose_w2c.inverse()).inverse()
        self.update()
    
    def current_campose_c2w(self):
        return self.pose_w2c.inverse().clone().cpu().detach().numpy()

    def update(self):
        self.world_view_transform = self.pose_w2c.transpose(0, 1).cuda()
        self.projection_matrix = getProjectionMatrix(znear=self.znear, zfar=self.zfar, fovX=self.FoVx, fovY=self.FoVy).transpose(0,1).cuda()
        self.full_proj_transform = (self.world_view_transform.unsqueeze(0).bmm(self.projection_matrix.unsqueeze(0))).squeeze(0)
        self.camera_center = self.world_view_transform.inverse()[3, :3]

# gaussian_splatting/utils/graphics_utils.py
def se3_to_SE3(w,v): # [...,3]
    deltaT = torch.zeros((4,4)).cuda()
    wx = skew_symmetric(w)
    theta = w.norm(dim=-1)
    I = torch.eye(3,device=w.device,dtype=torch.float32)
    A = taylor_A(theta)
    B = taylor_B(theta)
    C = taylor_C(theta)
    deltaT[:3, :3] = I+A*wx+B*wx@wx
    V = I+B*wx+C*wx@wx
    deltaT[:3, 3] = V@v
    deltaT[3, 3] = 1.
    return deltaT

따라서 3DGS에서도 se(3)에서 미분값을 계산하고 se3_to_SE3 함수로 최종적으로는 SE(3)로 반환하는 것을 확인할 수 있습니다.

SO(3)

\[SO(3) = \{ R \in \mathbb{R}^{3 \times 3} | R^T R = I, \text{det}(R) = 1 \}\]

여기서 $R$은 3x3 직교 행렬이며, $R^T$는 $R$의 전치 행렬을 나타냅니다. 이 그룹은 회전을 나타내며, 회전 행렬은 직교 행렬이며 행렬식이 1입니다.

so(3)

\[so(3) = \{ \omega \in \mathbb{R}^{3 \times 3} | \omega^T = -\omega \}\]

여기서 $\omega$는 3x3 반대칭 행렬을 나타내며, 이는 so(3)의 요소들입니다. so(3)는 회전의 미소 변화를 나타내는 대수적 구조로서, $\mathbb{R}^3$의 벡터를 3x3 반대칭 행렬로 변환함으로써 SO(3) 그룹의 접공간을 형성합니다. 이 대수 구조는 3차원 공간에서의 무한소 회전을 나타내며, 이는 SO(3)의 연속적인 회전 운동의 선형 근사를 제공합니다.

SE(3)

\[SE(3) = \{ T \in \mathbb{R}^{4 \times 4} | T = \begin{bmatrix} R & t \\ 0 & 1 \end{bmatrix}, R \in SO(3), t \in \mathbb{R}^3 \}\]

여기서 $T$는 4x4 변환 행렬을 나타내며, $R$은 회전 행렬, $t$은 이동 벡터를 나타냅니다. 이 그룹은 이동과 회전을 결합한 변환을 나타내며, 변환 행렬은 직교 행렬과 이동 벡터로 구성됩니다.

se(3)

\[se(3) = \{ \xi \in \mathbb{R}^6 | \xi = \begin{bmatrix} \omega \\ v \end{bmatrix}, \omega, v \in \mathbb{R}^3 \}\]

여기서 $\xi$는 6차원 벡터이며, $\omega$는 회전 속도 벡터, $v$는 이동 속도 벡터를 나타냅니다. 이 그룹은 작은 회전과 작은 이동을 나타내는 벡터 공간으로, SE(3) 그룹에서의 작은 변환을 나타냅니다.

se3에서 SE3로의 변환 함수 설명

함수 skew_symmetric(w)

• 입력: 각속도 벡터 w
• 출력: w의 스큐 대칭 행렬(skew-symmetric matrix)
• 설명: 각속도 벡터를 입력받아 해당 벡터의 스큐 대칭 행렬을 생성합니다. 이 행렬은 벡터의 외적을 행렬 곱셈으로 표현할 수 있게 해 줍니다.

함수 taylor_A(x, nth=10), taylor_B(x, nth=10), taylor_C(x, nth=10)

• 입력: 스칼라 또는 벡터 x, 급수의 항수 nth
• 출력: 테일러 급수로 근사된 각각의 함수 값
• 설명: 각 함수는 로드리게스 회전 공식(Rodrigues’ rotation formula)에서 사용되는 sin(x)/x, (1-cos(x))/x^2, (x-sin(x))/x^3 등을 근사합니다. 이러한 근사는 회전의 크기가 작을 때 더욱 정확하게 동작합니다.

함수 se3_to_SE3(w, v)

• 입력: 각속도 벡터 w, 선속도 벡터 v
• 출력: 4x4 변환 행렬 SE3
• 설명: 입력된 w와 v로부터 SE3 변환 행렬을 계산합니다. 이 행렬은 3D 공간에서의 리지드 바디 변환(rigid body transformation)을 표현하며, 회전과 이동을 모두 포함합니다.
  • 상위 3x3 부분은 회전을 나타내며, 로드리게스 공식과 테일러 급수 근사를 통해 계산됩니다.
  • 마지막 열의 상위 3개 요소는 이동을 나타내며, 회전을 고려한 테일러 급수 근사를 통해 계산됩니다.
  • 매트릭스(matrix)의 오른쪽 아래 요소는 항상 1입니다.

import torch

def skew_symmetric(w):
    # 입력된 각속도 벡터 w에 대한 스큐 대칭 행렬 반환
    w0, w1, w2 = w.unbind(dim=-1)
    O = torch.zeros_like(w0)
    wx = torch.stack([torch.stack([O, -w2, w1], dim=-1),
                      torch.stack([w2, O, -w0], dim=-1),
                      torch.stack([-w1, w0, O], dim=-1)], dim=-2)
    return wx

def taylor_A(x, nth=10):
    # sin(x)/x의 테일러 급수 근사, nth는 다항식의 최대 차수
    ans = torch.zeros_like(x)
    denom = 1.
    for i in range(nth+1):
        if i > 0: denom *= (2*i)*(2*i+1)
        ans += (-1)**i * x**(2*i) / denom
    return ans
    
def taylor_B(x, nth=10):
    # (1-cos(x))/x^2의 테일러 급수 근사, nth는 다항식의 최대 차수
    ans = torch.zeros_like(x)
    denom = 1.
    for i in range(nth+1):
        denom *= (2*i+1)*(2*i+2)
        ans += (-1)**i * x**(2*i) / denom
    return ans

def taylor_C(x, nth=10):
    # (x-sin(x))/x^3의 테일러 급수 근사, nth는 다항식의 최대 차수
    ans = torch.zeros_like(x)
    denom = 1.
    for i in range(nth+1):
        denom *= (2*i+2)*(2*i+3)
        ans += (-1)**i * x**(2*i) / denom
    return ans

def se3_to_SE3(w, v):
    # 각속도 w와 선속도 v를 입력으로 받아 4x4 변환 행렬 SE3 계산
    deltaT = torch.zeros((4,4)).cuda()
    wx = skew_symmetric(w)
    theta = w.norm(dim=-1)
    I = torch.eye(3, device=w.device, dtype=torch.float32)
    A = taylor_A(theta, nth=10)  # nth 설정에 따라 근사 정확도 결정
    B = taylor_B(theta, nth=10)
    C = taylor_C(theta, nth=10)
    deltaT[:3, :3] = I + A*wx + B*wx@wx  # 회전 행렬 계산
    V = I + B*wx + C*wx@wx  # 이동 벡터 계산을 위한 행렬 V 계산
    deltaT[:3, 3] = V@v  # 이동 벡터 설정
    deltaT[3, 3] = 1.  # 동차 좌표 설정
    return deltaT

deltaT에 대한 설명

deltaT는 SE(3) 그룹에 속하는 4x4 변환 행렬로, 회전과 이동을 결합한 rigid body transform을 나타냅니다. 구조는 다음과 같습니다:

\[\begin{bmatrix} R & t \\ 0 & 1 \end{bmatrix}\]

여기서 $R$은 3x3의 회전 행렬로, SO(3) 그룹에 속합니다. 이 행렬은 순수한 회전만을 나타내며, 직교성과 행렬식이 1이라는 특성을 가집니다. $t$는 3x1의 이동 벡터로, SE(3) 그룹에 속합니다. 이 벡터는 공간에서의 객체 위치 변환을 가능하게 하는 이동 요소를 포함합니다.

이 변환 행렬 deltaT는 각속도와 선속도를 이용하여 계산되며, 3D 공간에서 객체의 위치와 방향을 변환하는 데 사용됩니다. 각속도는 회전을, 선속도는 이동을 결정하는 데 중요한 역할을 합니다.

Lie Group: Rotation SO(3), Rigid-body SE(3)