gps heading 같은 걸 계산하는데, 결과가 괴랄하게 크게 나오는 경우가 있다.
그런 경우를 예방하려면
while (angle >= M_PI){
angle -= 2.*M_PI;
while(angle < -M_PI){
angle += 2.*M_PI;
}
return angle;
}
double XY2Theta(const double &x, const double &y){
auto atan_value = atan2(y,x);
return atan_value > 0 ? atan_value : atan_value + 2*M_PI;
}