LiDAR를 SLAM에 사용할 때 imu 값을 이용한 motion compensation을 전처리단에서 수행하는 경우가 많다.
왜냐하면 주행 차량에서 취득하는 경우와 같이 빠른 속도로 움직이는 경우 라이다 앵글컷 시작점과 종료점이 꽤 큰 차이를 가질 수 있기 때문이다.
이를 보상하지 않는 것은 라이다가 한바퀴 스캔하는 동안 정지해 있다고 가정하는 것과 같음.
근데 imu가 없다면 어떻게 해야할까?
현재 프레임의 포즈를 보상 없이 추정한 후, 맵을 쌓을 때 이전 포즈와 현재포즈의 차이를 이용해 보상한 프레임을 사용해보기로 했다.
Pandar 128 채널 라이다를 사용해 실험했다.
프레임 하나를 띄워보았다. 민트 화살표가 차량 주행 방향이다.
이 데이터는 후방이 +y, 왼쪽이 +x, 위가 +z인 좌표계를 사용함을 확인했다.
판다는 앵글 스캔이 +y축을 기준으로 시계방향으로 이루어진다. 즉 +y축이 앵글컷 지점이다.
앵글컷 시작점에서부터 종료점까지 point의 angle을 이용해 포즈 변화량에 곱해줄 상수를 구할 것이다.
pcl::PointCloud<pcl::PointXYZI>::Ptr original_frame(new pcl::PointCloud<pcl::PointXYZI>());
for (size_t i = 0; i < scan.points.size(); i++)
{
int ang = constrainAngle(RAD2DEG(atan2(scan.points[i].y, scan.points[i].x)));
ang = (90 - ang) % 360;
if (ang < 0)
ang += 360;
pcl::PointXYZI pt;
pt_.x = scan.points[i].x;
pt_.y = scan.points[i].y;
pt_.z = scan.points[i].z;
pt_.intensity = static_cast<float>(ang)/360*255;
original_frame->points.push_back(pt_);
}
original_frame->width = original_frame->size();
original_frame->height = 1;
pcl::io::savePCDFile<pcl::PointXYZI>("/home/yeonsoo/original_frame.pcd", *original_frame);
위 코드를 사용하여 확인해보니 +y축을 앵글컷으로 하여 상수가 잘 들어갔다. ang/360이 이전 프레임과 현재 프레임 사이의 포즈 변화량을 반영하는 상수가 된다.
(위 그림에서 파란색=0, 빨간색=255)
정면으로 3m 이동했을 때 상황을 구현해보자.
헤딩은 아이덴티티, (0,1,0)에서 (0,-2,0)로 움직였다.
Eigen::Matrix4f cur_pose, prev_pose;
prev_pose << 1, 0, 0, 0,
0, 1, 0, 1,
0, 0, 1, 0,
0, 0, 0, 1;
cur_pose << 1, 0, 0, 0,
0, 1, 0, -2,
0, 0, 1, 0,
0, 0, 0, 1;
Eigen::Vector3f del_pose_t; //translation 변화량
del_pose_t << cur_pose(0,3) - prev_pose(0,3),
cur_pose(1,3) - prev_pose(1,3),
cur_pose(2,3) - prev_pose(2,3);
del_pose_t /= 360.;
for (size_t i = 0; i < scan.points.size(); i++)
{
Eigen::Matrix<float, 4, 1> ori_pt, cur_pt, cor_pt, next_pt;
int ang = constrainAngle(RAD2DEG(atan2(scan.points[i].y, scan.points[i].x)));
ang = (90 - ang) % 360;
if (ang < 0)
ang += 360;
ori_pt << scan.points[i].x, scan.points[i].y, scan.points[i].z, 1;
Eigen::Matrix4f part_pose;
part_pose = prev_pose;
part_pose(0, 3) += del_pose_t(0, 3) * ang;
part_pose(1, 3) += del_pose_t(1, 3) * ang;
part_pose(2, 3) += del_pose_t(2, 3) * ang;
Eigen::Matrix4f prev_pose_inv;
prev_pose_inv.block(0,0,3,3) = prev_pose.block(0,0,3,3).transpose();
prev_pose_inv.col(3).topRows(3) = -prev_pose_inv.block(0,0,3,3)*prev_pose.col(3).topRows(3);
cor_pt = prev_pose_inv * part_pose * ori_pt;
cor_pt = part_pose * ori_pt;
pt_compen.x = cor_pt(0,3);
pt_compen.y = cor_pt(1,3);
pt_compen.z = cor_pt(2,3);
pt_compen.intensity = static_cast<float>(ang)/360*255;
compen_frame->points.push_back(pt_compen);
}
original_frame->width = original_frame->size();
original_frame->height = 1;
pcl::io::savePCDFile<pcl::PointXYZI>("/home/yeonsoo/original_frame.pcd", *original_frame);
compen_frame->width = compen_frame->size();
compen_frame->height = 1;
pcl::io::savePCDFile<pcl::PointXYZI>("/home/yeonsoo/compen_frame.pcd", *compen_frame);
짠.. 파란 점들은 prev_pose가 적용되었고 빨간 점들은 cur_pose가 적용되었다. 그 사이는 등속으로 보간되었다.
scan: 라이다 프레임
part_pose: 월드 프레임 (월드 프레임 상에서 각이 ang_i일때의 시간 t_i에서의 prev_pose->cur_pose 보간 위치)
compen_frame: 백 투 라이다 프레임
x, y를 한번에 움직여보자.
Eigen::Matrix4f cur_pose, prev_pose;
prev_pose << 1, 0, 0, 2,
0, 1, 0, 4,
0, 0, 1, 0,
0, 0, 0, 1;
cur_pose << 1, 0, 0, 7,
0, 1, 0, -2,
0, 0, 1, 0,
0, 0, 0, 1;이번에는 prev_pose에서의 스캔 점군과 cur_pose에서의 스캔 점군, 그 사이 보상이 적용된 스캔 점군을 모두 월드 프레임 상에서 띄워보았다.
흰색: prev_pose
초록색: cur_pose
앵글컷 시작점인 파란점은 prev_pose와 잘 물려있고 빨간점은 cur_pose와 잘 물려있다.
rotation을 적용해보자.
yaw로 60도를 돌려본다.
좌단: prev_pose
우단: yaw로 60도 돌린 cur_pose
rotation은 quaternion 보간을 사용한다.
참고로 회전변환 보간의 경우 다양한 방법이 존재한다. 회전행렬에서 회전축과 각을 추출해 각만 1/n때려도 되고.. 쿼터니언 보간시에도 선형보간이나 2차보간 등 다 가능하지만 가장 많이 쓰이는 것은 slerp이다. Eigen 라이브러리에서도 지원한다.
아무튼 나는 항상 쿼터니언을 무조건적으로 선호하기때문에 쿼터니언 slerp 보간을 하도록 한다.
회전행렬 보간에 대한 이론을 더 알아보고 싶은 사람은 아래 링크의 ppt를 추천함.
https://mycourses.aalto.fi/pluginfile.php/1120229/mod_resource/content/1/06_quaternions.pdf
Eigen::Matrix4f cur_pose, prev_pose;
prev_pose << 1, 0, 0, 10,
0, 1, 0, 10,
0, 0, 1, 0,
0, 0, 0, 1;
cur_pose << 1, 0, 0, 10,
0, 1, 0, 10,
0, 0, 1, 0,
0, 0, 0, 1;
cur_pose.block(0, 0, 3, 3) = map_builder_.eul2RotMatF(0, 0, -60*3.14159/180);
Eigen::Matrix3f prev_R = prev_pose.block(0, 0, 3, 3);
Eigen::Matrix3f cur_R = cur_pose.block(0, 0, 3, 3);
Eigen::Quaternionf v0, v1, vt;
v0 = prev_R;
v1 = cur_R;
pcl::PointXYZI pt;
pcl::PointCloud<pcl::PointXYZI>::Ptr original_frame(new pcl::PointCloud<pcl::PointXYZI>());
pcl::PointCloud<pcl::PointXYZI>::Ptr compen_frame(new pcl::PointCloud<pcl::PointXYZI>());
pcl::PointCloud<pcl::PointXYZI>::Ptr next_frame(new pcl::PointCloud<pcl::PointXYZI>());
for (size_t i = 0; i < scan.points.size(); i++)
{
Eigen::Matrix<float, 4, 1> ori_pt, cur_pt, cor_pt, next_pt;
int ang = map_builder_.constrainAngle(RAD2DEG(atan2(scan.points[i].y, scan.points[i].x)));
ang = (90 - ang) % 360;
if (ang < 0)
ang += 360;
ori_pt << scan.points[i].x, scan.points[i].y, scan.points[i].z, 1;
cur_pt = prev_pose * ori_pt;
pcl::PointXYZI pt_, pt_compen, pt_next;
pt_.x = cur_pt(0,3);
pt_.y = cur_pt(1,3);
pt_.z = cur_pt(2,3);
pt_.intensity = static_cast<float>(ang)/360*255;
original_frame->points.push_back(pt_);
Eigen::Matrix4f part_pose;
part_pose = prev_pose;
vt = v0.slerp(static_cast<float>(ang)/360., v1);
part_pose.block(0,0,3,3) = vt.toRotationMatrix();
Eigen::Matrix4f prev_pose_inv;
prev_pose_inv.block(0,0,3,3) = prev_pose.block(0,0,3,3).transpose();
prev_pose_inv.col(3).topRows(3) = -prev_pose_inv.block(0,0,3,3)*prev_pose.col(3).topRows(3);
cor_pt = prev_pose_inv * part_pose * ori_pt;
cor_pt = part_pose * ori_pt;
pt_compen.x = cor_pt(0,3);
pt_compen.y = cor_pt(1,3);
pt_compen.z = cor_pt(2,3);
pt_compen.intensity = static_cast<float>(ang)/360*255;
compen_frame->points.push_back(pt_compen);
next_pt = cur_pose * ori_pt;
pt_next.x = next_pt(0,3);
pt_next.y = next_pt(1,3);
pt_next.z = next_pt(2,3);
pt_next.intensity = static_cast<float>(ang)/360*255;
next_frame->points.push_back(pt_next);
}
original_frame->width = original_frame->size();
original_frame->height = 1;
pcl::io::savePCDFile<pcl::PointXYZI>("/home/yeonsoo/original_frame.pcd", *original_frame);
compen_frame->width = compen_frame->size();
compen_frame->height = 1;
pcl::io::savePCDFile<pcl::PointXYZI>("/home/yeonsoo/compen_frame.pcd", *compen_frame);
next_frame->width = next_frame->size();
next_frame->height = 1;
pcl::io::savePCDFile<pcl::PointXYZI>("/home/yeonsoo/next_frame.pcd", *next_frame);
평행이동과 회전을 같이 적용해보자.
Eigen::Matrix4f cur_pose, prev_pose;
prev_pose << 1, 0, 0, 10,
0, 1, 0, 10,
0, 0, 1, 0,
0, 0, 0, 1;
cur_pose << 1, 0, 0, 14,
0, 1, 0, 2,
0, 0, 1, 0,
0, 0, 0, 1;
prev_pose.block(0, 0, 3, 3) = eul2RotMatF(0, 0, -30*3.14159/180);
cur_pose.block(0, 0, 3, 3) = eul2RotMatF(0, 0, -60*3.14159/180);
Eigen::Vector3f del_pose_t;
del_pose_t << cur_pose(0,3) - prev_pose(0,3),
cur_pose(1,3) - prev_pose(1,3),
cur_pose(2,3) - prev_pose(2,3);
del_pose_t /= 360.;
Eigen::Matrix3f prev_R = prev_pose.block(0, 0, 3, 3);
Eigen::Matrix3f cur_R = cur_pose.block(0, 0, 3, 3);
Eigen::Quaternionf v0, v1, vt;
v0 = prev_R;
v1 = cur_R;
pcl::PointCloud<pcl::PointXYZI> scan;
pcl::fromROSMsg(*point_cloud_msg, scan);
pcl::PointXYZI pt;
pcl::PointCloud<pcl::PointXYZI>::Ptr original_frame(new pcl::PointCloud<pcl::PointXYZI>());
pcl::PointCloud<pcl::PointXYZI>::Ptr compen_frame(new pcl::PointCloud<pcl::PointXYZI>());
pcl::PointCloud<pcl::PointXYZI>::Ptr next_frame(new pcl::PointCloud<pcl::PointXYZI>());
for (size_t i = 0; i < scan.points.size(); i++)
{
Eigen::Matrix<float, 4, 1> ori_pt, cur_pt, cor_pt, next_pt;
int ang = constrainAngle(RAD2DEG(atan2(scan.points[i].y, scan.points[i].x)));
ang = (90 - ang) % 360;
if (ang < 0)
ang += 360;
ori_pt << scan.points[i].x, scan.points[i].y, scan.points[i].z, 1;
cur_pt = prev_pose * ori_pt;
pcl::PointXYZI pt_, pt_compen, pt_next;
pt_.x = cur_pt(0,3);
pt_.y = cur_pt(1,3);
pt_.z = cur_pt(2,3);
pt_.intensity = static_cast<float>(ang)/360*255;
original_frame->points.push_back(pt_);
Eigen::Matrix4f part_pose;
part_pose = prev_pose;
part_pose(0, 3) += del_pose_t(0, 3) * ang;
part_pose(1, 3) += del_pose_t(1, 3) * ang;
part_pose(2, 3) += del_pose_t(2, 3) * ang;
vt = v0.slerp(static_cast<float>(ang)/360., v1);
part_pose.block(0,0,3,3) = vt.toRotationMatrix();
Eigen::Matrix4f prev_pose_inv;
prev_pose_inv.block(0,0,3,3) = prev_pose.block(0,0,3,3).transpose();
prev_pose_inv.col(3).topRows(3) = -prev_pose_inv.block(0,0,3,3)*prev_pose.col(3).topRows(3);
cor_pt = prev_pose_inv * part_pose * ori_pt;
cor_pt = part_pose * ori_pt;
pt_compen.x = cor_pt(0,3);
pt_compen.y = cor_pt(1,3);
pt_compen.z = cor_pt(2,3);
pt_compen.intensity = static_cast<float>(ang)/360*255;
compen_frame->points.push_back(pt_compen);
next_pt = cur_pose * ori_pt;
pt_next.x = next_pt(0,3);
pt_next.y = next_pt(1,3);
pt_next.z = next_pt(2,3);
pt_next.intensity = static_cast<float>(ang)/360*255;
next_frame->points.push_back(pt_next);
}
original_frame->width = original_frame->size();
original_frame->height = 1;
pcl::io::savePCDFile<pcl::PointXYZI>("/home/yeonsoo/original_frame.pcd", *original_frame);
compen_frame->width = compen_frame->size();
compen_frame->height = 1;
pcl::io::savePCDFile<pcl::PointXYZI>("/home/yeonsoo/compen_frame.pcd", *compen_frame);
next_frame->width = next_frame->size();
next_frame->height = 1;
pcl::io::savePCDFile<pcl::PointXYZI>("/home/yeonsoo/next_frame.pcd", *next_frame);
예쁘게 잘 나온다.
그럼 이걸 이용해 slam을 한 번 해보자.
raw pointcloud frame이 들어왔을 때,
(0) NDT matching으로 cur_pose를 추정한다.
(1) 이전 프레임의 cur_pose 였던 prev_pose와, (0)에서 얻은 cur_pose를 이용해 위와 같이 프레임을 보상한다.
(2) 보상한 프레임으로 GICP를 돌려 최종 cur_pose를 얻고, 보상한 프레임을 키프레임으로 저장해 맵에 축적한다.
비교를 위해 라이다 보상파트를 넣지 않고
매 프레임별로 NDT -> GICP를 했을 때의 결과를 먼저 확인한다.
왼쪽 그림에서 파란색으로 표시된 부분의 가로등을 보면, 기둥이 두 개로 생긴다.
그 다음은 보상을 해줬을 때.
음.. 기대와 다르게 별 효과가 없다. 오히려 더 더럽게 보인다.
아! 혹시 이게 문제인가?
현재는 라이다 스캔의 시작점이 prev_pose이고 끝점이 cur_pose라고 가정하고 있는데 사실은 prev_pose와 cur_pose의 중점을 스캔의 시작점으로 놓고, 끝점을 cur_pose를 prev_pose<->cur_pose 간의 속도를 이용해 1.5만큼 연장한 지점으로 잡는게 맞나?
그래서 아래와 같이 바꿔보았다.
Eigen::Matrix4f cur_pose, prev_pose, real_prev_pose;
cur_pose = pose_;
prev_pose = getEPose(previous_pose_);
Eigen::Vector3f del_pose_t;
del_pose_t << cur_pose(0,3) - prev_pose(0,3),
cur_pose(1,3) - prev_pose(1,3),
cur_pose(2,3) - prev_pose(2,3);
Eigen::Matrix3f prev_R = prev_pose.block(0, 0, 3, 3);
Eigen::Matrix3f cur_R = cur_pose.block(0, 0, 3, 3);
Eigen::Quaternionf v0, v1, vt;
v0 = prev_R;
v1 = cur_R;
vt = v0.slerp(0.5, v1);
real_prev_pose = prev_pose;
real_prev_pose.col(3).topRows(3) += del_pose_t/2;
real_prev_pose.block(0, 0, 3, 3) = vt.toRotationMatrix();
del_pose_t /= 360.;
Eigen::Matrix3f real_prev_R = real_prev_pose.block(0, 0, 3, 3);
v0 = real_prev_R;
Eigen::Matrix4f prev_pose_inv;
prev_pose_inv.block(0,0,3,3) = real_prev_pose.block(0,0,3,3).transpose();
prev_pose_inv.col(3).topRows(3) = -prev_pose_inv.block(0,0,3,3)*real_prev_pose.col(3).topRows(3);
pcl::PointXYZI pt;
for (size_t i = 0; i < input_cloud->points.size(); i++)
{
Eigen::Matrix<float, 4, 1> ori_pt_f, compen_frame_w, compen_frame_f;
ori_pt_f << input_cloud->points[i].x, input_cloud->points[i].y, input_cloud->points[i].z, 1;
int ang = constrainAngle(RAD2DEG(atan2(input_cloud->points[i].y, input_cloud->points[i].x)));
ang = (90 - ang) % 360;
if (ang < 0)
ang += 360;
Eigen::Matrix4f part_pose; //on world frame
part_pose = real_prev_pose;
part_pose(0, 3) += del_pose_t(0, 3) * ang *2;
part_pose(1, 3) += del_pose_t(1, 3) * ang *2;
part_pose(2, 3) += del_pose_t(2, 3) * ang *2;
vt = v0.slerp(static_cast<float>(ang)/360.*2, v1);
part_pose.block(0,0,3,3) = vt.toRotationMatrix();
compen_frame_w = part_pose * ori_pt_f; //on world frame
compen_frame_f = prev_pose_inv * compen_frame_w; //back to prev frame
pt.x = compen_frame_f(0);
pt.y = compen_frame_f(1);
pt.z = compen_frame_f(2);
pt.intensity = input_cloud->points[i].intensity;
output_cloud->points.push_back(pt);
}여기 real_prev_pose가 prev_pose와 cur_pose의 중점이고, real_prev_pose와 prev_pose 사이의 포즈 변화량에 ang*2를 반영해줌으로써 구현하였다.
결과는 첫번째 시도와 비슷하게 안좋음..
그냥 imu 쓰자.
to be enterprising