LIO-SAM 실습해보기 (5)

들어가면서.

이제 LIO-SAM도 막바지다.
여태까지 센서로부터 이미지를 받아 전처리 과정을 거쳐 curvature와 surface를 찾아내는 과정을 거쳐왔다.
이제 찾아낸 것들을 어떻게 매칭시키고, 자기 위치를 보정하는 과정에 대해 알아보자.

학습 순서.

다음과 같은 순서로 최대한 스피디하게 진행해볼까 한다.

1. ROS2에 설치해보기
2. IMU/PointCloud 다뤄보기
3. 데이터셋을 이용하여 LIO-SAM 구현해보기
4. (New!) featureExtraction.cpp 둘러보기, Feature, IMU ablations
5. mapOptimziation 분석, 라즈베리파이에 실제 센서 연결해보기. <- 오늘

Day 5.

mapOptimization.cpp 파일의 구조를 먼저 살펴보자.

계층	목적	구현
로컬 최적화	현재 스캔의 6-DoF 자세 추정	edge/plane scan-to-map + Gauss-Newton
전역 최적화	키프레임 전체 궤적 보정	GTSAM iSAM2 pose graph

현재 LiDAR feature → local map과 scan-to-map 정합 → keyframe 결정 → factor graph에 LiDAR/GPS/loop factor 추가 → iSAM2 최적화 → global pose/map 갱신 의 순으로 넘어간다.

cloud_info 수신
      ↓
laserCloudInfoHandler()
      ↓
updateInitialGuess()
      │
      │  IMU / preintegration odometry를 이용해
      │  현재 pose initial guess 생성
      ↓
extractSurroundingKeyFrames()
      │
      │  기존 keyframe 중 주변 keyframe 추출
      ↓
downsampleCurrentScan()
      │
      │  현재 corner / surface feature downsampling
      ↓
scan2MapOptimization()
      │
      ├─ cornerOptimization()
      │     point-to-line residual
      │
      ├─ surfOptimization()
      │     point-to-plane residual
      │
      ├─ combineOptimizationCoeffs()
      │
      └─ LMOptimization()
      │     현재 LiDAR pose 최적화
      ↓
saveKeyFramesAndFactor()
      │
      ├─ addOdomFactor()
      ├─ addGPSFactor()
      ├─ addLoopFactor()
      │
      └─ iSAM2 update
      ↓
correctPoses()
      │
      │  loop closure 발생 시
      │  과거 keyframe pose까지 보정
      ↓
publishOdometry()
      ↓
publishFrames()

좌표의 표현

transformTobeMapped 이란 변수를 주목해야 한다. 클래스 선언 초반부에서 볼 수 있다.

