A-LOAM code study

A-LOAM code

홍콩과기대 Aerial Robotics Group

LOAM 코드가 따로 공개가 되지 않아 논문을 기반 코드를 작성하여 배포한게 A-LOAM이라고 한다.

Eigen과 Ceres Solver를 이용해 코드를 간략하게 만들었다고 하는데, 이참에 Ceres Solver에 대해 공부를 같이 할 수 있는 좋은 기회가 될 것 같다.

ROS를 기반으로 하며 launch 파일을 실행하면 세가지 노드가 실행된다:

  • Scan Registration (ascanRegistration)
  • Laser Odometry (alaserOdometry)
  • Laser Mapping (alaserMapping)

이를 rqt_graph로 보게 되면 다음과 같다.

RQT

  1. Scan Registration
    • Input: /velodyne_points
    • Output:
      • /velodyne_cloud_2
      • /laser_remove_points
      • /laser_cloud_flat
      • /laser_cloud_less_flat
      • /laser_cloud_sharp
      • /laser_cloud_less_sharp
  2. Laser Odometry
    • Input:
      • /velodyne_cloud_2
      • /laser_remove_points
      • /laser_cloud_flat
      • /laser_cloud_less_flat
      • /laser_cloud_sharp
      • /laser_cloud_less_sharp
    • Output:
      • /velodyne_cloud_3
      • /laser_cloud_surf_last
      • /laser_cloud_corner_last
      • /laser_odom_to_init
      • /laser_odom_path
  3. Laser Mapping
    • Input:
      • /velodyne_cloud_3
      • /laser_cloud_surf_last
      • /laser_cloud_corner_last
      • /laser_odom_to_init
    • Output:
      • /laser_cloud_map
      • /aft_mapped_to_init
      • /aft_mapped_to_init_high_frec
      • /aft_mapped_path
      • /velodyne_cloud_registered
      • /laser_cloud_surround

위와 같이 raw lidar cloud가 먼저 scan registeration 노드에 들어가 가공된 다음 laser odometery 노드에 들어가고 laser mapping에서 mapping 및 registeration이 되는 것 같다.

Scan Registration

LiDAR point cloud 가 처음 들어오면 먼저 하는 일이 NaN point와 근처 point cloud를 삭제하는 일이다. 이는 removeNaNFromPointCloud 함수와 removeNaNNormalsFromPointCloud 함수로 진행되며, 여기서 한가지 간단하지만 간과했던 점은 vector에서 resize를 통해 빠르게 점을 추가할 수 있다는 점이다. 예를 들어 어떤 집합에서 조건을 만족하는 element만 추려내고자 할 때 이전까지는 그러한 element를 새로운 vector에 push_back 함으로써 추려냈는데, 여기서는 새로운 vector를 집합의 크기와 같에 initialization 시킨 후, for문을 돌며 조건을 만족할 때 하나씩 채워나간 뒤, redundent한 나머지 vector element를 삭제하기 위해 resize를 이용했다는 점이다.

Velodyne LiDAR를 사용한적이 없는데, velodyne에서는 점이 몇번째 channel인지 (ring) 알려주지 않는 듯 하다. 따라서 NaN point와 close point를 삭제한 뒤, 각 point 각도에 따라 몇번째 ring인지 구분한 뒤, 각 ring별 point로 분류하며, scan의 시작 시간과 끝 시간을 고려해 각 point들이 들어온 시점도 계산한다.

이후 corner점과 surface점을 구분하기 위해 각 점마다 좌 우 5개씩 점들을 모아 계산을 진행한다. x, y, z 값에 따라 다음과 같이 계산한다.

\[\begin{align} diff_{x} = p_{i-5}^x + p_{i-4}^x + p_{i-3}^x + p_{i-2}^x + p_{i-1}^x - 10p_{i}^x + p_{i+1}^x + p_{i+2}^x + p_{i+3}^x + p_{i+4}^x + p_{i+5}^x \\ diff_{y} = p_{i-5}^y + p_{i-4}^y + p_{i-3}^y + p_{i-2}^y + p_{i-1}^y - 10p_{i}^y + p_{i+1}^y + p_{i+2}^y + p_{i+3}^y + p_{i+4}^y + p_{i+5}^y \\ diff_{z} = p_{i-5}^z + p_{i-4}^z + p_{i-3}^z + p_{i-2}^z + p_{i-1}^z - 10p_{i}^z + p_{i+1}^z + p_{i+2}^z + p_{i+3}^z + p_{i+4}^z + p_{i+5}^z \end{align}\]

그리고 curvature는 다음과 같이 정의한다.

\[c = diff_{x}^2 + diff_{y}^2 + diff_{z}^2\]

이후 point의 성질을 다음과 같이 네가지로 분류한다.

  • cornerPointsSharp
  • cornerPointsLessSharp
  • surfPointsFlat
  • surfPointsLessFlat

위의 분류를 하기 전 scanStartInd와 scanEndInd로 각 ring의 시작점과 끝점을 표현하는데, 위의 수식처럼 처음 5개와 끝의 5개에 대해서는 curvature를 구할 수 없으니 이들을 빼고 작성한다.

각 링에 대해서

