Publishing extra information using pcl Pointcloud and ros Pointcloud2

Publishing extra information in ros::Pointcloud2 message

Pohang Dataset으로 LIO-SAM을 돌리려다보니 안되길래 들여다보니 LIO-SAM은 ring value를 이용해 이미지로 만들어 이를 통해 각 feature를 뽑아 내는 것을 발견하게 되었다.

처음에는 그냥 대충 이미지로 projection 해보고 저장된 point sequence가 어떤식으로 저장되나 보면서 해보려고 했지만, 2048 * 32 혹은 * 64 의 점들을 파악하기가 여간 쉬운 일이 아닐꺼라 예상되어 실험 마지막날 ring value를 포함한 logging data를 만들었다.

ros::Pointcloud2 message를 읽어 여기에 해당되는 각 값들을 저장하는 방법(이거에 대해서는 따로 써야겠다)에서 읽혀지는 ring값을 보니 처음에는 scan 순서대로 vertical하게 먼저 저장될 줄 알았으나 (예로 32체널의 경우 0,1,…,31,0,1,…,31,…) 예상과는 다르게 horizontal 방향으로 먼저 저장되는 모습을 볼 수 있었다 (0,0,…,0,1,1,..1,…31,31,…31). 아마 한 스캔을 다 만들어놓은 후 처리하는 것이라 읽는건 ring별로 먼저 읽는 모양인다.

기존에 만들어둔 ros message player에서는 그냥 point cloud의 x, y, z, intensity만 사용하여 ros message를 내보내기 위해 googling을 통해 그냥 pcl::PointCloud<pcl::PointXYZI> 형식으로 해서

pcl::PointXYZI 객체 형성하고 push_back 함으로서 점들을 넣어주고

sensor_msgs::PointCloud2 에 pcl::toROSMsg(pcl객체, PointCloud2객체)로

메세지를 생성했다.

그럼 이 ring값을 어떻게 넣는가..

일단 binary에서 읽는 순서는 binary에 쓰는 순서랑 다른 듯 보였다. (이부분 좀 이해가 안됨. 다시 검증해야 할듯)

Point 순서에 따라 채널수로 나눈 몫을 ring값으로 해주면 되는데 (point_no % channel_no) 여기서 pcl::PointXYZI는 ring값을 지원하지 않는다.

(열심히 구글링 하다 보니 예전에 있었는데 왜 없앴냐는 말 비슷하게 본거 같다.)

그래서 어쩌지 어쩌지 하다가 LIO-SAM에서 아예 struct를 새로 만들어서 ‘읽어’오는 것을 보게 되었다.

뭐 어쨌든 다음과 같은 struct를 생성했다.

struct PointXYZIR {
    PCL_ADD_POINT4D;
    float intensity;
    uint8_t ring;
    uint32_t t;
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
} EIGEN_ALIGN16;
POINT_CLOUD_REGISTER_POINT_STRUCT(PointXYZIR,
    (float, x, x) (float, y, y) (float, z, z) (float, intensity, intensity)
    (uint8_t, ring, ring) (uint32_t, t, t)
)

여기서 t는 time으로 cloud deskew때 필요하다 해서 추가적으로 넣게 되었다.

여튼 이렇게 만든 struct로 다시

pcl::PointCloud<PointXYZIR>

에 각 점들에 대한

PointXYZIR

struct을 push back하여 이를 다시

pcl::toROSMsg(pcl::PointCloud<PointXYZIR>, ros::Pointcloud2)

형식으로 넣어주면 된다.

여기서.. 뭔가 모르겠지만, 이 struct이 그대로 전달돼 함부로 변수명을 바꾸면 (예를 들어 ring을 r로 표기하거나) 안될 것 같다.


© 2020. All rights reserved.

Powered by Hydejack v9.0.5