class mapOptimization : public ParamServer
{
...
public:
    float transformTobeMapped[6];

배열에 들어가는 값들은 다음과 같다 코드내내 자주 등장한다.

transformTobeMapped[0] = roll
transformTobeMapped[1] = pitch
transformTobeMapped[2] = yaw
transformTobeMapped[3] = x
transformTobeMapped[4] = y
transformTobeMapped[5] = z

이제 이 값이 어떻게 변환되어 들어가는지 확인해보자.

콜백함수와 입력

mapOptimizer가 실행되는 시점이다.

void laserCloudInfoHandler(const lio_sam::msg::CloudInfo::SharedPtr msgIn)
{
    static double timeLastProcessing = -1;
    if (timeLaserInfoCur - timeLastProcessing >= mappingProcessInterval)
    {
        timeLastProcessing = timeLaserInfoCur;
        updateInitialGuess();
        extractSurroundingKeyFrames();
        downsampleCurrentScan();
        scan2MapOptimization();
        saveKeyFramesAndFactor();
        correctPoses();
        publishOdometry();
        publishFrames();

우선, mappingProcessInterval 보다 빠르게 들어오는 스캔은 건너뛴다. 현재 설정은 0.15초이며, 이를 뒤집어보면 대략 6.7Hz라고 한다.

입력 메세지인 lio_sam::msg::CloudInfo::SharedPtr msgIn 는 다음과 같은 내용들이 들어있다.

• deskew된 원본 cloud
• corner feature cloud
• surface feature cloud
• IMU 초기 자세
• IMU preintegration에서 계산한 초기 odometry guess

초기자세 예측 updateInitialGuess()

    void updateInitialGuess()
    {
        // save current transformation before any processing
        // 1. 현재 transformTobeMapped 저장, 백업이다. 
        incrementalOdometryAffineFront = trans2Affine3f(transformTobeMapped);

        static Eigen::Affine3f lastImuTransformation;  //static 이렇게 하면 함수 호출이 끝나도 데이터가 저장된다. 
        // initialization
        // 2. 최초 프레임 초기화
        if (cloudKeyPoses3D->points.empty())
        {
            transformTobeMapped[0] = cloudInfo.imu_roll_init;
            transformTobeMapped[1] = cloudInfo.imu_pitch_init;
            transformTobeMapped[2] = cloudInfo.imu_yaw_init;

            if (!useImuHeadingInitialization)
                transformTobeMapped[2] = 0;

            lastImuTransformation = pcl::getTransformation(0, 0, 0, cloudInfo.imu_roll_init, cloudInfo.imu_pitch_init, cloudInfo.imu_yaw_init); // save imu before return;
            return;
        }

        // use imu pre-integration estimation for pose guess
        // 3. IMU preintegration odometry가 있으면 이것을 최우선 사용
        static bool lastImuPreTransAvailable = false;
        static Eigen::Affine3f lastImuPreTransformation;

        if (cloudInfo.odom_available == true)
        {   // transBack : 현재 시점의 IMU 기반 pose를 Eigen::Affine3f 변환행렬로 만든 것
            Eigen::Affine3f transBack = pcl::getTransformation(
                cloudInfo.initial_guess_x, cloudInfo.initial_guess_y, cloudInfo.initial_guess_z,
                cloudInfo.initial_guess_roll, cloudInfo.initial_guess_pitch, cloudInfo.initial_guess_yaw);
            //그래서 이전 imu 변환데이터를 쓸수 없으면, 걍 transBack을 쓴다. 
            if (lastImuPreTransAvailable == false)
            {
                lastImuPreTransformation = transBack;
                lastImuPreTransAvailable = true;
            } else { //요기가 핵심. 이전 imu 데이터가 있다면, 직전 mapping의 자세값에 IMU Odometry의 상대 이동량을 곱해서 업데이트한다. 
                Eigen::Affine3f transIncre = lastImuPreTransformation.inverse() * transBack;
                Eigen::Affine3f transTobe = trans2Affine3f(transformTobeMapped);
                Eigen::Affine3f transFinal = transTobe * transIncre;
                pcl::getTranslationAndEulerAngles(transFinal, transformTobeMapped[3], transformTobeMapped[4], transformTobeMapped[5], 
                                                              transformTobeMapped[0], transformTobeMapped[1], transformTobeMapped[2]);

                lastImuPreTransformation = transBack;

                lastImuTransformation = pcl::getTransformation(0, 0, 0, cloudInfo.imu_roll_init, cloudInfo.imu_pitch_init, cloudInfo.imu_yaw_init); // save imu before return;
                return;
            }
        }

        // use imu incremental estimation for pose guess (only rotation)
        // 4. odometry가 없으면 IMU 회전 증분만 사용
        if (cloudInfo.imu_available == true)
        {
            Eigen::Affine3f transBack = pcl::getTransformation(0, 0, 0, cloudInfo.imu_roll_init, cloudInfo.imu_pitch_init, cloudInfo.imu_yaw_init);
            Eigen::Affine3f transIncre = lastImuTransformation.inverse() * transBack;

            Eigen::Affine3f transTobe = trans2Affine3f(transformTobeMapped);
            Eigen::Affine3f transFinal = transTobe * transIncre;
            pcl::getTranslationAndEulerAngles(transFinal, transformTobeMapped[3], transformTobeMapped[4], transformTobeMapped[5], 
                                                          transformTobeMapped[0], transformTobeMapped[1], transformTobeMapped[2]);

            lastImuTransformation = pcl::getTransformation(0, 0, 0, cloudInfo.imu_roll_init, cloudInfo.imu_pitch_init, cloudInfo.imu_yaw_init); // save imu before return;
            return;
        }
    }

첫 키프레임

updateInitialGuess() 함수를 통해 초기 자세를 예측한다. 

    transformTobeMapped[0] = cloudInfo.imu_roll_init;
    transformTobeMapped[1] = cloudInfo.imu_pitch_init;
    transformTobeMapped[2] = cloudInfo.imu_yaw_init;

    if (!useImuHeadingInitialization)
        transformTobeMapped[2] = 0;

    lastImuTransformation = pcl::getTransformation(0, 0, 0, cloudInfo.imu_roll_init, cloudInfo.imu_pitch_init, cloudInfo.imu_yaw_init); // save imu before return;
    return;

IMU를 통해 받은 roll, pitch, yaw를 초기화하는 형태로 저장한다. 단, useImuHeadingInitialization == false이면 yaw는 0이 된다. 위치값은 0 이다.

이후 스캔 : IMU preintegration odometry

    static Eigen::Affine3f lastImuPreTransformation;
    if (cloudInfo.odom_available == true)
    {
        Eigen::Affine3f transBack = pcl::getTransformation(
            cloudInfo.initial_guess_x, cloudInfo.initial_guess_y, cloudInfo.initial_guess_z,
            cloudInfo.initial_guess_roll, cloudInfo.initial_guess_pitch, cloudInfo.initial_guess_yaw);  //변환값을 불러온다. 
        if (lastImuPreTransAvailable == false)
        {
            lastImuPreTransformation = transBack;
            lastImuPreTransAvailable = true;
        } else {
            Eigen::Affine3f transIncre = lastImuPreTransformation.inverse() * transBack;
            Eigen::Affine3f transTobe = trans2Affine3f(transformTobeMapped);
            Eigen::Affine3f transFinal = transTobe * transIncre;
            pcl::getTranslationAndEulerAngles(transFinal, transformTobeMapped[3], transformTobeMapped[4], transformTobeMapped[5], 
                                                            transformTobeMapped[0], transformTobeMapped[1], transformTobeMapped[2]);

            lastImuPreTransformation = transBack;

            lastImuTransformation = pcl::getTransformation(0, 0, 0, cloudInfo.imu_roll_init, cloudInfo.imu_pitch_init, cloudInfo.imu_yaw_init); // save imu before return;
            return;
        }
    }

transIncre = lastImuPreTransformation.inverse() * transBack;
transTobe = trans2Affine3f(transformTobeMapped);
transFinal = transTobe * transIncre;

직전 mapping의 자세값에 IMU Odometry의 상대 이동량을 곱해서 업데이트한다.

주변 로컬 맵 생성 extractSurroundingKeyFrames()

void extractSurroundingKeyFrames()
{
    if (cloudKeyPoses3D->points.empty() == true)
        return; 
    
    // if (loopClosureEnableFlag == true)
    // {
    //     extractForLoopClosure();    
    // } else {
    //     extractNearby();
    // }

    extractNearby();
}

코드가 주석처리되어있는데, 그냥 extractNearby()만 실행하도록 되어있다.
이래서, params.yaml에서 루프클로저를 지웠는데도 잘되었나보다….. 다음번에 한번 해봐야겠다.

extractNearby()함수를 보자.
extractNearby()는 mapOptimization.cpp에서 현재 LiDAR scan과 scan-to-map matching을 수행하기 위해 주변의 keyframe들을 골라 local map을 만드는 전처리 단계다. 핵심변수는 cloudKeyPoses3D다.

cloudKeyPoses3D는 SLAM이 지금까지 저장한 keyframe들의 위치(position) 를 PointCloud 형태로 저장해놓은 것이다.

void extractNearby()
{
    //주변 keyframe pose를 담을 공간을 마련한다. DS 는 다운샘플링을 뜻한다. 
    pcl::PointCloud<PointType>::Ptr surroundingKeyPoses(new pcl::PointCloud<PointType>());
    pcl::PointCloud<PointType>::Ptr surroundingKeyPosesDS(new pcl::PointCloud<PointType>());
    std::vector<int> pointSearchInd;
    std::vector<float> pointSearchSqDis;

    // extract all the nearby key poses and downsample them
    kdtreeSurroundingKeyPoses->setInputCloud(cloudKeyPoses3D); // create kd-tree
    kdtreeSurroundingKeyPoses->radiusSearch(cloudKeyPoses3D->back(), (double)surroundingKeyframeSearchRadius, pointSearchInd, pointSearchSqDis); //radius 서치

    // 2. 검색된 keyframe pose 수집
    for (int i = 0; i < (int)pointSearchInd.size(); ++i)
    {
        int id = pointSearchInd[i];
        surroundingKeyPoses->push_back(cloudKeyPoses3D->points[id]);
    }

    // 3. keyframe pose들을 voxel downsampling
    downSizeFilterSurroundingKeyPoses.setInputCloud(surroundingKeyPoses);
    downSizeFilterSurroundingKeyPoses.filter(*surroundingKeyPosesDS);

    // 4. downsampling 후 keyframe index 복구
    for(auto& pt : surroundingKeyPosesDS->points)
    {
        kdtreeSurroundingKeyPoses->nearestKSearch(pt, 1, pointSearchInd, pointSearchSqDis);
        pt.intensity = cloudKeyPoses3D->points[pointSearchInd[0]].intensity;
    }

    // also extract some latest key frames in case the robot rotates in one position
    // 5. 최근 10초 keyframe 추가
    int numPoses = cloudKeyPoses3D->size();
    for (int i = numPoses-1; i >= 0; --i)
    {
        if (timeLaserInfoCur - cloudKeyPoses6D->points[i].time < 10.0)
            surroundingKeyPosesDS->push_back(cloudKeyPoses3D->points[i]);
        else
            break;
    }
    // 6. 실제 point cloud local map 생성
    extractCloud(surroundingKeyPosesDS);
}

1) 마지막 키프레임 주변을 radius search 2) 키프레임 위치를 voxel downsampling 3) downsample된 각 위치를 실제 키프레치임 인덱스로 복원 4) 최근 10초 키프레임도 추가 5) 해당 키프레임들의 feature cloud를 map 좌표로 변환 6) 합친 cloud를 다시 downsampling