for (int i = 0; i < N_SCANS; i++)
{
    if( scanEndInd[i] - scanStartInd[i] < 6)
        continue;
    pcl::PointCloud<PointType>::Ptr surfPointsLessFlatScan(new pcl::PointCloud<PointType>);
    for (int j = 0; j < 6; j++)
    {
        int sp = scanStartInd[i] + (scanEndInd[i] - scanStartInd[i]) * j / 6; 
        int ep = scanStartInd[i] + (scanEndInd[i] - scanStartInd[i]) * (j + 1) / 6 - 1;

        TicToc t_tmp;
        std::sort (cloudSortInd + sp, cloudSortInd + ep + 1, comp);
        t_q_sort += t_tmp.toc();

        int largestPickedNum = 0;
        for (int k = ep; k >= sp; k--)
        {
            int ind = cloudSortInd[k]; 

            if (cloudNeighborPicked[ind] == 0 &&
                cloudCurvature[ind] > 0.1)
            {

                largestPickedNum++;
                if (largestPickedNum <= 2)
                {                        
                    cloudLabel[ind] = 2;
                    cornerPointsSharp.push_back(laserCloud->points[ind]);
                    cornerPointsLessSharp.push_back(laserCloud->points[ind]);
                }
                else if (largestPickedNum <= 20)
                {                        
                    cloudLabel[ind] = 1; 
                    cornerPointsLessSharp.push_back(laserCloud->points[ind]);
                }
                else
                {
                    break;
                }

                cloudNeighborPicked[ind] = 1; 

                for (int l = 1; l <= 5; l++)
                {
                    float diffX = laserCloud->points[ind + l].x - laserCloud->points[ind + l - 1].x;
                    float diffY = laserCloud->points[ind + l].y - laserCloud->points[ind + l - 1].y;
                    float diffZ = laserCloud->points[ind + l].z - laserCloud->points[ind + l - 1].z;
                    if (diffX * diffX + diffY * diffY + diffZ * diffZ > 0.05)
                    {
                        break;
                    }

                    cloudNeighborPicked[ind + l] = 1;
                }
                for (int l = -1; l >= -5; l--)
                {
                    float diffX = laserCloud->points[ind + l].x - laserCloud->points[ind + l + 1].x;
                    float diffY = laserCloud->points[ind + l].y - laserCloud->points[ind + l + 1].y;
                    float diffZ = laserCloud->points[ind + l].z - laserCloud->points[ind + l + 1].z;
                    if (diffX * diffX + diffY * diffY + diffZ * diffZ > 0.05)
                    {
                        break;
                    }

                    cloudNeighborPicked[ind + l] = 1;
                }
            }
        }

        int smallestPickedNum = 0;
        for (int k = sp; k <= ep; k++)
        {
            int ind = cloudSortInd[k];

            if (cloudNeighborPicked[ind] == 0 &&
                cloudCurvature[ind] < 0.1)
            {

                cloudLabel[ind] = -1; 
                surfPointsFlat.push_back(laserCloud->points[ind]);

                smallestPickedNum++;
                if (smallestPickedNum >= 4)
                { 
                    break;
                }

                cloudNeighborPicked[ind] = 1;
                for (int l = 1; l <= 5; l++)
                { 
                    float diffX = laserCloud->points[ind + l].x - laserCloud->points[ind + l - 1].x;
                    float diffY = laserCloud->points[ind + l].y - laserCloud->points[ind + l - 1].y;
                    float diffZ = laserCloud->points[ind + l].z - laserCloud->points[ind + l - 1].z;
                    if (diffX * diffX + diffY * diffY + diffZ * diffZ > 0.05)
                    {
                        break;
                    }

                    cloudNeighborPicked[ind + l] = 1;
                }
                for (int l = -1; l >= -5; l--)
                {
                    float diffX = laserCloud->points[ind + l].x - laserCloud->points[ind + l + 1].x;
                    float diffY = laserCloud->points[ind + l].y - laserCloud->points[ind + l + 1].y;
                    float diffZ = laserCloud->points[ind + l].z - laserCloud->points[ind + l + 1].z;
                    if (diffX * diffX + diffY * diffY + diffZ * diffZ > 0.05)
                    {
                        break;
                    }

                    cloudNeighborPicked[ind + l] = 1;
                }
            }
        }

        for (int k = sp; k <= ep; k++)
        {
            if (cloudLabel[k] <= 0)
            {
                surfPointsLessFlatScan->push_back(laserCloud->points[k]);
            }
        }
    }

    pcl::PointCloud<PointType> surfPointsLessFlatScanDS;
    pcl::VoxelGrid<PointType> downSizeFilter;
    downSizeFilter.setInputCloud(surfPointsLessFlatScan);
    downSizeFilter.setLeafSize(0.2, 0.2, 0.2);
    downSizeFilter.filter(surfPointsLessFlatScanDS);

    surfPointsLessFlat += surfPointsLessFlatScanDS;
}

워와 같은 코드로 작성이 되는데 하나 하나씩 들여다보자.

if( scanEndInd[i] - scanStartInd[i] < 6)
    continue;

먼저 한개의 ring안에 점이 6개 이하인 경우 continue.

for (int j = 0; j < 6; j++)
{
    int sp = scanStartInd[i] + (scanEndInd[i] - scanStartInd[i]) * j / 6; 
    int ep = scanStartInd[i] + (scanEndInd[i] - scanStartInd[i]) * (j + 1) / 6 - 1;

    ...

}

한개의 ring point cloud들에 대해 6분할을 하며, sp와 ep는 각 분할의 시작 index와 끝 index이다.

std::sort (cloudSortInd + sp, cloudSortInd + ep + 1, comp);

comp함수는 i, j point index에 대해 curvature(i) < curvature(j) 를 return하는 함수로서, 위의 함수는 cloudSortInd의 sp번째부터 ep번째까지를 curvature 오름차순으로 정렬한다.

이후 int largestPickedNum = 0; 으로 놓고 curvature가 가장 큰 순번부터 작아지는 순번으로 for문을 돌린다.

첫번째 for문: Corner점 뽑기

for (int k = ep; k >= sp; k--)
{
    int ind = cloudSortInd[k]; 

    if (cloudNeighborPicked[ind] == 0 &&
        cloudCurvature[ind] > 0.1)
    {

        largestPickedNum++;
        if (largestPickedNum <= 2)
        {                        
            cloudLabel[ind] = 2;
            cornerPointsSharp.push_back(laserCloud->points[ind]);
            cornerPointsLessSharp.push_back(laserCloud->points[ind]);
        }
        else if (largestPickedNum <= 20)
        {                        
            cloudLabel[ind] = 1; 
            cornerPointsLessSharp.push_back(laserCloud->points[ind]);
        }
        else
        {
            break;
        }

        cloudNeighborPicked[ind] = 1; 

        for (int l = 1; l <= 5; l++)
        {
            float diffX = laserCloud->points[ind + l].x - laserCloud->points[ind + l - 1].x;
            float diffY = laserCloud->points[ind + l].y - laserCloud->points[ind + l - 1].y;
            float diffZ = laserCloud->points[ind + l].z - laserCloud->points[ind + l - 1].z;
            if (diffX * diffX + diffY * diffY + diffZ * diffZ > 0.05)
            {
                break;
            }

            cloudNeighborPicked[ind + l] = 1;
        }
        for (int l = -1; l >= -5; l--)
        {
            float diffX = laserCloud->points[ind + l].x - laserCloud->points[ind + l + 1].x;
            float diffY = laserCloud->points[ind + l].y - laserCloud->points[ind + l + 1].y;
            float diffZ = laserCloud->points[ind + l].z - laserCloud->points[ind + l + 1].z;
            if (diffX * diffX + diffY * diffY + diffZ * diffZ > 0.05)
            {
                break;
            }

            cloudNeighborPicked[ind + l] = 1;
        }
    }
}

여기서, 몇가지만 보자:

  • cloudNeighborPicked는 바로 이웃 점이 4가지 점중 하나로 뽑혔을 경우 1, 아닌 경우 0이며
  • cloudLabel의 경우
    • -1: Flat
    • 0: Less Flat
    • 1: Less Sharp
    • 2: Sharp 이다.
if (cloudNeighborPicked[ind] == 0 &&
    cloudCurvature[ind] > 0.1)

