pi2pi

DONGJE LEE·2022년 6월 18일
0

ROS

목록 보기
4/8
post-thumbnail

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;
}
profile
LiDAR & SLAM & Robotics & Autonomous System

0개의 댓글