[HDL Localization 사용설명서] 소스코드 상세 설명 (3)

앞 장에서는 패키지의 전체 구조와 개별 모듈의 역할을 확인했다. 본 장에서는 각 모듈마다 핵심적인 역할을 하는 소스코드를 설명한다.

5. 주요 소스코드 분석

5.1.1 hdl_localization_nodelet.cpp

Class : HdlLocalizationNodelet

public

• onInit
• Callback
  • imu_callback
  • points_callback
  • globalmap_callback
  • initailpose_callback
• Publish
  • publish_odometry
  • publish_scan_matching_status

위와 같은 메소드를 가지고있다. 메소드 이름만 보면 구조가 이해되는 직관적인 구성이다. 핵심 알고리즘은 LiDAR로부터 데이터를 받는 points_callback 함수에서 수행된다.

void points_callback(const sensor_msgs::PointCloud2ConstPtr& points_msg) {

******* 1. global map이 없으면 실행되지 않음
    if(!globalmap) {
      NODELET_ERROR("globalmap has not been received!!");
      return;
    }

******* 2. 라이다 데이터가 입력되지 않으면 실행되지 않음 (IMU는 입력없어도 동작)
    if(pcl_cloud->empty()) {
      NODELET_ERROR("cloud is empty!!");
      return;
    }

******* 3. 다운샘플링 수행 -> 연산시간 단축 (Voxelgrid 기반)
    auto filtered = downsample(cloud);
    last_scan = filtered;

******* 4. IMU를 사용해서 pose prediction 수행 (option)
    
		*** IMU가 없는경우 -> constant 하게 직선운동 한다고 가정
    if(!use_imu) {
      pose_estimator->predict(stamp);
      } else {
		*** IMU가 있는경우 -> imu 가속도&각속도 (6축) 사용해서 칼만필터 갱신
        pose_estimator->predict((*imu_iter)->header.stamp, acc_sign * Eigen::Vector3f(acc.x, acc.y, acc.z), gyro_sign * Eigen::Vector3f(gyro.x, gyro.y, gyro.z));
      }
    }

******* 5. Odometry 기반 pose prediction 수행 (option)

    if(private_nh.param<bool>("enable_robot_odometry_prediction", false) && !last_correction_time.isZero()) {
				pose_estimator->predict_odom(delta.cast<float>().matrix());
      }
    }

******* 6. NDT 알고리즘(라이다 기반)을 통한 pose measurement 수행 (필수)
    // but 특별한 알고리즘 없이 pcl 알고리즘의 registeration 사용....
    auto aligned = pose_estimator->correct(stamp, filtered);
				// 1. 오도메트리 비교
				//      * IMU, Odometry 기반 prediction의 오돔
				//      * NDT 스캔매칭 기반 오돔
		    // 2. 두 오돔의 TF차이를 통해 칼만필터의 오차공분산 및 평균을 구함
 		    // 3. 위 값으로 칼만게인 수렴

******* 7. 데이터 출력
    if(aligned_pub.getNumSubscribers()) {
      aligned->header.frame_id = "map";
      aligned->header.stamp = cloud->header.stamp;
      aligned_pub.publish(aligned);
    }

    if(status_pub.getNumSubscribers()) {
      publish_scan_matching_status(points_msg->header, aligned);
    }

    publish_odometry(points_msg->header.stamp, pose_estimator->matrix());
  }

중간중간 생략하고 중요한 부분만 남겼다. 위와 같은 과정을 통해 알고리즘이 수행된다. Localization의 핵심 기술은 4, 5번의 예측과 6번의 스캔매칭 정합 과정이다.

4번 기술의 경우 같은 패키지 내의 pose_estimator.cpp 소스코드에서, 6번 기술의 경우 ndt_omp 패키지에서 수행함

5.1.2 pose_estimator.cpp

Class : PoseEstimator

Public

• predict
• predict_odom
• correct

위 3개 메소드가 주요 기능이며, 나머지들은 단순 Getter 또는 상태확인용 메소드이다.

void PoseEstimator::predict(const ros::Time& stamp, const Eigen::Vector3f& acc, const Eigen::Vector3f& gyro) {
1. 초기화
  if (init_stamp.is_zero()) {
    init_stamp = stamp;
  }
2. 쿨다운 체크
  if ((stamp - init_stamp).toSec() < cool_time_duration || prev_stamp.is_zero() || prev_stamp == stamp) {
    prev_stamp = stamp;
    return;
  }

  double dt = (stamp - prev_stamp).toSec();
  prev_stamp = stamp;

3. 칼만필터 오차 계산
  ukf->setProcessNoiseCov(process_noise * dt);
  ukf->system.dt = dt;
  std::cout<<"ACC : "<<acc<<std::endl;
  std::cout<<"GYR : "<<gyro<<std::endl;

4. 칼만필터 예측
  Eigen::VectorXf control(6);
  control.head<3>() = acc;
  control.tail<3>() = gyro;

  ukf->predict(control);
}

predict 함수는 stamp 버젼과 stamp+IMU 버젼으로 오버라이딩 되어있다.

주요 내용은 완전히 같으며, 유일한 차이점은 IMU가 없는 버젼의 경우

→차량이 직진으로 등속도 운동을 하고있다는 가정을 기반으로 pseudo IMU데이터를 만들어 사용한다.

 

   2. 쿨다운 체크