주변의 point가 Neighbor가 아닌 경우와 curvature가 0.1 이상인 경우 수행:

if (largestPickedNum <= 2)
{                        
    cloudLabel[ind] = 2;
    cornerPointsSharp.push_back(laserCloud->points[ind]);
    cornerPointsLessSharp.push_back(laserCloud->points[ind]);
}

가장 curvature가 큰 2개에 대해서 sharp corner, less sharp corner에 추가

else if (largestPickedNum <= 20)
{                        
    cloudLabel[ind] = 1; 
    cornerPointsLessSharp.push_back(laserCloud->points[ind]);
}
else
{
    break;
}

나머지 18개에 대해서는 less sharp corner에 추가하고 (총 20개, 2개 중복), 나머지에 대해서는 break. 재밌는건, 난 curvature에 따라 분류할 줄 알았는데, 상대적인 curvature에 대해 분류를 진행한다는 점이다.

cloudNeighborPicked[ind] = 1; 

현재 선택된 점에 대해서는 NeighborPicked가 1로 설정: 나중에 flat point때 선택하지 않기 위해서 놓은 장치인 듯 하다.

for (int l = 1; l <= 5; l++)
{
    float diffX = laserCloud->points[ind + l].x - laserCloud->points[ind + l - 1].x;
    float diffY = laserCloud->points[ind + l].y - laserCloud->points[ind + l - 1].y;
    float diffZ = laserCloud->points[ind + l].z - laserCloud->points[ind + l - 1].z;
    if (diffX * diffX + diffY * diffY + diffZ * diffZ > 0.05)
    {
        break;
    }

    cloudNeighborPicked[ind + l] = 1;
}
for (int l = -1; l >= -5; l--)
{
    float diffX = laserCloud->points[ind + l].x - laserCloud->points[ind + l + 1].x;
    float diffY = laserCloud->points[ind + l].y - laserCloud->points[ind + l + 1].y;
    float diffZ = laserCloud->points[ind + l].z - laserCloud->points[ind + l + 1].z;
    if (diffX * diffX + diffY * diffY + diffZ * diffZ > 0.05)
    {
        break;
    }

    cloudNeighborPicked[ind + l] = 1;
}

좌 우 5개 점들에 대해 판단하는데, 점간 거리의 제곱이 0.05m 이상인 경우 논문에서 말하는 redundent point일 수 있으니, 이들에 대해서는 판단을 보류하는 듯

두번째 for문: Flat점 뽑기

int smallestPickedNum = 0;
for (int k = sp; k <= ep; k++)
{
    int ind = cloudSortInd[k];

    if (cloudNeighborPicked[ind] == 0 &&
        cloudCurvature[ind] < 0.1)
    {

        cloudLabel[ind] = -1; 
        surfPointsFlat.push_back(laserCloud->points[ind]);

        smallestPickedNum++;
        if (smallestPickedNum >= 4)
        { 
            break;
        }

        cloudNeighborPicked[ind] = 1;
        for (int l = 1; l <= 5; l++)
        { 
            float diffX = laserCloud->points[ind + l].x - laserCloud->points[ind + l - 1].x;
            float diffY = laserCloud->points[ind + l].y - laserCloud->points[ind + l - 1].y;
            float diffZ = laserCloud->points[ind + l].z - laserCloud->points[ind + l - 1].z;
            if (diffX * diffX + diffY * diffY + diffZ * diffZ > 0.05)
            {
                break;
            }

            cloudNeighborPicked[ind + l] = 1;
        }
        for (int l = -1; l >= -5; l--)
        {
            float diffX = laserCloud->points[ind + l].x - laserCloud->points[ind + l + 1].x;
            float diffY = laserCloud->points[ind + l].y - laserCloud->points[ind + l + 1].y;
            float diffZ = laserCloud->points[ind + l].z - laserCloud->points[ind + l + 1].z;
            if (diffX * diffX + diffY * diffY + diffZ * diffZ > 0.05)
            {
                break;
            }

            cloudNeighborPicked[ind + l] = 1;
        }
    }
}

이번엔 작은 순부터 본다. Curvature가 0.1보다 작은 첫 4개에 대해서만 진행을 하며 마찬가지로 redundent한 점들은 제외

잘 이해는 안되지만, 논문에서 말한 6개 (corner 2개, flat 4개) 외에도 less sharp, less flat을 각각 20개, 4개를 뽑는다. (20개중 2개, 4개중 4개는 중복)

그중, Less flat에 대해서는 0.2m 짜기 voxel grid로 downsize한다.

이후 publish:


sensor_msgs::PointCloud2 laserCloudOutMsg;
pcl::toROSMsg(*laserCloud, laserCloudOutMsg);
laserCloudOutMsg.header.stamp = laserCloudMsg->header.stamp;
laserCloudOutMsg.header.frame_id = "/camera_init";
pubLaserCloud.publish(laserCloudOutMsg);

sensor_msgs::PointCloud2 cornerPointsSharpMsg;
pcl::toROSMsg(cornerPointsSharp, cornerPointsSharpMsg);
cornerPointsSharpMsg.header.stamp = laserCloudMsg->header.stamp;
cornerPointsSharpMsg.header.frame_id = "/camera_init";
pubCornerPointsSharp.publish(cornerPointsSharpMsg);

sensor_msgs::PointCloud2 cornerPointsLessSharpMsg;
pcl::toROSMsg(cornerPointsLessSharp, cornerPointsLessSharpMsg);
cornerPointsLessSharpMsg.header.stamp = laserCloudMsg->header.stamp;
cornerPointsLessSharpMsg.header.frame_id = "/camera_init";
pubCornerPointsLessSharp.publish(cornerPointsLessSharpMsg);

sensor_msgs::PointCloud2 surfPointsFlat2;
pcl::toROSMsg(surfPointsFlat, surfPointsFlat2);
surfPointsFlat2.header.stamp = laserCloudMsg->header.stamp;
surfPointsFlat2.header.frame_id = "/camera_init";
pubSurfPointsFlat.publish(surfPointsFlat2);

sensor_msgs::PointCloud2 surfPointsLessFlat2;
pcl::toROSMsg(surfPointsLessFlat, surfPointsLessFlat2);
surfPointsLessFlat2.header.stamp = laserCloudMsg->header.stamp;
surfPointsLessFlat2.header.frame_id = "/camera_init";
pubSurfPointsLessFlat.publish(surfPointsLessFlat2);

Laser cloud 는 각 ring의 처음과 끝 5개점을 제외한 point들이다.

Laser Odometry

Laser Odometry node에서는 Scan Registration에서 처리된 메세지들을 받아 다음과 같은 event를 수행한다.

