[PyICP-SLAM] PoseGraphManager.py 코드 공부

#ref : https://github.com/gisbi-kim/PyICP-SLAM/tree/master

import numpy as np
np.set_printoptions(precision=4)

# import minisam
import gtsam
from utils.UtilsMisc import *
    
class PoseGraphManager:
    def __init__(self):

        self.prior_cov = gtsam.noiseModel.Diagonal.Sigmas(np.array([1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4]))
        self.const_cov = np.array([0.5, 0.5, 0.5, 0.1, 0.1, 0.1])
        self.odom_cov = gtsam.noiseModel.Diagonal.Sigmas(self.const_cov)
        self.loop_cov = gtsam.noiseModel.Diagonal.Sigmas(self.const_cov)

        self.graph_factors = gtsam.NonlinearFactorGraph()
        self.graph_initials = gtsam.Values()

        self.opt_param = gtsam.LevenbergMarquardtParams()
        self.opt = gtsam.LevenbergMarquardtOptimizer(self.graph_factors, self.graph_initials, self.opt_param)

        self.curr_se3 = None
        self.curr_node_idx = None
        self.prev_node_idx = None

        self.graph_optimized = None

    def addPriorFactor(self):
        self.curr_node_idx = 0
        self.prev_node_idx = 0

        self.curr_se3 = np.eye(4)

        self.graph_initials.insert(gtsam.symbol('x', self.curr_node_idx), gtsam.Pose3(self.curr_se3))
        self.graph_factors.add(gtsam.PriorFactorPose3(
                                                gtsam.symbol('x', self.curr_node_idx), 
                                                gtsam.Pose3(self.curr_se3), 
                                                self.prior_cov))

    def addOdometryFactor(self, odom_transform):

        self.graph_initials.insert(gtsam.symbol('x', self.curr_node_idx), gtsam.Pose3(self.curr_se3))
        self.graph_factors.add(gtsam.BetweenFactorPose3(
                                                gtsam.symbol('x', self.prev_node_idx), 
                                                gtsam.symbol('x', self.curr_node_idx), 
                                                gtsam.Pose3(odom_transform), 
                                                self.odom_cov))

    def addLoopFactor(self, loop_transform, loop_idx):

        self.graph_factors.add(gtsam.BetweenFactorPose3(
                                        gtsam.symbol('x', loop_idx), 
                                        gtsam.symbol('x', self.curr_node_idx), 
                                        gtsam.Pose3(loop_transform), 
                                        self.odom_cov))

    def optimizePoseGraph(self):

        self.opt = gtsam.LevenbergMarquardtOptimizer(self.graph_factors, self.graph_initials, self.opt_param)
        self.graph_optimized = self.opt.optimize()

        # status = self.opt.optimize(self.graph_factors, self.graph_initials, self.graph_optimized)
        # if status != minisam.NonlinearOptimizationStatus.SUCCESS:
            # print("optimization error: ", status)

        # correct current pose 
        pose_trans, pose_rot = getGraphNodePose(self.graph_optimized, self.curr_node_idx)
        self.curr_se3[:3, :3] = pose_rot
        self.curr_se3[:3, 3] = pose_trans
        
        

원본 Git 주소

GTSAM을 활용한 PoseGraph 관리

제공된 코드는 GTSAM 라이브러리를 사용하여 기본 포즈 그래프 관리 시스템을 구현한다. GTSAM은 SLAM 및 기타 로봇공학 애플리케이션에 자주 사용된다. 다음은 구현 및 요소에 대한 분석이다.

• 코드 개요 :
  클래스 생성자 부분에 다양한 팩터에 대한 노이즈 모델을 초기화한다.
  노드의 초기값으로 모든 포즈의 0을 넣는 addPriorFactor 함수, 연속적인 포즈 간의 constraint를 추가하는 addOdometryFactor 함수, 이전에 방문한 포즈에 연결하는 addLoopFactor 함수, slam back-end 부분인 그래프 최적화하는 optimizePoseGraph 함수가 있다.

1. init(self)

   이 생성자 함수는 PoseGraphManager 인스턴스를 초기화한다. 여기서 주요 변수들과 객체들이 설정되며, 포즈 그래프의 기본 구성을 준비한다.

   변수 설정: 다양한 노이즈 모델(prior_cov, odom_cov, loop_cov)이 초기화되며, 이는 센서 데이터나 동작 정보의 불확실성을 모델링하는 데 사용된다.

   그래프 구조: gtsam.NonlinearFactorGraph와 gtsam.Values 인스턴스를 생성하여, 각각 팩터(제약 조건)와 초기 추정치를 저장한다.

   최적화 파라미터: gtsam.LevenbergMarquardtParams를 사용하여 최적화 프로세스를 설정하고, gtsam.LevenbergMarquardtOptimizer를 통해 이를 초기화한다.

2. addPriorFactor(self)

   이 함수는 그래프에 첫 번째 노드를 추가하고, 이 노드에 대한 우선 순위 팩터(고정된 초기 위치)를 설정한다. 이는 그래프의 나머지 부분에 대한 정확한 위치 추정의 기준점을 제공한다.

   초기 포즈 설정: 초기 포즈를 단위 행렬(eye matrix)로 설정하여 위치와 방향을 나타낸다.
   우선 순위 팩터 추가: gtsam.PriorFactorPose3를 사용하여 첫 노드에 대한 우선 순위 팩터를 그래프에 추가한다.

3. addOdometryFactor(self, odom_transform)

   오도메트리 정보를 사용하여 연속된 두 노드 사이에 동작 관련 팩터를 추가한다.

   self.graph_initials.insert(gtsam.symbol('x', self.curr_node_idx), gtsam.Pose3(self.curr_se3))
   
   self.graph_factors.add(gtsam.BetweenFactorPose3(
                                                  gtsam.symbol('x',  self.prev_node_idx), 
                                                  gtsam.symbol('x', self.curr_node_idx), 
                                                  gtsam.Pose3(odom_transform), 
                                                  self.odom_cov))

   gtsam.Values().insert 함수를 통해 self.curr_se3 포즈를 curr_node_idx 노드에 넣는다.
   gtsam.NonlinearFactorGraph().add 함수를 통해 직전 노드와 현재 노드의 spatial constraint를 추가한다.
   오도메트리 변환 적용: 제공된 odom_transform 매트릭스를 사용하여 현재 노드와 이전 노드 사이의 관계를 정의하는 gtsam.BetweenFactorPose3 팩터를 추가한다.

4. addLoopFactor(self, loop_transform, loop_idx)

   루프 클로저 팩터를 추가하여 그래프의 특정 이전 노드(Loop Detection)와 현재 노드 간의 관계를 설정한다. 이는 장시간 동안 움직이면서 쌓인 에러를 최적화 한다.

   루프 클로저 추가 : loop_transform을 사용하여 지정된 loop_idx 노드와 현재 노드 사이에 gtsam.BetweenFactorPose3 팩터를 추가한다.

5. optimizePoseGraph(self)

   그래프에 추가된 모든 팩터를 기반으로 최적화를 수행하여 노드의 최적 위치를 계산한다. 이 과정은 그래프에서 정의된 모든 제약 조건을 만족하는 최적의 포즈를 찾아내기 위해 반복적으로 수행된다.

   최적화 실행 : gtsam.LevenbergMarquardtOptimizer를 사용하여 포즈 그래프를 최적화하고, 결과를 self.graph_optimized에 저장한다.

   포즈 업데이트: 최적화된 결과에서 현재 노드의 최신 포즈를 추출하여 내부 상태를 업데이트 한다.