Pointcloul Channel을 이용하여서 clustering에 활용

KD TREE 나 clustering을 한 이후에 우리는 Rviz나 PCL VIEWER에 Visualzation이나, class id를 포인터에 저장하고 싶다.

std::vector를 이용해서 point idx별로 class id를 구분 할 수 도 있지만, 더 효율적으로 더 똑똑하게 하고자 우리는 sensor_msgs/Pointcloud의 데이터 타입을 이용하여서 할 수 있다.

이때 Pointcloud1경우 channel이라는 data에 대한 이름을 정하고, 거기다 class id도 줄 수 있따. 이를 이용하여서 clustering에 대한 정보를 저장하여서 이후 topic으로 쏠때 받는 사람이 clustering에 대한 id도 같이 받을 수 있다.

아래와 같은 참조를 통해서 구현하였다.

http://www.360doc.com/content/17/0505/17/41168298_651355475.shtml

https://zhuanlan.zhihu.com/p/145939299

https://blog.csdn.net/lovelyaiq/article/details/118530388

https://blog.csdn.net/xhtchina/article/details/119707553

http://t.zoukankan.com/conscience-remain-p-14288723.html

https://zhuanlan.zhihu.com/p/145939299

2-D Object Tracking using Kalman Filter(single sensor)_GPS원리

설명 잘 되어 있음.

https://machinelearningspace.com/2d-object-tracking-using-kalman-filter/

Environment에 대한 Motion Model만 잘 정의한다면 센서 하나를 가지고도 Object tracking을 할 수 있다.

여기서는 초기 motion에 대한 모델 정의화 초기값이 추어지고, image에서 취득되는 측정값(x,y)값을 반영하여서 update를 통한 모델 속도를 추정함과 동시에 자세를 추정을 한다.

Lidar SLAM Utilizing Normal Distribution Transform and Measurement Consesus

GPS-IMU Sensor Fusion 원리 및 2D mobile robot sensor fusion Implementation(Kalman Filter and Extended Kalman filter)

우리가 차를 타다보면 핸드폰으로부터 GPS정보가 UTM-K좌표로 변환이 되어서 지도상의 우리의 위치를 알려주고, 속도도 알려주는데 이는 무슨 방법을 쓴걸까?

바로 Kalman Filter를 이용하여서 우리의 위치와 속도값을 추정을 해주는 것이다.

GPS에서 제공하는 데이터 값은 x,y,z값

IMU에서 제공하는 데이터 값은 x,y,z, vx,vy,angular velocity

GPS의 데이터는 절대좌표에서 쏴주는 데이터 값이고, IMU에서 주는 데이터는 상대좌표에서 쏴주는 데이터 값이다.

상대좌표에서 데이터 값은 Drift나 센서 노이즈로 인하여 시간이 갈수록 불안전해 진다.(error Propagation)

절재좌표도 또한 가림이나 환경적 요인으로 인해서 데이터가 불안정해진다.(error Propagation)

센서에 대한 이런 문제를 보안하기 위해서 주로 Kalman Filter를 대중적으로 사용하게 된다.

Kalman Filter는 Prediction Model과 Update Model이 있는데,

주로 Prediction Model은 상대좌표(IMU)에서 얻어지는 데이터를 가지고 상태를 예측한다.(Predicted means, Predicted Covariance Matrix)

Predict 데이터를 통해, 절대좌표에서 측정된 데이턱 값을 업데이트 해준다.(Kalman Gain, Covariance Propagation with Kalman Gain, Measurement Model)

아래와 같다.

예를 들어 2D Mobile robot의 Localization을 추정한다고 한다면,

여기서 C는 error Matrix이다.

선형적인 환경에서는 이 칼만필터가 잘 사용되지만 사실 현실세계는 비선형(앞으로만 가지 않고 여러 방향으로 가기도 하기 때문에)이기 때문에 비선형 모델을 사용하여서 많은 문제를 풀기도 한다.

Kalman Filter도 마찬가지로 비선형 방식인 Extended Kalman Filter가 있는데 간단히 Motion Model과 Measurement Model에 Talor Series expension를 통하여 Jacobian Matrix로 상태추정을 하는 것입니다.

J는 때에 따라 XYZ에 대한 자코비안만 하는 경우가 많다.(즉 Transition Matrix A(x_k-1, y_k-1,vx_k-1,vy_k-1,theta_k-1)만 고려, Control Matrix에 대해 Jacobian 하지 않음)

Code Reference

https://github.com/search?p=1&q=imu+gps&type=Repositories

https://limitsinx.tistory.com/78

KD-TREE and K-Means clustering Implementation

KD-Tree

바이너리 형태로 트리를 작성을 하며 Search에 대한 time Complexity(klog2)로 가져가게 된다.

다차원 공간에서 Search를 하는데 많이 사용되며, 이때 Dependancy로 Distance Threshold와 Tree maximum leaf

들이 있다.

Depth level에 의해서 Binaray Tree가 형성이 되는데, 즉 2차원 x,y를 생각 할 때 Root는 Depth 0으로 x데이터에 대해서 Parent leaf가 형성이 되고, 다음 데이터가 들어왔을떄 Depth 1 Level로 y에 대한 데이터를 Parent y값과 비교해서 tree를 형성을 하면서 구간 구간을 만들게 된다.

실제 포인트클라우드에서 주로 Segmentation 이후에 나눠어진 Ground와 Unknown Classification에 대한 데이터가 구분이 되어 있을 때, Unknown에 대한 데이터를 kd-tree로 Tree를 만든 다음에 point를 하나 하나 방문하면서 주변 neihbor 포인트가 특정 Distance Threshold안에 들어 오게 되면 같은 Class로 define하여서 clustering을 하게 된다.(정의가 된 포인트에 대해서는 neihbor를 설치할때 마크가 되어있어서 스킵하게 된다.)

그러나 KD트리도 단점이 있는데, 만약 차원의 수가 계속 증가하게 되면, 서치에 대한 정확도가 떨어질 수도 있다.

K-Means clustering

Machine Learning 쪽에서 많이 쓰인다. 특히 Bag of Word나 Object Detection Detector에서 주어진 데이터셋과 Label 데이터를 통해서 Learning을 통해 Classification Model을 만들거나(SVM, Bayes Naive 등).

주어진 unlabel된 데이터를 k라는 수 만큼 clustering을 하는데 사용이 된다.

엄청 쉽다.

무수한 포인트들이 있다면, 그 중에 k 수만큼 랜덤으로 포인트를 잡는다(초기 k-means를 잡는다). 이후 포인트들 하나씩 k수 만큼 선정된 클러스터링 아이디를 중심으로 방문하면서 포인트들이 k 포인트들의 특정 distance Threshold에 들게 되면 해당하는 포인트들은 특정 k포인트로 그룹이 되고 그룹의 means 값을 만든다. 그룹에 대한 means값을 가지고 다시한번 포인트들을 k-menas값들에 방문을 하면서 특정 threshold에 포함이 되면 다시 groupping을 하고 다시 means값을 만드는 작업을 특정 수만큼 iteration을 하여서 k-means를 이용한 클러스터링을 하게 된다.

문제는 Dependancy가 K값이기 때문에 clustering할 객체에 수가 항상 랜덤할 경우 좋지 못하다.

또한 timecomplexity가 O(n^2)이므로 데이터셋이 커지면 커질수록 비효율적이다.

Code Reference

https://github.com/Isacarthuso/KDtree

https://zhuanlan.zhihu.com/p/64180937

https://github.com/jacquesmats/Lidar_Obstacle_Detection