ros::Subscriber subCornerPointsSharp = nh.subscribe<sensor_msgs::PointCloud2>("/laser_cloud_sharp", 100, laserCloudSharpHandler);
ros::Subscriber subCornerPointsLessSharp = nh.subscribe<sensor_msgs::PointCloud2>("/laser_cloud_less_sharp", 100, laserCloudLessSharpHandler);
ros::Subscriber subSurfPointsFlat = nh.subscribe<sensor_msgs::PointCloud2>("/laser_cloud_flat", 100, laserCloudFlatHandler);
ros::Subscriber subSurfPointsLessFlat = nh.subscribe<sensor_msgs::PointCloud2>("/laser_cloud_less_flat", 100, laserCloudLessFlatHandler);
ros::Subscriber subLaserCloudFullRes = nh.subscribe<sensor_msgs::PointCloud2>("/velodyne_cloud_2", 100, laserCloudFullResHandler);

Event는 std::queue에 대해 메세지를 추가한다.

void laserCloudSharpHandler(const sensor_msgs::PointCloud2ConstPtr &cornerPointsSharp2)
{
    mBuf.lock();
    cornerSharpBuf.push(cornerPointsSharp2);
    mBuf.unlock();
}

void laserCloudLessSharpHandler(const sensor_msgs::PointCloud2ConstPtr &cornerPointsLessSharp2)
{
    mBuf.lock();
    cornerLessSharpBuf.push(cornerPointsLessSharp2);
    mBuf.unlock();
}

void laserCloudFlatHandler(const sensor_msgs::PointCloud2ConstPtr &surfPointsFlat2)
{
    mBuf.lock();
    surfFlatBuf.push(surfPointsFlat2);
    mBuf.unlock();
}

void laserCloudLessFlatHandler(const sensor_msgs::PointCloud2ConstPtr &surfPointsLessFlat2)
{
    mBuf.lock();
    surfLessFlatBuf.push(surfPointsLessFlat2);
    mBuf.unlock();
}

//receive all point cloud
void laserCloudFullResHandler(const sensor_msgs::PointCloud2ConstPtr &laserCloudFullRes2)
{
    mBuf.lock();
    fullPointsBuf.push(laserCloudFullRes2);
    mBuf.unlock();
}

while문에서 각 메세지가 제대로 도착한 경우 다음으로 넘어간다. 멤버 변수로 지정되어있는 처리해야 할 point cloud를 clear한 후, 버퍼에서 가장 앞에 있는(가장 처음 들어온) point cloud를 pop으로 가져온다.

mBuf.lock();
cornerPointsSharp->clear();
pcl::fromROSMsg(*cornerSharpBuf.front(), *cornerPointsSharp);
cornerSharpBuf.pop();

이 과정을 모든 buffer에 대해 진행해준다.

Sharp point와 flat point의 크기를 local 변수에 저장한 후

int cornerPointsSharpNum = cornerPointsSharp->points.size();
int surfPointsFlatNum = surfPointsFlat->points.size();

Optimization을 두번정도 돌려주는 것 같다.

for (size_t opti_counter = 0; opti_counter < 2; ++opti_counter)
{
    ...
}

이 안에서 KDtree search 도 하는데, Iteration이 너무 돌아가지 않게 두번에 끝내는 것 같다.

이 for loop 안에서 ceres solver에 대한 내용이 등장한다… (두둥)

ceres::LossFunction *loss_function = new ceres::HuberLoss(0.1);
ceres::LocalParameterization *q_parameterization =
    new ceres::EigenQuaternionParameterization();
ceres::Problem::Options problem_options;

ceres::Problem problem(problem_options);
problem.AddParameterBlock(para_q, 4, q_parameterization);
problem.AddParameterBlock(para_t, 3);

Loss function은 Huber loss로 정의하고, 아마 rotation에 대한 parameterization으로 EigenQuaternionParameterization을 설정해준 것이라 추측된다. problem_option은 따로 설정하지 않고 넣고 (아마 optimization시 print해주는 설정 같은 것을 넣을 수 있는 option이 아닐까?), problem을 만든다.

Problem에 AddParameterBlock을 통해 Rotation에 대한 parameter를 설정해주고, quaternion이기에 앞서 정의한 q_parameterization 설정을 넣는다. 두번째 AddParameterBlock에는 위치를 넣고 위치의 경우 세 coordinate간 제약이 없기에 그냥 넣은 것 같다. 각 AddParameterBlock의 숫자 (4, 3)은 parameter의 크기를 뜻하는 것 같다.

여기서 para_q는 double[4] 배열로, {1, 0, 0, 0}이며, quaternion identity를 나타내고, para_t는 double[3] 배열로 {0, 0, 0}으로 translation의 identity를 나타낸다.

논문에서 나와있듯이 corner point와 flat point correspondence에 대해 cost function이 다르기에, 각각 for문을 돌리며 correspondence를 찾아 ceres_solver를 구축한다.

첫번째 for문: Corner점에 대한 correspondence 처리

