참고.
https://github.com/hku-mars/FAST_LIO
https://github.com/gisbi-kim/SC-A-LOAM
https://github.com/hku-mars/loam_livox

pc 환경 : Ubuntu 18.04 Melodic, ROS1, desktop, ouster lidar 64ch

1. 워크스페이스 생성

sc-pgo-tutorial 이란 폴더를 만들고, 안의 src 폴더를 만들어서 아래 명령으로 3개의 폴더를 복사한다.

git clone https://github.com/hku-mars/FAST_LIO
git clone https://github.com/gisbi-kim/SC-A-LOAM
git clone https://github.com/hku-mars/loam_livox

그리고 sc-pgo-tutorial 폴더에서 컴파일을 실행한다.

cd ..
catkin_make

❗ 2022년 04월 현재 FAST_LIO의 ikd-Tree 폴더에 파일이 없어 의존성 오류가 난다.
FAST_LIO - inclue - ikd-Tree 로 들어가 다운 받아 넣어주자.
❗ 만약 다른 의존성 오류가 나면, 그 오류만 나는 폴더를 빼고(FAST_LIO , SC-A-LOAM or loam_livox) 먼저 캣킨 메이크를 한 뒤 오류가 나면 다시 그 폴더를 넣고 컴파일을 하자.

2. 사전 작업

PGO node(i.e., laserPosegraphOptimization.cpp in SC-A-LOAM)는 다음 두 토픽 타입으로 이용되는데

• odom
• keyframe scan

두 모듈 사이의 토픽 이름을 똑같이 만들어야 한다.
편집기를 띄워주자.
src의 FAST_LIO의 src로 들어가서 ouster 전용으로 만들기 위해, lasermapping.cpp를 복사하여 lasermapping_ouster로 이름을 변경하고 열자. 여기서 다음의 publishes를 찾자. 이 퍼블리셔 위치를 기억하자.

• odometry topic "Odometry"
• point cloud scan topic "cloud_registered"
  

이제 SC-A-LOAM의 launch에서 aloma_mulran.launch를 aloam_fast_lio.launch로 바꿔주고,

nodes에 alaserPGO type만 남겨두고 주석 처리를 해준다.

그리고 다음을 추가 해준다.

    <remap from="/velodyne_cloud_registered_local" to="/cloud_registered"/>
    <remap from="/aft_mapped_to_init" to="/Odometry"/>

또한, utils의 save 경로 설정이 있는데, 경로를 본인 폴더에 맞게 바꿔주자.

"cloud_registered" 토픽은 글로벌 프레임으로 표현되지만, Scan Context는 로컬로 표현된다.
그래서 local로 받아 줄 필요가 있는데, Fast_LIO의 lasermapping_ouster.cpp로 다시 들어가

cloud_registered의 퍼블리셔를 찾아 하단 그림의 아래에

다음을 복사하여 넣어준다.(local 변수를 새로 생성 해줬다) 그리고 상단 그림의 publish_frame_world 함수의 save map 전체를 주석 처리 해준다.

void publish_frame_local(const ros::Publisher & pubLaserCloudFullLocal)
{
    if(scan_pub_en)
    {
        PointCloudXYZI::Ptr laserCloudFullRes(dense_pub_en ? feats_undistort : feats_down_body);
        int size = laserCloudFullRes->points.size();
        PointCloudXYZI::Ptr laserCloudWorldlocal( \
                        new PointCloudXYZI(size, 1));

        for (int i = 0; i < size; i++)
        {
            RGBpointBodyToWorld(&laserCloudFullRes->points[i], \
                                &laserCloudWorldlocal->points[i]);
        }

        sensor_msgs::PointCloud2 laserCloudmsgLocal;
        pcl::toROSMsg(*laserCloudFullRes, laserCloudmsgLocal);
        laserCloudmsgLocal.header.stamp = ros::Time().fromSec(lidar_end_time);
        laserCloudmsgLocal.header.frame_id = "camera_init";
        pubLaserCloudFullLocal.publish(laserCloudmsgLocal);
        publish_count -= PUBFRAME_PERIOD;

    }

    /**************** save map ****************/
    /* 1. make sure you have enough memories
    /* 2. noted that pcd save will influence the real-time performences **/
    if (pcd_save_en)
    {
        int size = feats_undistort->points.size();
        PointCloudXYZI::Ptr laserCloudWorldlocal( \
                        new PointCloudXYZI(size, 1));

        for (int i = 0; i < size; i++)
        {
            RGBpointBodyToWorld(&feats_undistort->points[i], \
                                &laserCloudWorldlocal->points[i]);
        }
        *pcl_wait_save += *laserCloudWorldlocal;

        static int scan_wait_num = 0;
        scan_wait_num ++;
        if (pcl_wait_save->size() > 0 && pcd_save_interval > 0  && scan_wait_num >= pcd_save_interval)
        {
            pcd_index ++;
            string all_points_dir(string(string(ROOT_DIR) + "PCD/scans_") + to_string(pcd_index) + string(".pcd"));
            pcl::PCDWriter pcd_writer;
            cout << "current scan saved to /PCD/" << all_points_dir << endl;
            pcd_writer.writeBinary(all_points_dir, *pcl_wait_save);
            pcl_wait_save->clear();
            scan_wait_num = 0;
        }
    }
}