최근 10초 키프레임을 추가하는 이유는 로봇이 제자리에서 회전하는 경우다. 위치 기반 voxel filtering만 쓰면 같은 장소의 서로 다른 방향 키프레임들이 하나로 사라질 수 있다

이후 extractCloud() 함수를 통해 local map을 생성한다.

downsampleCurrentScan();

다운샘플링 자체는 심플하다.

void downsampleCurrentScan()
{
    // Downsample cloud from current scan
    laserCloudCornerLastDS->clear();
    downSizeFilterCorner.setInputCloud(laserCloudCornerLast);
    downSizeFilterCorner.filter(*laserCloudCornerLastDS);
    laserCloudCornerLastDSNum = laserCloudCornerLastDS->size();

    laserCloudSurfLastDS->clear();
    downSizeFilterSurf.setInputCloud(laserCloudSurfLast);
    downSizeFilterSurf.filter(*laserCloudSurfLastDS);
    laserCloudSurfLastDSNum = laserCloudSurfLastDS->size();
}

필터라고 해서 거창한건 아니고, voxel 공간에 포인트를 넣고, 그중 대표 포인트만 뽑아낸다.

scan2MapOptimization();

이제, 현재 scan의 pose를 local map에 맞춰 정밀하게 보정하는 핵심 블록을 훑어보자.