for (int i = 0; i < cornerPointsSharpNum; ++i)
{
    TransformToStart(&(cornerPointsSharp->points[i]), &pointSel);
    kdtreeCornerLast->nearestKSearch(pointSel, 1, pointSearchInd, pointSearchSqDis);

    int closestPointInd = -1, minPointInd2 = -1;
    if (pointSearchSqDis[0] < DISTANCE_SQ_THRESHOLD)
    {
        closestPointInd = pointSearchInd[0];
        int closestPointScanID = int(laserCloudCornerLast->points[closestPointInd].intensity);

        double minPointSqDis2 = DISTANCE_SQ_THRESHOLD;
        // search in the direction of increasing scan line
        for (int j = closestPointInd + 1; j < (int)laserCloudCornerLast->points.size(); ++j)
        {
            // if in the same scan line, continue
            if (int(laserCloudCornerLast->points[j].intensity) <= closestPointScanID)
                continue;

            // if not in nearby scans, end the loop
            if (int(laserCloudCornerLast->points[j].intensity) > (closestPointScanID + NEARBY_SCAN))
                break;

            double pointSqDis = (laserCloudCornerLast->points[j].x - pointSel.x) *
                                    (laserCloudCornerLast->points[j].x - pointSel.x) +
                                (laserCloudCornerLast->points[j].y - pointSel.y) *
                                    (laserCloudCornerLast->points[j].y - pointSel.y) +
                                (laserCloudCornerLast->points[j].z - pointSel.z) *
                                    (laserCloudCornerLast->points[j].z - pointSel.z);

            if (pointSqDis < minPointSqDis2)
            {
                // find nearer point
                minPointSqDis2 = pointSqDis;
                minPointInd2 = j;
            }
        }

        // search in the direction of decreasing scan line
        for (int j = closestPointInd - 1; j >= 0; --j)
        {
            // if in the same scan line, continue
            if (int(laserCloudCornerLast->points[j].intensity) >= closestPointScanID)
                continue;

            // if not in nearby scans, end the loop
            if (int(laserCloudCornerLast->points[j].intensity) < (closestPointScanID - NEARBY_SCAN))
                break;

            double pointSqDis = (laserCloudCornerLast->points[j].x - pointSel.x) *
                                    (laserCloudCornerLast->points[j].x - pointSel.x) +
                                (laserCloudCornerLast->points[j].y - pointSel.y) *
                                    (laserCloudCornerLast->points[j].y - pointSel.y) +
                                (laserCloudCornerLast->points[j].z - pointSel.z) *
                                    (laserCloudCornerLast->points[j].z - pointSel.z);

            if (pointSqDis < minPointSqDis2)
            {
                // find nearer point
                minPointSqDis2 = pointSqDis;
                minPointInd2 = j;
            }
        }
    }
    if (minPointInd2 >= 0) // both closestPointInd and minPointInd2 is valid
    {
        Eigen::Vector3d curr_point(cornerPointsSharp->points[i].x,
                                    cornerPointsSharp->points[i].y,
                                    cornerPointsSharp->points[i].z);
        Eigen::Vector3d last_point_a(laserCloudCornerLast->points[closestPointInd].x,
                                        laserCloudCornerLast->points[closestPointInd].y,
                                        laserCloudCornerLast->points[closestPointInd].z);
        Eigen::Vector3d last_point_b(laserCloudCornerLast->points[minPointInd2].x,
                                        laserCloudCornerLast->points[minPointInd2].y,
                                        laserCloudCornerLast->points[minPointInd2].z);

        double s;
        if (DISTORTION)
            s = (cornerPointsSharp->points[i].intensity - int(cornerPointsSharp->points[i].intensity)) / SCAN_PERIOD;
        else
            s = 1.0;
        ceres::CostFunction *cost_function = LidarEdgeFactor::Create(curr_point, last_point_a, last_point_b, s);
        problem.AddResidualBlock(cost_function, loss_function, para_q, para_t);
        corner_correspondence++;
    }
}

indexing을 i로 놓고 모든 cornerPoint에 대해 진행을 한다.

이전 코너점의 좌표계 기준으로 undistortion 및 transformation을 진행, pointSel 변수에 넣는다.

TransformToStart(&(cornerPointsSharp->points[i]), &pointSel);

여기서 TransfromToStart가 있는데 코드는 다음과 같다.

void TransformToStart(PointType const *const pi, PointType *const po)
{
    //interpolation ratio
    double s;
    if (DISTORTION)
        s = (pi->intensity - int(pi->intensity)) / SCAN_PERIOD;
    else
        s = 1.0;
    //s = 1;
    Eigen::Quaterniond q_point_last = Eigen::Quaterniond::Identity().slerp(s, q_last_curr);
    Eigen::Vector3d t_point_last = s * t_last_curr;
    Eigen::Vector3d point(pi->x, pi->y, pi->z);
    Eigen::Vector3d un_point = q_point_last * point + t_point_last;

    po->x = un_point.x();
    po->y = un_point.y();
    po->z = un_point.z();
    po->intensity = pi->intensity;
}

DISTORTION 옵션이 있을 때 s = (pi->intensity - int(pi->intensity)) / SCAN_PERIOD 로 계산한다.

여기서 intensity는, 앞서 언금하진 않았지만. intensity = point.intensity = scanID + scanPeriod * relTime 로 scanRegistration node에서 계산된다. 이는 즉, 채널번호(scanID) + scanPeriod (0.1s) *relTime (현재/전체) 로 나타내지며, 정수 부분은 scanID가 나오고 소숫점은 scan 시작 후 지난 시간을 나타낸다.

결국 s는 (현재/전체)가 된다. (현재/전체)에 대한 정확한 정의는 한 스캔을 도는 시간(전체) 분의 현재 점이 찍힌 시간 이다.

여튼, slerp은 sperical linear interpolation을 뜻하며, 회전 변환에서의 smooth한 interpolation을 위해 만들어진거 같다. Quaternion이기에, Quationion의 identity {1, 0, 0, 0}과 q_last_curr의 s만큼의 interpolation을 나타내는 것 같다.

여튼, undistortion의 방식에 대해서 생각을 해보면, 여기서 나온 q_point_last와 t_point_last는 이전 step에서 구한 relative pose를 이용해 constant velocity, constant angular rate를 가정하고 이전 결과를 가지고 undistortion을 진행한 것으로 보인다.

이후 이전 좌표계를 기준으로 하여 현재 점들을 undistortion 및 translation한다. (그래서 DISTORTION이 아닐 때 s = 1이였구나 ㅎ), 근데 그런식으로 생각하면 s가 1보다 커야되는데, 그럼 (현재/전체)+1이 되는걸까? 아니면 코드가 잘못된걸까 ㅜ 여튼 진행

지난 프레임의 마지막 corner의 kdTree 구조를 이용해 undistortion이 된 현재 점들간의 correspondence를 구한다.

kdtreeCornerLast->nearestKSearch(pointSel, 1, pointSearchInd, pointSearchSqDis);

가장 가까운 순으로 pointSearchInd에 쌓이며, distance 값들도 pointSearchSqDis에 쌓인다. 여기서 1은 가장 가까운것만 찾는다는 뜻.

if (pointSearchSqDis[0] < DISTANCE_SQ_THRESHOLD)

여기서부터가 조금 이해가 안가긴 한다. 가장 가까운 distance가 미리 정해진 DISTANCE_SQ_THRESHOLD보다 작으면,

먼저 가장 가까운 point index에 대한 정보를 closestPointInd에 넣고

closestPointInd = pointSearchInd[0];

closestPointScanID (scan no)를 closestPointScanID 에 넣어준다. 여기서 ID는 이전 corner점들을 기준으로 id인듯

int closestPointScanID = int(laserCloudCornerLast->points[closestPointInd].intensity);

그리고 일단 minPointSqDis2를 DISTANCE_SQ_THRESHOLD로 지정해놓고

double minPointSqDis2 = DISTANCE_SQ_THRESHOLD;

closestPointScanID 부터 증가하는 방향으로 시작한다. 주석으로 “search in the direction of increasing scan line”로 적혀있는 것을 보면, 이전 sacnRegistration에서 스캔라인 순으로 publish한듯 (아니면 이 노드에서 저장을 그렇게 했던가)

여기서부터 과정은 다음과 같다. 먼저 이전 과정에서 전 corner점중 가장 가까운 점을 찾았으니, edge correspondence를 넣기 위해 두번째 가까운 점을 찾는 과정이다. (왜 knn search를 이용하지 않는지는 모르겠다. 계산과정에 차이가 그렇게 차이가 날까?)