IMU 데이터를 수신했을 때 어느정도 주기로 칼만필터를 갱신할 것인지에 대한 옵션이다. 너무 자주하면 칼만필터의 갱신비용이 늘고, 너무 드물게 하면 오차가 누적된다. 적절한 값을 사용하자.

 

3. 칼만필터 오차 계산

칼만필터에 대한 이해가 필요하다. process_noise는 예측 오차로, 주어진 dt에 대해서 어느정도 오차가 발생할지 예측한다.

 

void PoseEstimator::predict_odom(const Eigen::Matrix4f& odom_delta) {
  if(!odom_ukf) {
    Eigen::MatrixXf odom_process_noise = Eigen::MatrixXf::Identity(7, 7);
    Eigen::MatrixXf odom_measurement_noise = Eigen::MatrixXf::Identity(7, 7) * 1e-3;

    Eigen::VectorXf odom_mean(7);
    odom_mean.block<3, 1>(0, 0) = Eigen::Vector3f(ukf->mean[0], ukf->mean[1], ukf->mean[2]);
    odom_mean.block<4, 1>(3, 0) = Eigen::Vector4f(ukf->mean[6], ukf->mean[7], ukf->mean[8], ukf->mean[9]);
    Eigen::MatrixXf odom_cov = Eigen::MatrixXf::Identity(7, 7) * 1e-2;

    OdomSystem odom_system;
    odom_ukf.reset(new kkl::alg::UnscentedKalmanFilterX<float, OdomSystem>(odom_system, 7, 7, 7, odom_process_noise, odom_measurement_noise, odom_mean, odom_cov));
  }

  // invert quaternion if the rotation axis is flipped
  Eigen::Quaternionf quat(odom_delta.block<3, 3>(0, 0));
  if(odom_quat().coeffs().dot(quat.coeffs()) < 0.0) {
    quat.coeffs() *= -1.0f;
  }

  Eigen::VectorXf control(7);
  control.middleRows(0, 3) = odom_delta.block<3, 1>(0, 3);
  control.middleRows(3, 4) = Eigen::Vector4f(quat.w(), quat.x(), quat.y(), quat.z());

  Eigen::MatrixXf process_noise = Eigen::MatrixXf::Identity(7, 7);
  process_noise.topLeftCorner(3, 3) = Eigen::Matrix3f::Identity() * odom_delta.block<3, 1>(0, 3).norm() + Eigen::Matrix3f::Identity() * 1e-3;
  process_noise.bottomRightCorner(4, 4) = Eigen::Matrix4f::Identity() * (1 - std::abs(quat.w())) + Eigen::Matrix4f::Identity() * 1e-3;

  odom_ukf->setProcessNoiseCov(process_noise);
  odom_ukf->predict(control);
}

런치파일의 enable_robot_odometry_prediction 파라미터를 설정하면 수행되는 함수이다. 과거의 odom을 기반으로 현 시점의 odom을 예측해준다.

5.1.3 globalmap_server_nodelet.cpp

Class : GlobalmapServerNodelet

Public

• onInit

Private

• initialize_params
• pub_once_cb
• map_update_callback

중요하지 않은 패키지라 쓸까말까 하다가 썼다.

void initialize_params() {

1. PCD 파일 읽기
    // read globalmap from a pcd file
    std::string globalmap_pcd = private_nh.param<std::string>("globalmap_pcd", "");
    globalmap.reset(new pcl::PointCloud<PointT>());
    pcl::io::loadPCDFile(globalmap_pcd, *globalmap);
    globalmap->header.frame_id = "map";

2. UTM 파일 읽기+좌표변환
    std::ifstream utm_file(globalmap_pcd + ".utm");
    if (utm_file.is_open() && private_nh.param<bool>("convert_utm_to_local", true)) {
      double utm_easting;
      double utm_northing;
      double altitude;
      utm_file >> utm_easting >> utm_northing >> altitude;
      for(auto& pt : globalmap->points) {
        pt.getVector3fMap() -= Eigen::Vector3f(utm_easting, utm_northing, altitude);
      }
      ROS_INFO_STREAM("Global map offset by UTM reference coordinates (x = "
                      << utm_easting << ", y = " << utm_northing << ") and altitude (z = " << altitude << ")");
    }

3. 다운샘플링
    // downsample globalmap
    double downsample_resolution = private_nh.param<double>("downsample_resolution", 0.1);
    boost::shared_ptr<pcl::VoxelGrid<PointT>> voxelgrid(new pcl::VoxelGrid<PointT>());
    voxelgrid->setLeafSize(downsample_resolution, downsample_resolution, downsample_resolution);
    voxelgrid->setInputCloud(globalmap);

    pcl::PointCloud<PointT>::Ptr filtered(new pcl::PointCloud<PointT>());
    voxelgrid->filter(*filtered);

    globalmap = filtered;
  }

1. PCD 파일 읽기

PCD 파일을 읽어들인다. 런치파일에서 ~~.pcd로 경로 설정하던 파라미터가 여기 들어간다.

PCD 파일의 데이터가 pcl 라이브러리의 PointCloud<PointT> 형식으로 저장된다.

 

  2. UTM 파일 읽기+좌표변환

 

SLAM을 사용해서 지도 생성하면, 시작한 위치를 원점으로 좌표계를 구성한다. 이 때 알고있는 UTM 좌표계의 값을 오프셋으로 주는 기능을 한다.

 

  3. 다운샘플링

 

복셀그리드 기반의 다운샘플링을 진행한다. 연산량을 줄이고, 형상을 단순화하기 위함