void scan2MapOptimization()
{
    //다운샘플링된 코너의 숫자가 기준값보다 크다면, 다운샘플링된 평면이 기준값보다 크다면 수행된다. 모두 AND 조건이다. 즉 뽑아온 feature의 개수가 충분한지 검사한다. 
    //즉 충분치 않다면 최적화과정이 생략된다는거다...
    if (laserCloudCornerLastDSNum > edgeFeatureMinValidNum && laserCloudSurfLastDSNum > surfFeatureMinValidNum)
    {
        //앞에서 주변 keyframe들로 이미 만들어 둔 local map을 KD-tree에 등록해서, 현재 scan point의 nearest neighbor를 빠르게 찾을 수 있게 한다
        //만들어진 local map point cloud를 KD-tree의 검색 대상으로 등록하는 단계
        //laserCloudCornerFromMapDS를 KDtree에 넣는다. 
        kdtreeCornerFromMap->setInputCloud(laserCloudCornerFromMapDS);
        kdtreeSurfFromMap->setInputCloud(laserCloudSurfFromMapDS);

        for (int iterCount = 0; iterCount < 30; iterCount++)
        {
            laserCloudOri->clear();
            coeffSel->clear();


            // 현재 scan과 local맵사이의 오차값을 만든다. 
            //corner correspondence 생성
            cornerOptimization();
            //surface correspondence 생성
            surfOptimization();
            //constraint 합치기
            combineOptimizationCoeffs();

            if (LMOptimization(iterCount) == true)  //최적화 연산, 수렴하면 True. 
                break;              
        }

        //transformTobeMapped 갱신
        transformUpdate();
    } else {
        RCLCPP_WARN(get_logger(), "Not enough features! Only %d edge and %d planar features available.", laserCloudCornerLastDSNum, laserCloudSurfLastDSNum);
    }

if (laserCloudCornerLastDSNum > edgeFeatureMinValidNum && laserCloudSurfLastDSNum > surfFeatureMinValidNum)

map optimization을 뭐라 한국말로 표현해야 딱 알맞을지 모르겠다만. 여튼 들어가는 최적조건은 params.yaml에 선언되어있다.

(in params.yaml)
# LOAM feature threshold
edgeThreshold: 1.0
surfThreshold: 0.1
edgeFeatureMinValidNum: 10
surfFeatureMinValidNum: 100

그다음 주변 keyframe들로 local map을 만들고, 그 local map의 corner/surface feature point들을 KD-tree에 등록한다. 이후 현재 LiDAR scan의 feature point들에 대해 local map에서 가까운 feature들을 검색하고, point-to-line 및 point-to-plane 오차를 최소화하여 현재 scan의 pose를 추정한다.

그 feature들간의 오차를 계산하는 계산식을 만드는 과정이 그 다음 나오는 함수들이다.

• cornerOptimization() 현재 scan의 corner point를 local map의 corner KD-tree에서 검색하고,
  가까운 점들로 line을 만든 뒤 point-to-line 오차를 계산

• surfOptimization() 현재 scan의 surface point를 local map의 surface KD-tree에서 검색하고,
  가까운 점들로 plane을 만든 뒤 point-to-plane 오차를 계산

• LMOptimization(iterCount) 실제로 Pose를 업데이트하는 과정

cornerOptimization()

이 함수는 corner feature 한 점과 local map의 edge line 사이의 point-to-line 거리와 그 거리의 방향 미분값을 만드는 함수다.

최근접 5점 → PCA → line 판정 → point-to-line 거리 → coefficient 생성 순서로 이해하면 좋다.

void cornerOptimization()
    {
        updatePointAssociateToMap();

        #pragma omp parallel for num_threads(numberOfCores)
        for (int i = 0; i < laserCloudCornerLastDSNum; i++)
        {
            PointType pointOri, pointSel, coeff;
            std::vector<int> pointSearchInd;
            std::vector<float> pointSearchSqDis;

            //현재 scan point를 map 좌표계로 보낸다
            pointOri = laserCloudCornerLastDS->points[i];
            pointAssociateToMap(&pointOri, &pointSel);

            //local map에서 가장 가까운 corner 5개를 찾는다
            kdtreeCornerFromMap->nearestKSearch(pointSel, 5, pointSearchInd, pointSearchSqDis);

            cv::Mat matA1(3, 3, CV_32F, cv::Scalar::all(0));  //covariance matrix
            cv::Mat matD1(1, 3, CV_32F, cv::Scalar::all(0));  //eigenvalue
            cv::Mat matV1(3, 3, CV_32F, cv::Scalar::all(0));  //eigenvector
                    
            if (pointSearchSqDis[4] < 1.0) {
                
                //5개 점의 중심점 centroid 계산
                float cx = 0, cy = 0, cz = 0;
                for (int j = 0; j < 5; j++) {
                    cx += laserCloudCornerFromMapDS->points[pointSearchInd[j]].x;
                    cy += laserCloudCornerFromMapDS->points[pointSearchInd[j]].y;
                    cz += laserCloudCornerFromMapDS->points[pointSearchInd[j]].z;
                }
                cx /= 5; cy /= 5;  cz /= 5;  //5개의 평균이다. 
                
                //covariance matrix 를 계산한다. 
                float a11 = 0, a12 = 0, a13 = 0, a22 = 0, a23 = 0, a33 = 0;
                for (int j = 0; j < 5; j++) {
                    float ax = laserCloudCornerFromMapDS->points[pointSearchInd[j]].x - cx;
                    float ay = laserCloudCornerFromMapDS->points[pointSearchInd[j]].y - cy;
                    float az = laserCloudCornerFromMapDS->points[pointSearchInd[j]].z - cz;

                    a11 += ax * ax; a12 += ax * ay; a13 += ax * az;
                    a22 += ay * ay; a23 += ay * az;
                    a33 += az * az;
                }
                a11 /= 5; a12 /= 5; a13 /= 5; a22 /= 5; a23 /= 5; a33 /= 5;


                //matA1 = 최근접 5개 map corner point의 3D 분포를 나타내는 행렬
                matA1.at<float>(0, 0) = a11; matA1.at<float>(0, 1) = a12; matA1.at<float>(0, 2) = a13;
                matA1.at<float>(1, 0) = a12; matA1.at<float>(1, 1) = a22; matA1.at<float>(1, 2) = a23;
                matA1.at<float>(2, 0) = a13; matA1.at<float>(2, 1) = a23; matA1.at<float>(2, 2) = a33;

                //eigen decomposition
                cv::eigen(matA1, matD1, matV1);


                //이 5개 점이 진짜 line인지 검사. lambda_1 > 3*lambda_2
                //가장 큰 주성분의 분산이 두 번째 주성분보다 최소 3배 이상 크다.
                // ->즉 5점이 한 방향으로 충분히 길게 늘어서 있다는 뜻.
                if (matD1.at<float>(0, 0) > 3 * matD1.at<float>(0, 1)) {
                    
                    //line 위의 두 점 p1, p2를 만든다
                    float x0 = pointSel.x;
                    float y0 = pointSel.y;
                    float z0 = pointSel.z;
                    float x1 = cx + 0.1 * matV1.at<float>(0, 0);
                    float y1 = cy + 0.1 * matV1.at<float>(0, 1);
                    float z1 = cz + 0.1 * matV1.at<float>(0, 2);
                    float x2 = cx - 0.1 * matV1.at<float>(0, 0);
                    float y2 = cy - 0.1 * matV1.at<float>(0, 1);
                    float z2 = cz - 0.1 * matV1.at<float>(0, 2);
                    //간단한 vector cross product를 하드코딩으로 구현했다. 
                    //이걸 구하면 두 벡터가 만드는 평행사변형의 면적이 나온다. p0-p1과 p0-p2로 만드는 평행사변형이다. 
                    float a012 = sqrt(((x0 - x1)*(y0 - y2) - (x0 - x2)*(y0 - y1)) * ((x0 - x1)*(y0 - y2) - (x0 - x2)*(y0 - y1)) 
                                    + ((x0 - x1)*(z0 - z2) - (x0 - x2)*(z0 - z1)) * ((x0 - x1)*(z0 - z2) - (x0 - x2)*(z0 - z1)) 
                                    + ((y0 - y1)*(z0 - z2) - (y0 - y2)*(z0 - z1)) * ((y0 - y1)*(z0 - z2) - (y0 - y2)*(z0 - z1)));
                    
                    //l12: line을 정의하는 두 점 사이 거리
                    float l12 = sqrt((x1 - x2)*(x1 - x2) + (y1 - y2)*(y1 - y2) + (z1 - z2)*(z1 - z2));

                    //la, lb, lc : point-to-line distance d를 현재 point 위치에 대해 미분한 값,즉 gradient
                    float la = ((y1 - y2)*((x0 - x1)*(y0 - y2) - (x0 - x2)*(y0 - y1)) 
                              + (z1 - z2)*((x0 - x1)*(z0 - z2) - (x0 - x2)*(z0 - z1))) / a012 / l12;

                    float lb = -((x1 - x2)*((x0 - x1)*(y0 - y2) - (x0 - x2)*(y0 - y1)) 
                               - (z1 - z2)*((y0 - y1)*(z0 - z2) - (y0 - y2)*(z0 - z1))) / a012 / l12;

                    float lc = -((x1 - x2)*((x0 - x1)*(z0 - z2) - (x0 - x2)*(z0 - z1)) 
                               + (y1 - y2)*((y0 - y1)*(z0 - z2) - (y0 - y2)*(z0 - z1))) / a012 / l12;
                    
                    //평행사변형의 면적과 한 변의 길이를 알았다. 그러면 나웠을때, 그 점과 벡터의 거리를 알수 있다. 
                    float ld2 = a012 / l12;

                    //s: residual weight, distance가 작으면 웨이트가 커진다. 
                    float s = 1 - 0.9 * fabs(ld2);

                    //최종 coefficient
                    coeff.x = s * la;
                    coeff.y = s * lb;
                    coeff.z = s * lc;
                    coeff.intensity = s * ld2;

                    // 유효한 correspondenc인지 넣는 작업
                    if (s > 0.1) {
                        laserCloudOriCornerVec[i] = pointOri;
                        coeffSelCornerVec[i] = coeff;
                        laserCloudOriCornerFlag[i] = true;  //이게 나중에 합칠때 쓰인다. 
                    }
                }
            }
        }

surfOptimization()

평면(plane)을 추정하고 현재 surface point와 그 평면 사이의 거리를 residual로 만드는 함수다.
최근접 surface 5점을 찾고, 그 5점으로 평면 ax+by+cz+d=0을 적합한 뒤, point-to-plane 오차와 계수를 만든다.

여기도 코드가 긴 이유는 하드코딩이 많아서 그렇다. 두려워하지말고 천천히 나아가보자.

void surfOptimization()
{
    updatePointAssociateToMap();

    #pragma omp parallel for num_threads(numberOfCores)
    for (int i = 0; i < laserCloudSurfLastDSNum; i++)
    {
        PointType pointOri, pointSel, coeff;
        std::vector<int> pointSearchInd;
        std::vector<float> pointSearchSqDis;

        //현재 surface point를 map 좌표계로 변환
        pointOri = laserCloudSurfLastDS->points[i];
        pointAssociateToMap(&pointOri, &pointSel); 

        //Local map에서 surface point 5개 검색
        kdtreeSurfFromMap->nearestKSearch(pointSel, 5, pointSearchInd, pointSearchSqDis);

        //Ax = B . 선형식을 만들 자리다. 
        Eigen::Matrix<float, 5, 3> matA0; //최근접 5점의 좌표
        Eigen::Matrix<float, 5, 1> matB0;  [-1 -1 -1 -1 ]
        Eigen::Vector3f matX0;  // 구하고자 하는 평면의 계수. ax + by + cz  = -1 
        //ax1​+by1​+cz1​=−1
        //ax2​+by2​+cz2​=−1
        //ax3​+by3​+cz3​=−1
        // ......

        //일단 빈칸채우기
        matA0.setZero();
        matB0.fill(-1);
        matX0.setZero();

        //최근접점 5개의 좌표를 넣는 과정이다. 
        if (pointSearchSqDis[4] < 1.0) {
            for (int j = 0; j < 5; j++) {
                matA0(j, 0) = laserCloudSurfFromMapDS->points[pointSearchInd[j]].x;
                matA0(j, 1) = laserCloudSurfFromMapDS->points[pointSearchInd[j]].y;
                matA0(j, 2) = laserCloudSurfFromMapDS->points[pointSearchInd[j]].z;
            }

            // Ax=B 니까, x 를 구하려면 A^(-1) B 겠지. 
            //그런데, 5개 포인트니까 식은 5개, 미지수는 3개이므로 overdetermined다. 
            //그래서 엄밀해 대신 least-squares를 통해 근사치(x*)를 구한다.
            matX0 = matA0.colPivHouseholderQr().solve(matB0);

            //결과값의 저장
            float pa = matX0(0, 0);
            float pb = matX0(1, 0);
            float pc = matX0(2, 0);
            float pd = 1;

            //법선벡터가 unit length가 아니므로, normalize한다. 
            float ps = sqrt(pa * pa + pb * pb + pc * pc);
            pa /= ps; pb /= ps; pc /= ps; pd /= ps;

            //이러면 pa​x+pb​y+pc​z+pd​ 값이 signed point-to-plane distance가 되기 때문
            //평면방정식과 점 사이의 거리를 어떻게 구하는지 한번 찾아보라. 

            //추정한 plane이 정말 유효한지 검사
            //5점 중 하나라도 plane에서 20 cm 이상 떨어져 있으면 false
            bool planeValid = true;
            for (int j = 0; j < 5; j++) {
                if (fabs(pa * laserCloudSurfFromMapDS->points[pointSearchInd[j]].x +
                            pb * laserCloudSurfFromMapDS->points[pointSearchInd[j]].y +
                            pc * laserCloudSurfFromMapDS->points[pointSearchInd[j]].z + pd) > 0.2) {
                    planeValid = false;
                    break;
                }
            }
            //만약 현재 plane이 유효하다면, 
            if (planeValid) {
                //현재 point와 plane 사이 distance pd2. 즉 point-to-plane signed distance residual
                float pd2 = pa * pointSel.x + pb * pointSel.y + pc * pointSel.z + pd;

                //그리고 평면의 법선(normal)이 곧 gradient가 된다. pa, pb, pc

                // 가중치 연산이다. 거리가 멀면 가중치가 낮아진다. 
                float s = 1 - 0.9 * fabs(pd2) / sqrt(sqrt(pointOri.x * pointOri.x
                        + pointOri.y * pointOri.y + pointOri.z * pointOri.z));

                //최종 coefficient
                coeff.x = s * pa;
                coeff.y = s * pb;
                coeff.z = s * pc;
                coeff.intensity = s * pd2;
                
                //단 weight가 너무 작으면 해당 correspondence를 사용하지 않는다.
                // 유효한 correspondenc인지 넣는 작업
                if (s > 0.1) {
                    laserCloudOriSurfVec[i] = pointOri;
                    coeffSelSurfVec[i] = coeff;
                    laserCloudOriSurfFlag[i] = true;
                }
            }
        }
    }
}

단계	cornerOptimization()	surfOptimization()
현재 feature	edge point	surface point
KD-tree	corner map	surface map
최근접 점	5개	5개
geometry 추정	PCA로 line	least squares로 plane
geometry 검증	(\lambda_1>3\lambda_2)	5점의 plane 거리 < 0.2 m
residual	point-to-line	point-to-plane
gradient	la, lb, lc	plane normal pa,pb,pc
weight	1-0.9 | d |	range를 고려한 weight
결과	coeff	coeff

combineOptimizationCoeffs()

cornerOptimization()과 surfOptimization()에서 각각 계산한 유효한 constraint들을 하나로 합치는 함수다. 즉 코너의 residual과 gradient + 평면의 residual과 gradient를 합쳐서 최적화식을 위한 하나의 입력으로 만드는 과정이다.

void combineOptimizationCoeffs()
{
    // combine corner coeffs
    for (int i = 0; i < laserCloudCornerLastDSNum; ++i){
        //이 flag는 해당 corner point가 실제로 좋은 correspondence를 만들었는지 확인한다. 
        if (laserCloudOriCornerFlag[i] == true){
            laserCloudOri->push_back(laserCloudOriCornerVec[i]);
            coeffSel->push_back(coeffSelCornerVec[i]);
        }
    }
    // combine surf coeffs
    for (int i = 0; i < laserCloudSurfLastDSNum; ++i){
        if (laserCloudOriSurfFlag[i] == true){
            laserCloudOri->push_back(laserCloudOriSurfVec[i]);
            coeffSel->push_back(coeffSelSurfVec[i]);
        }
    }
    // reset flag for next iteration
    std::fill(laserCloudOriCornerFlag.begin(), laserCloudOriCornerFlag.end(), false);
    std::fill(laserCloudOriSurfFlag.begin(), laserCloudOriSurfFlag.end(), false);
}

cornerOptimization(),surfOptimization() 마지막부분에 보면, 이게 유효한 coef 인지 아닌지 저장하는 부분이 있다. 그 값을 읽어들이면서 그 coef 를 laserCloudOri와 coeffSel에 밀어 넣는 작업을 한다.

예를 들어 유효한 corner가 3개, surface가 4개라면

laserCloudOri
────────────────
[0] corner point C1
[1] corner point C2
[2] corner point C3
[3] surface point S1
[4] surface point S2
[5] surface point S3
[6] surface point S4


coeffSel
────────────────
[0] corner coeff C1
[1] corner coeff C2
[2] corner coeff C3
[3] surface coeff S1
[4] surface coeff S2
[5] surface coeff S3
[6] surface coeff S4

이런 형식으로 들어가게 되는 것이다. 그러면 이제 LMOptimization() 입장에서는 이제 corner인지 surface인지 크게 신경 쓸 필요가 없다.

LMOptimization(iterCount)

edge와 surface residual을 이용해 자세 increment를 구한다. LMOptimization이지만 실제 구현에는 Levenberg-Marquardt damping term인 (\lambda I)가 없다. 수학적으로는 Gauss-Newton normal equation에 더 가깝다.

cornerOptimization()
surfOptimization()
        ↓
laserCloudOri
coeffSel
        ↓
LMOptimization()
        │
        ├─ Jacobian A 생성
        ├─ residual B 생성
        ├─ AᵀA Δx = AᵀB 풀이
        ├─ degeneracy 검사
        ├─ Δx를 pose에 더함
        └─ 수렴 여부 판단

이걸 하드코딩으로 구현해놓은 상황이다. 상세한 분석은 건너뛸까 한다… 양이 너무 길다. 이건 코드 리팩토링을 해보는 것도 좋은 공부가 될 듯 싶다.

IMU 보정과 motion constraint : transformUpdate()

scan-to-map 결과의 roll/pitch를 IMU와 약하게 융합한다. 이 과정의 핵심은 세가지다.

1. IMU roll/pitch와 LiDAR 최적화 결과를 보간
2. roll/pitch를 rotationTolerance로 제한
3. z를 zTolerance로 제한 LMOptimization()이 만든 결과를 그대로 쓰는 게 아니라, 한 번 더 안정화하는 단계다. 

    void transformUpdate()
    {
        //IMU가 있을 때만 roll/pitch 보정, yaw는 섞지 않는다
        if (cloudInfo.imu_available == true)
        {
            //pitch가 너무 크면 fusion을 안한다. 
            if (std::abs(cloudInfo.imu_pitch_init) < 1.4)
            {
                //imuRPYWeight: 0.01 였다. params.yaml 참조
                double imuWeight = imuRPYWeight;
                tf2::Quaternion imuQuaternion;
                tf2::Quaternion transformQuaternion;
                double rollMid, pitchMid, yawMid;
                조
                //SLERP는 quaternion spherical linear interpolation
                // roll = (1−w)*roll_lidar​+w*roll_imu​
                // slerp roll
                transformQuaternion.setRPY(transformTobeMapped[0], 0, 0);
                imuQuaternion.setRPY(cloudInfo.imu_roll_init, 0, 0);
                tf2::Matrix3x3(transformQuaternion.slerp(imuQuaternion, imuWeight)).getRPY(rollMid, pitchMid, yawMid);
                transformTobeMapped[0] = rollMid;
                
                //마찬가지로 내삽한다
                // slerp pitch
                transformQuaternion.setRPY(0, transformTobeMapped[1], 0);
                imuQuaternion.setRPY(0, cloudInfo.imu_pitch_init, 0);
                tf2::Matrix3x3(transformQuaternion.slerp(imuQuaternion, imuWeight)).getRPY(rollMid, pitchMid, yawMid);
                transformTobeMapped[1] = pitchMid;
            }
        }
        //constraintTransformation()은 값이 정해진 범위를 넘지 못하게 clamp하는 함수
        transformTobeMapped[0] = constraintTransformation(transformTobeMapped[0], rotation_tollerance);
        transformTobeMapped[1] = constraintTransformation(transformTobeMapped[1], rotation_tollerance);
        transformTobeMapped[5] = constraintTransformation(transformTobeMapped[5], z_tollerance);
        //updateInitialGuess() 에서 incrementalOdometryAffineFront를 저장했었다. 
        // 이제, incrementalOdometryAffineBack 을 저장함으로써, 두 pose의 차이를 이용해
        // incremental odometry를 만들 수 있다. 
        incrementalOdometryAffineBack = trans2Affine3f(transformTobeMapped);
    }

saveKeyFramesAndFactor();

mapOptimization.cpp에서 로컬 scan-to-map 결과를 factor graph의 새로운 keyframe state로 등록하고, iSAM2로 최적화한 뒤 그 결과를 keyframe pose와 point cloud로 저장하는 함수다.

실행순서는 saveFrame() → addOdomFactor() → addGPSFactor() → addLoopFactor() → isam->update() → 최신 pose 저장의 과정으로 진행된다.

scan2MapOptimization()
        ↓
transformTobeMapped
현재 scan의 LiDAR pose
        ↓
saveFrame()
        │
        ├─ keyframe 아님 → return
        │
        └─ keyframe 맞음
                ↓
        addOdomFactor()
        addGPSFactor()
        addLoopFactor()
                ↓
             iSAM2
                ↓
        최적화된 pose 획득
                ↓
     cloudKeyPoses3D / 6D 저장
                ↓
 cornerCloudKeyFrames
 surfCloudKeyFrames 저장

saveFrame()

LiDAR scan을 keyframe으로 저장하지 않는다.

if (saveFrame() == false)
    return; 

bool saveFrame()
{
    //이건 초기값인듯하다. 그럼 키프레임
    if (cloudKeyPoses3D->points.empty())
        return true;

    // LIVOX센서일때, 센서시간이 일정 이상 벌어지면 키프레임이다. 
    if (sensor == SensorType::LIVOX)
    {
        if (timeLaserInfoCur - cloudKeyPoses6D->back().time > 1.0)
            return true;
    }


    Eigen::Affine3f transStart = pclPointToAffine3f(cloudKeyPoses6D->back());
    Eigen::Affine3f transFinal = pcl::getTransformation(transformTobeMapped[3], transformTobeMapped[4], transformTobeMapped[5], 
                                                        transformTobeMapped[0], transformTobeMapped[1], transformTobeMapped[2]);
    Eigen::Affine3f transBetween = transStart.inverse() * transFinal;
    float x, y, z, roll, pitch, yaw;
    pcl::getTranslationAndEulerAngles(transBetween, x, y, z, roll, pitch, yaw);

    //어느정도 변화량이 크지 않다면 키프레임으로 만들지 않는다. 하나라도 넘으면 키프레임이 된다.  
    //이동 1m, 각도 변화량은 0.2rad, 11.5도다. params.yaml 참조
    if (abs(roll)  < surroundingkeyframeAddingAngleThreshold &&
        abs(pitch) < surroundingkeyframeAddingAngleThreshold &&
        abs(yaw)   < surroundingkeyframeAddingAngleThreshold &&
        sqrt(x*x + y*y + z*z) < surroundingkeyframeAddingDistThreshold)
        return false;

    return true;
}

이제 키프레임이라는 판단이 섰으면 추가하는 과정을 진행한다.

addOdomFactor()

scan-to-map 결과를 factor graph에 연결하는 핵심 부분. 최초 keyframe이면 PriorFactor로 들어가고, 두 번째 keyframe부터는 BetweenFactor가 된다.

    void addOdomFactor()
    {
        if (cloudKeyPoses3D->points.empty())
        {
            //첫프레임을 저장하는 과정
            //중간에 variance가 큰 값들(10^8, 10e8)은 prior로 쓰지 않겠다는 뜻과 같다.
            // 작은 값(10e-2)일수록 신뢰하는 값이란 뜻이 된다. 
            noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Variances((Vector(6) << 1e-2, 1e-2, M_PI*M_PI, 1e8, 1e8, 1e8).finished()); // rad*rad, meter*meter
            gtSAMgraph.add(PriorFactor<Pose3>(0, trans2gtsamPose(transformTobeMapped), priorNoise));
            initialEstimate.insert(0, trans2gtsamPose(transformTobeMapped));
        }else{
            noiseModel::Diagonal::shared_ptr odometryNoise = noiseModel::Diagonal::Variances((Vector(6) << 1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4).finished());
            
            //이전 keyframe의 최적화된 pose
            gtsam::Pose3 poseFrom = pclPointTogtsamPose3(cloudKeyPoses6D->points.back());
            //현재 scan-to-map으로 구한 pose
            gtsam::Pose3 poseTo   = trans2gtsamPose(transformTobeMapped);

            //gtSAM에 넣는과정이다. 
            //betweenFactor란건 이전 keyframe에서 현재 keyframe까지의 상대 움직임은 scan-to-map 결과가 알려준 만큼이어야 한다는 의미를 가진다. 
            gtSAMgraph.add(BetweenFactor<Pose3>(cloudKeyPoses3D->size()-1, cloudKeyPoses3D->size(), poseFrom.between(poseTo), odometryNoise));
            initialEstimate.insert(cloudKeyPoses3D->size(), poseTo);
        }
    }

addGPSFactor();

이게… 내가 평상시에 gps를 쓸 일이 잘 없어서.. 생략하고 싶어진달까.

우선 GPS데이터가 그래프에 들어가는 조건들이 있다.

• gps가 없다면 끝.

  if (gpsQueue.empty())
    return;
  

• pose가 이미 확실하면 GPS를 굳이 안 넣는다 : 즉 현재 SLAM pose의 x/y covariance가 충분히 작으면….

  if (poseCovariance(3,3) < poseCovThreshold &&
    poseCovariance(4,4) < poseCovThreshold)
    return;
  

• 시간적으로 현재 LiDAR와 가까운 GPS 선택

  timeLaserInfoCur ± 0.2 sec
  

• GPS covariance도 검사해본다.

  float noise_x = thisGPS.pose.covariance[0];
  float noise_y = thisGPS.pose.covariance[7];
  if (noise_x > gpsCovThreshold || noise_y > gpsCovThreshold)
    continue;
  

• GPS elevation을 쓰지 않을 수도 있다

  if (!useGpsElevation)
  {
    gps_z = transformTobeMapped[5];
    noise_z = 0.01;
  }
  

• GPS를 너무 자주 넣지도 않는다

  if (pointDistance(curGPSPoint,lastGPSPoint) < 5.0)
   continue;
  

이걸 만족하면, GPSFactor를 생성한다

gtsam::GPSFactor gps_factor( cloudKeyPoses3D->size(),  
//크기값을 넣으면 자동으로 인덱스가 된다. 9개 들어가있으면, 9가 뜨고, 그게 index가 되면 0~9까지 10개가 되는 원리다. 
                             gtsam::Point3(gps_x, gps_y, gps_z), gps_noise); //요건 그 팩터의 데이터들
gtSAMgraph.add(gps_factor);

addLoopFactor()

ICP가 현재 키프레임과 과거 submap을 정합하면

BetweenFactor<Pose3>(current, previous, relative_pose, noise)

를 추가한다.

loopclosure는 performLoopClosure()함수를 통해 별도 스레드에서 수행되고 있다. rformLoopClosure()는 현재 최신 keyframe이 과거의 어떤 keyframe 근처에 다시 왔다고 판단되면, 두 구간의 point cloud를 ICP로 정합해서 loop closure용 상대 pose constraint를 만드는 함수다. LeGO-LOAM에서 가져온 ICP 기반 proof-of-concept 방식이며, 거리 기반 후보 탐색 + ICP 검증 구조를 가진다.

최신 keyframe 존재?
    ↓
key pose 복사
    ↓
loop 후보 keyframe 탐색
    ├─ 외부 loop detector
    └─ 거리 기반 탐색
    ↓
현재 keyframe cloud 구성
과거 주변 keyframe cloud 구성
    ↓
ICP
    ↓
수렴 여부 / fitness 검사
    ↓
ICP correction으로 현재 pose 보정
    ↓
현재 ↔ 과거 keyframe BetweenFactor 계산
    ↓
loopPoseQueue 등에 저장
    ↓
다음 saveKeyFramesAndFactor()에서 GTSAM graph에 추가

상세한 코드는 나중에 따로 보기로 하자.

isam->update();

좀 희한한 과정이 하나 더 지나간다.

// update iSAM
        isam->update(gtSAMgraph, initialEstimate); //여기는 초기추정치가 들어간다. 
        isam->update(); //현재 내부 graph를 가지고 한번더 업데이트한다. 

        if (aLoopIsClosed == true) //루프나, gps처럼 큰 전역 correction이 생기면
        {
            //refinement를 해본다.             
            isam->update();
            isam->update();
            isam->update();
            isam->update();
            isam->update();
        }

이후

최적화과정을 거쳤다면, 이젠 데이터를 정리해서 저장하는 과정을 거친다. 이후의 내용들을 정리해보면 다음과 같다.

1. iSAM2 최적화 결과 가져오기
        ↓
isamCurrentEstimate = isam->calculateEstimate();

2. 가장 최신 keyframe pose 꺼내기
        ↓
latestEstimate

3. 최신 pose를 cloudKeyPoses3D / cloudKeyPoses6D에 저장
        ↓
keyframe pose 등록

4. 최신 pose covariance 계산
        ↓
poseCovariance

5. transformTobeMapped를
   iSAM2 최적화 결과로 갱신
        ↓
현재 pose를 graph 결과와 동기화

6. 현재 scan의 corner/surface feature를
   keyframe cloud로 저장
        ↓
cornerCloudKeyFrames
surfCloudKeyFrames

7. path 업데이트

isamCurrentEstimate = isam->calculateEstimate();
latestEstimate = isamCurrentEstimate.at<gtsam::Pose3>(isamCurrentEstimate.size() - 1);

// 최적화된 pose 저장
cloudKeyPoses3D->push_back(thisPose3D);
cloudKeyPoses6D->push_back(thisPose6D);

// pose 불확실성 저장
poseCovariance = isam->marginalCovariance(isamCurrentEstimate.size() - 1);

// 현재 pose를 GTSAM 결과로 갱신
transformTobeMapped[...] = latestEstimate...;

// 현재 feature cloud를 keyframe으로 저장
cornerCloudKeyFrames.push_back(thisCornerKeyFrame);
surfCloudKeyFrames.push_back(thisSurfKeyFrame);

// trajectory/path 갱신
updatePath(thisPose6D);

correctPoses();

거의 다왔다. 여기까지만 더 보자.
loop closure나 GPS factor 때문에 iSAM2가 과거 keyframe들의 pose를 바꿨을 때,
그 최적화 결과를 실제로 cloudKeyPoses3D, cloudKeyPoses6D, globalPath에 다시 반영하는 함수다.

void correctPoses()
{
    //keyframe이 없으면 종료
    if (cloudKeyPoses3D->points.empty())
        return;
    
    //aLoopIsClosed == true일 때만 전체 correction
    // loop가 추가되거나, GPS가 추가되는 경우다. 
    if (aLoopIsClosed == true)
    {
        // clear map cache 
        // 과거에 특정 keyframe cloud를 그 당시 pose로 map 좌표계에 변환한 결과를 캐싱해둔 자료구조
        //이걸 지우고 다시 최적화하는 과정이 필요하다. 
        laserCloudMapContainer.clear();
        // clear path 기존 path도 삭제
        globalPath.poses.clear();
        
        // update key poses
        //iSAM2에 몇 개 pose가 있는지 확인
        int numPoses = isamCurrentEstimate.size();
        for (int i = 0; i < numPoses; ++i)
        {
            //cloudKeyPoses3D 업데이트
            cloudKeyPoses3D->points[i].x = isamCurrentEstimate.at<Pose3>(i).translation().x();
            cloudKeyPoses3D->points[i].y = isamCurrentEstimate.at<Pose3>(i).translation().y();
            cloudKeyPoses3D->points[i].z = isamCurrentEstimate.at<Pose3>(i).translation().z();

            //cloudKeyPoses6D도 업데이트
            cloudKeyPoses6D->points[i].x = cloudKeyPoses3D->points[i].x;
            cloudKeyPoses6D->points[i].y = cloudKeyPoses3D->points[i].y;
            cloudKeyPoses6D->points[i].z = cloudKeyPoses3D->points[i].z;
            cloudKeyPoses6D->points[i].roll  = isamCurrentEstimate.at<Pose3>(i).rotation().roll();
            cloudKeyPoses6D->points[i].pitch = isamCurrentEstimate.at<Pose3>(i).rotation().pitch();
            cloudKeyPoses6D->points[i].yaw   = isamCurrentEstimate.at<Pose3>(i).rotation().yaw();
            //path를 다시 만들기
            updatePath(cloudKeyPoses6D->points[i]);
        }

        aLoopIsClosed = false;
    }
}

publishOdometry()와 publishFrames()

publishOdometry()와 publishFrames()는 둘 다 최적화 결과를 ROS2 topic으로 내보내는 출력 단계다. 알고리즘 자체의 핵심이라기보다 결과를 다른 노드와 RViz에서 사용할 수 있게 만드는 역할을 한다.

publishOdometry()는 현재 추정 pose를 nav_msgs::msg::Odometry 형태로 publish한다. 크게 두 종류를 내보낸다. 하나는 transformTobeMapped를 기반으로 한 global/map 기준 odometry이고, 다른 하나는 incrementalOdometryAffineFront/Back을 이용해 만든 연속적인 incremental odometry다. 즉 전자는 “현재 map에서 어디에 있는가”, 후자는 “직전 대비 얼마나 움직였는가”에 가깝다.

publishFrames()는 point cloud와 path 같은 시각화용/외부 사용용 데이터를 publish한다. 대표적으로 keyframe pose cloud, 주변 local map, 현재 등록된 LiDAR scan, global path 등을 topic으로 내보낸다. pubLaserCloudSurround, pubLaserCloudRegistered, pubPath 등의 subscriber가 있을 때 필요한 point cloud를 만들고 publish하는 구조다.

이번편이 너무 길어져서…..별도의 코드리뷰는 생략한다.

오늘은 여기까지..