여기서 같은 scan line일 경우 그냥 통과하고 (왜그럴까 생각해보니 찰나의 순간이라 같은 스캔라인이면 너무 거리가 짧아서 그런가?)

if (int(laserCloudCornerLast->points[j].intensity) <= closestPointScanID)
    continue;

또 근처 scan id가 아니면 (예로 지금은 3번째 채널인데 query가 막 10번째거나 그럴경우 하나로 이어지는 edge로 볼 수 없기에) loop break를 한다. (나라면 거리로 생각할 것 같다.. 거리에 따라서 resolution이 달라지기 때문에 차라리 scan id보단 euclidean distance가 낫지 않을까?)

if (int(laserCloudCornerLast->points[j].intensity) > (closestPointScanID + NEARBY_SCAN))
    break;

그리고, 거리 제곱을 계산해서, 이게 가장 작은것으로 선택한다. (가장 가까운 이웃을 선택!)

double pointSqDis = (laserCloudCornerLast->points[j].x - pointSel.x) *
                        (laserCloudCornerLast->points[j].x - pointSel.x) +
                    (laserCloudCornerLast->points[j].y - pointSel.y) *
                        (laserCloudCornerLast->points[j].y - pointSel.y) +
                    (laserCloudCornerLast->points[j].z - pointSel.z) *
                        (laserCloudCornerLast->points[j].z - pointSel.z);

if (pointSqDis < minPointSqDis2)
{
    // find nearer point
    minPointSqDis2 = pointSqDis;
    minPointInd2 = j;
}

같은 과정을 scan id 가 작아지는 부분에 대해서도 한다. 이후 minPointInd2가 valid한 경우 (그니까 위의 과정에서 각 조건들에 맞는 두번째 point를 찾은 경우), 3개의 Eigen Vector3d에 각각 현재의 corner점과 위 과정을 통해 찾은 이전 corner점 2개를 넣는다.

그리고, s를 계산해줘서 (현재/전체) ceres의 cost function을 만든 뒤 problem의 residual block에 넣어준다.

ceres solver를 잘 모르지만, 이런식으로 constraint를 쭉쭉 넣어줄 수 있나보다.

ceres::CostFunction *cost_function = LidarEdgeFactor::Create(curr_point, last_point_a, last_point_b, s);
problem.AddResidualBlock(cost_function, loss_function, para_q, para_t);

LidarEdgeFactor는 다음과 같다.

struct LidarEdgeFactor
{
	LidarEdgeFactor(Eigen::Vector3d curr_point_, Eigen::Vector3d last_point_a_,
					Eigen::Vector3d last_point_b_, double s_)
		: curr_point(curr_point_), last_point_a(last_point_a_), last_point_b(last_point_b_), s(s_) {}

	template <typename T>
	bool operator()(const T *q, const T *t, T *residual) const
	{

		Eigen::Matrix<T, 3, 1> cp{T(curr_point.x()), T(curr_point.y()), T(curr_point.z())};
		Eigen::Matrix<T, 3, 1> lpa{T(last_point_a.x()), T(last_point_a.y()), T(last_point_a.z())};
		Eigen::Matrix<T, 3, 1> lpb{T(last_point_b.x()), T(last_point_b.y()), T(last_point_b.z())};

		//Eigen::Quaternion<T> q_last_curr{q[3], T(s) * q[0], T(s) * q[1], T(s) * q[2]};
		Eigen::Quaternion<T> q_last_curr{q[3], q[0], q[1], q[2]};
		Eigen::Quaternion<T> q_identity{T(1), T(0), T(0), T(0)};
		q_last_curr = q_identity.slerp(T(s), q_last_curr);
		Eigen::Matrix<T, 3, 1> t_last_curr{T(s) * t[0], T(s) * t[1], T(s) * t[2]};

		Eigen::Matrix<T, 3, 1> lp;
		lp = q_last_curr * cp + t_last_curr;

		Eigen::Matrix<T, 3, 1> nu = (lp - lpa).cross(lp - lpb);
		Eigen::Matrix<T, 3, 1> de = lpa - lpb;

		residual[0] = nu.x() / de.norm();
		residual[1] = nu.y() / de.norm();
		residual[2] = nu.z() / de.norm();

		return true;
	}

	static ceres::CostFunction *Create(const Eigen::Vector3d curr_point_, const Eigen::Vector3d last_point_a_,
									   const Eigen::Vector3d last_point_b_, const double s_)
	{
		return (new ceres::AutoDiffCostFunction<
				LidarEdgeFactor, 3, 4, 3>(
			new LidarEdgeFactor(curr_point_, last_point_a_, last_point_b_, s_)));
	}

	Eigen::Vector3d curr_point, last_point_a, last_point_b;
	double s;
};

Edge point 3개 (curr_point_, last_point_a_, last_point_b_)와 시간 percent(? 이걸 뭐라 표현해야되나 진짜.. 여튼 현재/전체) s를 넣어주게 되며 Operator에서 quaternion q, translation t, 그리고 residual을 주소값으로 받는다.

3 by 1 vector:

  • nu = (lp - lpa).cross(lp - lpb), 그리고
  • de = lpa - lpb;

로 정의하며 residual은 nu의 각 element를 de의 norm의 각 element로 각각 나눠준것과 같다. Point to line distance

Create에서는 jacobian을 정의해주는 것 같은데, ceres에서 제공하는 AutoDiffCostFunction을 사용한듯, gtsam이랑 비슷하면서도 조금 다른 구석이 있긴 하다. Uncertainty를 따로 고려하지는 않는듯.

두번째 for문: Surface점에 대한 correspondence 처리

