Obstacle detection을 이용하여 받아온 Obstacle, 즉 라이다에서 받아온 obstacle의 상대좌표를 World 좌표계 상의 고정 절대좌표로 변환하는 방법이다.

전체적인 개념은 Obstacle의 상대좌표를 절대좌표로 변환하기 위해 GPS의 localization을 통해서 받아온 차량의 위치값을 Obstacle의 상대좌표값에 더해준다고 생각하면 된다.

절대 좌표 상의 World 좌표계와 Lidar 좌표계는 아래와 같이 나타내어진다.

편의상 z축은 생략했다.

World 좌표계는 정동 방향을 기준으로 반시계 방향으로 양의 방향인 좌표계이고 Lidar 좌표계는 오른손 좌표계를 따른다고 생각하면 된다. Lidar 좌표계에서 detection된 obstacle의 좌표를 World 좌표상의 절대 좌표로 나타내려면 우선 두 좌표계를 일치시키는 작업이 필요하다.

1차원적으로 생각하면 Lidar 좌표계를 오른쪽으로 90도 회전시켜서 일치시키면 된다고 생각하겠지만 그럴 필요 없이 차체에 Lidar를 아래와 같은 그림으로 장착시키면 저절로 좌표계가 일치하게 된다.

한 가지 예를 들어 위의 그림과 같은 상황에 놓여있다고 가정한다.

현재 World 좌표상의 차량의 좌표를 (x, y)라 하고 Lidar 좌표상의 obstacle의 좌표를 (x_lidar, y_lidar)라고 하면 최종적인 obstacle의 고정 좌표는 Lidar 좌표계를 World 좌표계와 일치시킨 후의 obstacle 좌표에 (x, y)를 더해준 값이 될 것이다.

이제 여기서 차량의 yaw값이 도입된다. yaw값에 따른 회전 변환이 필요하기 때문이다.

이렇게 전체적인 과정을 거치면 최종적인 World 좌표계 상의 obstacle의 고정 절대좌표 (X', Y')값은 아래와 같은 값을 가지게 될 것이다.

이 부분에 대한 코드는 아래와 같다.

double theta = yaw * pi / 180;  // yaw값을 라디안값으로 바꾼 후 theta에 대입
for (const Segment& s : segments_) {
	obstacle_detector::SegmentObstacle segment;
	segment.first_point.x = s.first_point().x;
	segment.first_point.y = s.first_point().y;
	segment.last_point.x = s.last_point().x;
	segment.last_point.y = s.last_point().y;

/*segment.first_point.x = s.first_point().x * cos(theta) + s.first_point().y * -sin(theta);
	segment.first_point.y = s.first_point().x * sin(theta) + s.first_point().y * cos(theta);
	segment.last_point.x = s.last_point().x * cos(theta) + s.first_point().y * -sin(theta);
	segment.last_point.y = s.last_point().x * sin(theta) + s.first_point().y * cos(theta);*/

	obstacles.segments.push_back(segment);
}

for (const Circle& c : circles_) {
  obstacle_detector::CircleObstacle circle;
  /*circle.center.x = c.center().x;
  circle.center.y = c.center().y;*/

  // 절대좌표 변환 , theta만큼 회전변환 시켜준 후 현재 좌표인 cur_x와 cur_y를 더해줌
  circle.center.x = c.center().x * cos(theta) + c.center().y * -sin(theta) + cur_x;
  circle.center.y = c.center().x * sin(theta) + c.center().y * cos(theta) + cur_y;
  cout << circle.center.x << endl;
  circle.radius = c.radius();

  obstacles.circles.push_back(circle);
}

obstacles_pub_.publish(obstacles);