전후

퍼블리셔도 똑같이 복사하여 Local을 붙여준다.

전후

다음과 같이 publish point를 생성하는 부분에 publish_frame_world 함수는 주석처리하고

if (scan_pub_en || pcd_save_en)      publish_frame_local(pubLaserCloudFullLocal);

코드를 넣어준다.

\sc-pgo-tutorial\src\FAST_LIO\src에 preprocess.h를 보면, lidar 타입을 정할 수 있는데, Ouster 64ch도 포함되어 있다. 우리는 ouster를 사용하기 때문에 이 변수를 사용하자.

다시 laserMapping_ouster.cpp로 가서 모든 AVIA를 OUST64로 바꾸자.

이제 \SC-A-LOAM\aloam_fast_lio.launch 파일의 remap을 다시 수정해준다.

• 기존 cloud_registered를 cloud_registered_local로 바꿔준다.

    <remap from="/velodyne_cloud_registered_local" to="/cloud_registered_local"/>
    <remap from="/aft_mapped_to_init" to="/Odometry"/>

• 그리고 mapping 시각화 밀도를 변경하기 위해 keyframe_meter_gap의 값을 낮춰준다.

<param name="keyframe_meter_gap" type="double" value="0.2"/>

• for MulRan 파트에서 param과 remap은 주석 처리를 해준다.
  (여기서 pgo node만 사용하기 때문에A-LOAM의 다른 odometry와 mapping node를 제거해준다.)

    <!-- for MulRan Don't care here -->
    <!-- <param name="lidar_type" type="string" value="OS1-64"/>  -->
    <!-- <remap from="/velodyne_points" to="/os1_points"/> -->

그리고 \SC-A-LOAM\laserPosegraphOptimization.cpp로 이동하여 voxel downsampling 값도 줄여준다.
또한 빠른 컴퓨팅 처리를 위해 iteration 수를 조정한다.
\FAST_LIO\launch\mapping_ouster64.launch의 max_interaion에 적당한 값을 준다.

	<param name="max_iteration" type="int" value="25" />

\sc-pgo-tutorial\src\FAST_LIO\launch의 mapping_ouster64.launch파일의 node pkg의 이름을 우리가 만든 lasermapping_ouster로 바꿔주자.

ouster64.yaml 파일에 라이다 매핑 fov 각과 거리 등을 조절할 수 있다. 참고하자.

마지막으로 compile을 해주자.

catkin_make

3. 실행

aloam_fast_lio.launch를 실행해보겠다.

source devel/setup.bash
roslaunch aloam_velodyne aloam_fast_lio.launch 

pgo node가 실행되면 rviz가 같이 켜지는데 어차피 후술할 fast_lio를 실행하면 rviz가 실행되기 때문에 rviz를 꺼주겠다.
\SC-A-LOAM\launch의 aloam_fast_lio.launch의 visual 부분을 주석 처리하자.

    <!-- visulaization -->
    <!-- <arg name="rviz" default="true" />
    <group if="$(arg rviz)">
        <node launch-prefix="nice" pkg="rviz" type="rviz" name="rviz" args="-d $(find aloam_velodyne)/rviz_cfg/aloam_velodyne.rviz" />
    </group> -->

다시 런치를 실행시키면, pgo node는 odom과 pointcloud2 토픽 플로우를 기다리게 된다.
다음 런치를 실행하면, rviz가 뜨면서 모든 준비가 끝나게 된다.

roslaunch fast_lio mapping_ouster64.launch

이제 ouster rosbag을 실행하면, rviz에 slam이 되는 것을 볼 수 있다.

4. 결과

SLAM using FAST-LIO in kmu

참고 : https://www.youtube.com/watch?v=Fw9S6D6HozA