for (int i = 0; i < surfPointsFlatNum; ++i)
{
    TransformToStart(&(surfPointsFlat->points[i]), &pointSel);
    kdtreeSurfLast->nearestKSearch(pointSel, 1, pointSearchInd, pointSearchSqDis);

    int closestPointInd = -1, minPointInd2 = -1, minPointInd3 = -1;
    if (pointSearchSqDis[0] < DISTANCE_SQ_THRESHOLD)
    {
        closestPointInd = pointSearchInd[0];

        // get closest point's scan ID
        int closestPointScanID = int(laserCloudSurfLast->points[closestPointInd].intensity);
        double minPointSqDis2 = DISTANCE_SQ_THRESHOLD, minPointSqDis3 = DISTANCE_SQ_THRESHOLD;

        // search in the direction of increasing scan line
        for (int j = closestPointInd + 1; j < (int)laserCloudSurfLast->points.size(); ++j)
        {
            // if not in nearby scans, end the loop
            if (int(laserCloudSurfLast->points[j].intensity) > (closestPointScanID + NEARBY_SCAN))
                break;

            double pointSqDis = (laserCloudSurfLast->points[j].x - pointSel.x) *
                                    (laserCloudSurfLast->points[j].x - pointSel.x) +
                                (laserCloudSurfLast->points[j].y - pointSel.y) *
                                    (laserCloudSurfLast->points[j].y - pointSel.y) +
                                (laserCloudSurfLast->points[j].z - pointSel.z) *
                                    (laserCloudSurfLast->points[j].z - pointSel.z);

            // if in the same or lower scan line
            if (int(laserCloudSurfLast->points[j].intensity) <= closestPointScanID && pointSqDis < minPointSqDis2)
            {
                minPointSqDis2 = pointSqDis;
                minPointInd2 = j;
            }
            // if in the higher scan line
            else if (int(laserCloudSurfLast->points[j].intensity) > closestPointScanID && pointSqDis < minPointSqDis3)
            {
                minPointSqDis3 = pointSqDis;
                minPointInd3 = j;
            }
        }

        // search in the direction of decreasing scan line
        for (int j = closestPointInd - 1; j >= 0; --j)
        {
            // if not in nearby scans, end the loop
            if (int(laserCloudSurfLast->points[j].intensity) < (closestPointScanID - NEARBY_SCAN))
                break;

            double pointSqDis = (laserCloudSurfLast->points[j].x - pointSel.x) *
                                    (laserCloudSurfLast->points[j].x - pointSel.x) +
                                (laserCloudSurfLast->points[j].y - pointSel.y) *
                                    (laserCloudSurfLast->points[j].y - pointSel.y) +
                                (laserCloudSurfLast->points[j].z - pointSel.z) *
                                    (laserCloudSurfLast->points[j].z - pointSel.z);

            // if in the same or higher scan line
            if (int(laserCloudSurfLast->points[j].intensity) >= closestPointScanID && pointSqDis < minPointSqDis2)
            {
                minPointSqDis2 = pointSqDis;
                minPointInd2 = j;
            }
            else if (int(laserCloudSurfLast->points[j].intensity) < closestPointScanID && pointSqDis < minPointSqDis3)
            {
                // find nearer point
                minPointSqDis3 = pointSqDis;
                minPointInd3 = j;
            }
        }

        if (minPointInd2 >= 0 && minPointInd3 >= 0)
        {

            Eigen::Vector3d curr_point(surfPointsFlat->points[i].x,
                                        surfPointsFlat->points[i].y,
                                        surfPointsFlat->points[i].z);
            Eigen::Vector3d last_point_a(laserCloudSurfLast->points[closestPointInd].x,
                                            laserCloudSurfLast->points[closestPointInd].y,
                                            laserCloudSurfLast->points[closestPointInd].z);
            Eigen::Vector3d last_point_b(laserCloudSurfLast->points[minPointInd2].x,
                                            laserCloudSurfLast->points[minPointInd2].y,
                                            laserCloudSurfLast->points[minPointInd2].z);
            Eigen::Vector3d last_point_c(laserCloudSurfLast->points[minPointInd3].x,
                                            laserCloudSurfLast->points[minPointInd3].y,
                                            laserCloudSurfLast->points[minPointInd3].z);

            double s;
            if (DISTORTION)
                s = (surfPointsFlat->points[i].intensity - int(surfPointsFlat->points[i].intensity)) / SCAN_PERIOD;
            else
                s = 1.0;
            ceres::CostFunction *cost_function = LidarPlaneFactor::Create(curr_point, last_point_a, last_point_b, last_point_c, s);
            problem.AddResidualBlock(cost_function, loss_function, para_q, para_t);
            plane_correspondence++;
        }
    }
}

Surface correspondence를 찾는 것은 corner correspondence를 찾는 것과 크게 다른점은 없고, 3점을 뽑는데 차이가 좀 있다. 첫번째 차이점은 같거나 작은, 또는 같거나 큰 scanID를 배제하지는 않아 아래 코드가 surface correspondence에는 없다.

if (int(laserCloudCornerLast->points[j].intensity) <= closestPointScanID)
    continue;

대신 두 점을 다른 scanID에서 찾는데, 가장 가까운 점의 scan line의 다음, 이전 scanID에서 점을 찾는다.

if (int(laserCloudSurfLast->points[j].intensity) <= closestPointScanID && pointSqDis < minPointSqDis2)
{
    minPointSqDis2 = pointSqDis;
    minPointInd2 = j;
}
// if in the higher scan line
else if (int(laserCloudSurfLast->points[j].intensity) > closestPointScanID && pointSqDis < minPointSqDis3)
{
    minPointSqDis3 = pointSqDis;
    minPointInd3 = j;
}

Point to plane distance를 구하는데 있어 plane model이 잘못될 수 있기에 뭔가 세점이 어떤 형태로 놓여져있을지를 고려할 것 같았는데, 딱히 그런 내용은 없고, 세점을 뽑은 후 바로 LidarPlaneFactor로 넣는다.

ceres::CostFunction *cost_function = LidarPlaneFactor::Create(curr_point, last_point_a, last_point_b, last_point_c, s);
problem.AddResidualBlock(cost_function, loss_function, para_q, para_t);

LidarPlaneFactor는 다음과 같다.


struct LidarPlaneFactor
{
	LidarPlaneFactor(Eigen::Vector3d curr_point_, Eigen::Vector3d last_point_j_,
					 Eigen::Vector3d last_point_l_, Eigen::Vector3d last_point_m_, double s_)
		: curr_point(curr_point_), last_point_j(last_point_j_), last_point_l(last_point_l_),
		  last_point_m(last_point_m_), s(s_)
	{
		ljm_norm = (last_point_j - last_point_l).cross(last_point_j - last_point_m);
		ljm_norm.normalize();
	}

	template <typename T>
	bool operator()(const T *q, const T *t, T *residual) const
	{

		Eigen::Matrix<T, 3, 1> cp{T(curr_point.x()), T(curr_point.y()), T(curr_point.z())};
		Eigen::Matrix<T, 3, 1> lpj{T(last_point_j.x()), T(last_point_j.y()), T(last_point_j.z())};
		//Eigen::Matrix<T, 3, 1> lpl{T(last_point_l.x()), T(last_point_l.y()), T(last_point_l.z())};
		//Eigen::Matrix<T, 3, 1> lpm{T(last_point_m.x()), T(last_point_m.y()), T(last_point_m.z())};
		Eigen::Matrix<T, 3, 1> ljm{T(ljm_norm.x()), T(ljm_norm.y()), T(ljm_norm.z())};

		//Eigen::Quaternion<T> q_last_curr{q[3], T(s) * q[0], T(s) * q[1], T(s) * q[2]};
		Eigen::Quaternion<T> q_last_curr{q[3], q[0], q[1], q[2]};
		Eigen::Quaternion<T> q_identity{T(1), T(0), T(0), T(0)};
		q_last_curr = q_identity.slerp(T(s), q_last_curr);
		Eigen::Matrix<T, 3, 1> t_last_curr{T(s) * t[0], T(s) * t[1], T(s) * t[2]};

		Eigen::Matrix<T, 3, 1> lp;
		lp = q_last_curr * cp + t_last_curr;

		residual[0] = (lp - lpj).dot(ljm);

		return true;
	}

	static ceres::CostFunction *Create(const Eigen::Vector3d curr_point_, const Eigen::Vector3d last_point_j_,
									   const Eigen::Vector3d last_point_l_, const Eigen::Vector3d last_point_m_,
									   const double s_)
	{
		return (new ceres::AutoDiffCostFunction<
				LidarPlaneFactor, 1, 4, 3>(
			new LidarPlaneFactor(curr_point_, last_point_j_, last_point_l_, last_point_m_, s_)));
	}

	Eigen::Vector3d curr_point, last_point_j, last_point_l, last_point_m;
	Eigen::Vector3d ljm_norm;
	double s;
};

먼저 struct를 생성할 때 이전 surface point 3개를 이용해 normal direction을 계산한다.

LidarPlaneFactor(Eigen::Vector3d curr_point_, Eigen::Vector3d last_point_j_,
                    Eigen::Vector3d last_point_l_, Eigen::Vector3d last_point_m_, double s_)
    : curr_point(curr_point_), last_point_j(last_point_j_), last_point_l(last_point_l_),
        last_point_m(last_point_m_), s(s_)
{
    ljm_norm = (last_point_j - last_point_l).cross(last_point_j - last_point_m);
    ljm_norm.normalize();
}

Operator에서는 residual을 계산하고, 마찬가지로 Creat에서는 AutoDiffCostFunction을 이용한다.

여기서 AutoDiffCostFunction<function, no1, no2, no3> 이런식으로 corner에 대해서도 같은 문법이 이용된다. Corner에 대해서는:

  • return (new ceres::AutoDiffCostFunction<LidarEdgeFactor, 3, 4, 3>(new LidarEdgeFactor(curr_point_, last_point_a_, last_point_b_, s_))); Surface에 대해서는:
  • return (new ceres::AutoDiffCostFunction<LidarPlaneFactor, 1, 4, 3>(new LidarPlaneFactor(curr_point_, last_point_j_, last_point_l_, last_point_m_, s_)));

를 사용하는데, 숫자의 의미가 뭘까?

  • no1: Corner의 residual의 벡터 크기는 3이고, Surface의 residual의 벡터 크기는 1이니, no1은 residual의 벡터 크기를 나타내며
  • no2, no3: 각 parameter block의 크기를 나타낸다.

AddResidualBlock에서 추가된 parameter block은 para_q, para_t이기에, quaternion에 해당되는 4와 translation에 해당되는 3이 추가된 것 같다.

problem.AddResidualBlock(cost_function, loss_function, para_q, para_t);

그리고 optimization을 진행해준다.

if ((corner_correspondence + plane_correspondence) < 10)
{
    printf("less correspondence! *************************************************\n");
}

TicToc t_solver;
ceres::Solver::Options options;
options.linear_solver_type = ceres::DENSE_QR;
options.max_num_iterations = 4;
options.minimizer_progress_to_stdout = false;
ceres::Solver::Summary summary;
ceres::Solve(options, &problem, &summary);
printf("solver time %f ms \n", t_solver.toc());

Corner와 plane correspondence 수가 10보다 작으면 진행하지 않는다.

Solver는 Linear solver를 사용하며 Maximum iteration은 4로 제한한다.

이전에 para_q와 para_t가 왜 update가 안되나 싶었는데, optimization을 돌리면서 값이 변하는 것 같다.

q_last_curr, t_last_curr도 update하는 함수가 없길래 뭔가 했는데 아예 정의를 Eigen::Map으로 되어있다.

Eigen::Map<Eigen::Quaterniond> q_last_curr(para_q);
Eigen::Map<Eigen::Vector3d> t_last_curr(para_t);

솔까 잘 모르겠지만, 아마 para_q가 update되면 자동으로 q_last_curr도 업데이트 되는 모양이다.

그리고 10Hz로 Odometery를 publish하고

nav_msgs::Odometry laserOdometry;
laserOdometry.header.frame_id = "/camera_init";
laserOdometry.child_frame_id = "/laser_odom";
laserOdometry.header.stamp = ros::Time().fromSec(timeSurfPointsLessFlat);
laserOdometry.pose.pose.orientation.x = q_w_curr.x();
laserOdometry.pose.pose.orientation.y = q_w_curr.y();
laserOdometry.pose.pose.orientation.z = q_w_curr.z();
laserOdometry.pose.pose.orientation.w = q_w_curr.w();
laserOdometry.pose.pose.position.x = t_w_curr.x();
laserOdometry.pose.pose.position.y = t_w_curr.y();
laserOdometry.pose.pose.position.z = t_w_curr.z();
pubLaserOdometry.publish(laserOdometry);

geometry_msgs::PoseStamped laserPose;
laserPose.header = laserOdometry.header;
laserPose.pose = laserOdometry.pose.pose;
laserPath.header.stamp = laserOdometry.header.stamp;
laserPath.poses.push_back(laserPose);
laserPath.header.frame_id = "/camera_init";
pubLaserPath.publish(laserPath);

1Hz로 point cloud를 publish한다.

if (frameCount % skipFrameNum == 0)
{
    frameCount = 0;

    sensor_msgs::PointCloud2 laserCloudCornerLast2;
    pcl::toROSMsg(*laserCloudCornerLast, laserCloudCornerLast2);
    laserCloudCornerLast2.header.stamp = ros::Time().fromSec(timeSurfPointsLessFlat);
    laserCloudCornerLast2.header.frame_id = "/camera";
    pubLaserCloudCornerLast.publish(laserCloudCornerLast2);

    sensor_msgs::PointCloud2 laserCloudSurfLast2;
    pcl::toROSMsg(*laserCloudSurfLast, laserCloudSurfLast2);
    laserCloudSurfLast2.header.stamp = ros::Time().fromSec(timeSurfPointsLessFlat);
    laserCloudSurfLast2.header.frame_id = "/camera";
    pubLaserCloudSurfLast.publish(laserCloudSurfLast2);

    sensor_msgs::PointCloud2 laserCloudFullRes3;
    pcl::toROSMsg(*laserCloudFullRes, laserCloudFullRes3);
    laserCloudFullRes3.header.stamp = ros::Time().fromSec(timeSurfPointsLessFlat);
    laserCloudFullRes3.header.frame_id = "/camera";
    pubLaserCloudFullRes.publish(laserCloudFullRes3);
}

Laser Mapping


© 2020. All rights reserved.

Powered by Hydejack v9.0.5