Daddy Makers: 스캔

레이블이 스캔인 게시물을 표시합니다. 모든 게시물 표시

2023년 9월 4일 월요일

의사(psuedo) 라이다 스캔 데이터 시뮬레이션 기술 소개

이 글은 의사(psuedo) 라이다 스캔 데이터 시뮬레이션 기술을 간략히 소개한다.

레퍼런스

2021년 10월 12일 화요일

pyLiDAR-SLAM 사용하기

이 글은 라이다 스캔을 통해 얻은 데이터를 이용해 실시간 SLAM 처리하는 알고리즘을 파이썬으로 라이브러리화한 pyLiDAR-SLAM 사용 방법을 간략히 공유한다.

설치

우분투 리눅스 운영체제에서 실행하므로, 아래 링크 등을 참고해 실행 환경을 미리 준비한다.

우분투 리눅스 운영체제 설치

다음 메뉴얼을 참고해 설치한다. 의존하고 있는 패키지 pycp_icp (python3 binding 필요), pyviz3d, opencv, g2o, open3d 모두 메뉴얼을 참고해 설치한다.

Install pyLiDAR

설치 시 여러가지 에러가 발생한다. 아래를 참고해 해결해 나간다.

설치하기 전에 패키지 의존 에러가 발생할 수 있으므로, 가상환경을 터미널에서 아래와 같이 활성화한다.

pip install --force-reinstall virtualenv
pip3 install --force-reinstall virtualenv
virtualenv venv_pylidar

source ./venv_pylidar/bin/activate

cmake 처리 시 virtualenv를 제대로 인식해 python3를 못찾을 수 있다. 이 경우, 아래와 같이 경로를 지정해 준다.

cmake .. -DPYTHON_EXECUTABLE=/home/ktw/Projects/pyLiDAR-SLAM/venv_pylidar/bin/python3

의존 패키지가 사용하는 github 소스코드 빌드 처리를 위해, github 로그인을 위한 계정, RSA 암호 키 등을 아래 링크를 참고해 터미널에서 설정한다.

First time git setting

몇몇 의존 패키지에서 사용하는 라이브러리에 에러가 발생할 수 있다. 아래는 이와 관련해 설치한 라이브러리이다.

pip install hydra-core --upgrade
pip install pygame
pip install PyOpenGL
pip install PyOpenGL-accelerate
pip install omegaconf

Eigen 패키지에서 버전 차이로 인해 Quaternion에서 컴파일 에러가 발생할 수 있다. 아래와 같이 처리한다.

/* .def("x", (NonConstCoeffReturnType (Eigen::Quaterniond::*) () const) &Eigen::Quaterniond::x())
.def("y", (NonConstCoeffReturnType (Eigen::Quaterniond::*) () const) &Eigen::Quaterniond::y())
.def("z", (NonConstCoeffReturnType (Eigen::Quaterniond::*) () const) &Eigen::Quaterniond::z())
.def("w", (NonConstCoeffReturnType (Eigen::Quaterniond::*) () const) &Eigen::Quaterniond::w()) */

설치가 끝나면 다음과 같다.

사용

사용 방법을 확인하기 위해 예제 데이터를 프로젝트 폴더에 다운받고, 다음을 실행한다.

python3 run.py -m num_workers=1 slam/initialization=NI slam/preprocessing=none slam/odometry=ct_icp_robust_shaky, icp_odometry slam.odometry.viz_debug=True slam/loop_closure=none slam/backend=none dataset=rosbag dataset.main_topic=horizontal_laser_3d dataset.accumulate_scans=True dataset.file_path=./b3-2016-04-05-15-51-36.bag hydra.run.dir=./TEST_DOC

혹은 아래와 같이 실행할 수도 있다.

python3 run.py num_workers=1 slam/initialization=NI slam/preprocessing=none slam/odometry=ct_icp_robust_shaky slam.odometry.viz_debug=True slam/loop_closure=none slam/backend=none dataset=rosbag dataset.main_topic=horizontal_laser_3d dataset.accumulate_scans=True dataset.file_path=./b3-2016-04-05-15-51-36.bag hydra.run.dir=./TEST_DOC

참고로, 이 프로그램의 형식은 다음과 같다.

python3 run.py num_workers=1 / # The number of process workers to load the dataset

slam/initialization=NI / # The initialization (NI=No Initialization / CV=Constant Velocity)

slam/preprocessing=grid_sample / # Preprocessing on the point clouds

slam/odometry=icp_odometry / # The Odometry algorithm

slam.odometry.viz_debug=True / # Whether to launch the visualization of the odometry

slam/loop_closure=none / # The loop closure algorithm selected (none by default)

slam/backend=none / # The backend algorithm (none by default)

dataset=rosbag / # The dataset selected (a simple rosbag here)

dataset.main_topic=horizontal_laser_3d / # The pointcloud topic of the rosbag

dataset.accumulate_scans=True / # Whether to accumulate multiple messages

dataset.file_path=./b3-2016-04-05-15-51-36.bag / # The path to the rosbag file

hydra.run.dir=./TEST_DOC # The log directory where the trajectory will be saved

실행 시 다음과 같이 hydra 패키지에서 에러가 발생할 수 있다.

사용 후에는 반듯이 가상환경을 빠져나와 패키지 설치 환경이 오염되지 않도록 한다.

deactivate

레퍼런스

pyLiDAR-SLAM

2020년 1월 9일 목요일

스테레오 이미지/IR/IMU 내장된 MYNT EYE 센서 사용기

이 글은 MYNT EYE 센서 사용 후기이다. 현재 3차원 역설계 비전 연구를 진행 중이라, 가성비 높은 센서가 필요한 부분이 있다. 지금까지 다음과 같은 종류의 장치를 구매해 환경을 설치하고 예제를 실행해 보았다.

고정식 고가 LiDAR (Trimble, FARO. $30,000~120,000): 고정밀 엔지니어링용
실시간 LiDAR (Velodyne. $4,000~20,000): 중저가 무인 이동체 객체 충돌 회피용 3차원 맵 생성
스테레오 이미지 기반 센서 (ZED. $400): 저가용 실내외 3차원 맵 생성
적외선 기반 센서 (Kinect, Realsense, $200): 저가용 실내 3차원 맵 생성
저가 2D LiDAR (RpLiDAR, $99): 저가용 실내 2차원 맵 생성
복합 3D 센서 (MYNT EYE, $250): 스테레오 이미지+IR+IMU 활용 3차원 맵 생성

오늘 소개할 MYNT EYE 센서는 TZBOT이란 회사에서 개발한 저가 3차원 DEPTH MAP 스캐너로 IMU, IR, 스테레오 카메라가 포함되어 있다.

MYNT EYE 사용 예시

사용 방법

AMAZON 등에서 해당 센서를 구입한다. 직구하면 가격은 200~300달러 정도 된다.

빌드하기 전에 UBUNTU, NVIDIA DRIVER, CUDA 등 기본적인 개발환경을 설치해야 놓아야 한다. 환경 준비가 되어 있다면, 다음 같이 소스코드를 다운로드 받는다.
git clone https://github.com/slightech/MYNT-EYE-S-SDK

생성된 폴더 안에서 다음과 같은 명령을 입력해, 코드를 빌드한다.
make
make init
make sample

참고로, NVIDIA JETSON TK 보드에서는 호환성 문제로 빌드되지 않았다. 이 센서는 INTEL 계열 API를 사용한다.

센서를 컴퓨터 USB에 연결하고, 빌드된 예제를 실행해 본다. 그리고, 센서 번호인 0을 입력한다.

./samples/_output/bin/camera_with_junior_device_api

문제없다면, 센서 데이터를 출력할 것이다.

다음은 get_depth_and_points 예제를 실행해 본 결과이다. 가까운 0.8미터쯤에서 포인트가 인식되며, 거리가 멀어질수록 오차가 커지는 것을 알 수 있다. 2미터 정도까지는 어느 정도 정확하게 인식된다. 10미터까지 인식된다.

이 센서는 IMU가 포함되어 있다. 다음은 이와 관련된 예제이다.

개인적 의견으로 다양한 DEPTH MAP 센서를 사용해 스캔을 해보니 가격은 저렴하나 성능은 서로 비슷하다는 것을 느꼈다. 스캔 센서는 가격에 비례해 정확도, 정밀도, 스캔 거리가 높아진다. IR 적외선 센서와 스테레오 이미지 센서는 각각 실외, 실내에서 사용할 때 강하고, LiDAR 센서는 정확도는 높지만 RGB가 없다. 이런 이유로 스캔 센서는 응용 목적에 따라 적합한 것을 활용해야 한다.

추신 - 이 글을 올리고 있을 때 벨로다인에서 10만원대 LiDAR 센서가 CES에서 공개되었다는 뉴스를 SNS를 통해 들었다. 올해는 3D point cloud processing 기술이 보편화되는 시점이 될 것 같다.

2019년 12월 11일 수요일

NVIDIA jetson nano 기반 LiDAR, IMU 만들기

이 글은 저렴한 RpLiDAR A1과 엔비디아 젯슨 나노를 이용해 이동형 스캐너를 개발하는 방법을 간단히 정리한다. RpLiDAR는 저렴한 2D 스캐너로 인터넷에서 99달러에 구매할 수 있다.

개발환경은 우분투 18.04와 ROS(설치)이다. 개발환경은 나노 개발자 설치 설명 사이트를 따라 다운로드하고 설치하면 된다(아래 링크 참고).

NVIDIA JETSON NANO 사용 및 설치 방법

참고로, 라즈베리파이나 아두이노를 이용한 방법은 아래 링크를 참고바란다.

RpLiDAR based on RaspberryPi and arduino

RpLiDAR 스캔 원리

이 장치는 레이저 송수신기가 내장되어 있고, 이를 회전해 거리를 측정한다.

좌표계는 왼쪽손 좌표 기준이다. 다음 그림과 같이 각도를 1도씩 회전하며 거리를 측정한다.

좀 더 상세한 설명은 다음 데이터 시트를 참고한다.

Data sheet A1, A2

장치 연결

RpLiDAR를 엔비디아 나노의 USB포트와 연결한다.

RpLiDAR A1

그리고, 다음과 같이 연결된 라이다를 확인해 본다.

$ usb-devices

$ usb-devices | grep "CP2102"

다음과 같은 부분을 발견하면, 엔비디아 나노와 제대로 연결된 것이다.

Product=CP2102 USB to UART Bridge Controller

If#= 0 Alt= 0 #EPs= 2 Cls=ff(vend.) Sub=00 Prot=00 Driver=cp210x

RpLiDAR 설치

RpLiDAR SDK를 다음과 같이 다운로드하고 메이크한다.

$ git clone https://github.com/Slamtec/rplidar_sdk

$ cd rplidar_sdk/sdk

$ make

다음과 같이 ttyUSB 읽기 권한을 획득한다.

sudo chmod 666 /dev/ttyUSB0

혹은 다음과 같이 권한을 영구히 설정한다.

cd /etc/udev/rules.d/

sudo gedit 50-usb-serial.rules

KERNEL=="ttyUSB0", GROUP="ktw", MODE="0666"

다음 명령을 실행하면 권한 획득 된 것을 알 수 있다.

ls -l /dev | grep ttyUSB

이제 빌드된 rp_lidar/output/Linux/Release 폴더 밑의 RpLidar 예제를 다음과 같이 실행해 본다.

$ sudo ./ultra_simple

다음 화면은 실행된 스캔 데이터 결과이다.

ROS node 소스를 다음과 같이 다운로드하여 빌드한다(ROS 설치되어 있다고 가정함. ROS는 이전 블로그 참고요).

cd ~/catkin_ws/src

git clone https://github.com/Slamtec/rplidar_ros

cd ..

catkin_make

빌드가 정상적으로 진행되면, 터미널에서 roscore를 실행한 후, 다음과 같이 roslaunch를 실행한다.

$ roslaunch rplidar_ros view_rplidar.launch

그럼, 다음과 같이 시각화된 스캔 데이터를 확인할 수 있다.

실행화면

rviz를 별도로 실행하려면 다음과 같이 실행하면 된다.

roslaunch rplidar_ros rplidar.launch

RpLiDAR 소스 분석

ROS 패키지는 client, node 두개 파일이 포함되어 있다. rplidar.launch 는 이를 실행하기 위한 설정이다. 설정은 node 이름, serial port 이름, 데이터 전송 속도, 프레임 id 및 각도 보정 설정이 포함된다.

node는 다음 순서로 실행된다.

1. scan, tcp ip, port, serial bps 등 설정 코드를 실행한다.

2. RPlidarDriver 를 생성한다.

3. 드라이버를 시리얼 포트와 연결한다.

4. 드라이버 정보를 얻는다.

5. stop_motor, start_motor 서비스를 등록한다.

6. 드라이버 스캔 시작 함수를 호출한다.

drv->startScan();

7. 스캔 데이터를 얻는다.

8. 스캔 데이터(/LaserScan Message)를 publish 한다.

     publish_scan(&scan_pub, angle_compensate_nodes, angle_compensate_nodes_count,
                             start_scan_time, scan_duration, inverted,
                             angle_min, angle_max, max_distance,
                             frame_id);

실제 퍼블리쉬되는 데이터는 rplidar_response_measurement_node_hq_t nodes[360*8] 에 담긴다. 구조체 의미는 다음과 같다.

typedef struct rplidar_response_measurement_node_hq_t {
    _u16   angle_z_q14;   // 각도
    _u32   dist_mm_q2;   // 거리(mm)
    _u8    quality; // 강도값
    _u8    flag;
} __attribute__((packed)) rplidar_response_measurement_node_hq_t;

센서에서 얻은 데이터는 ROS /LaserScan 메시지로 변환되어 토픽 메시지로 발행된다. 이 메시지 형식은 헤더, 최대/최소 각도, 각도 증가값, 스캔 시간 각격, 측정 값 최대/최소, 거리 값 데이터(ranges), 강도값으로 구성된다. 이 메시지는 ROS 표준 토픽이므로, RViz 등 ROS 패키지에서 호환되어 사용된다.

std_msgs/Header header
float32 angle_min
float32 angle_max
float32 angle_increment
float32 time_increment
float32 scan_time
float32 range_min
float32 range_max
float32[] ranges
float32[] intensities

실제 각도에 대한 거리는 다음과 같이 계산할 수 있다.

count = scan_time / time_increment

for i in range(count):

angle = angle_min + angle_increment * i

distance = ranges[i]

client는 단순히 /scan 토픽을 구독하는 역할을 한다. 구독된 토픽 값은 다음과 같이 각도와 거리값으로 출력한다.

void scanCallback(const sensor_msgs::LaserScan::ConstPtr& scan)
{
    int count = scan->scan_time / scan->time_increment;
    ROS_INFO("I heard a laser scan %s[%d]:", scan->header.frame_id.c_str(), count);
    ROS_INFO("angle_range, %f, %f", RAD2DEG(scan->angle_min), RAD2DEG(scan->angle_max));

    for(int i = 0; i < count; i++) {
        float degree = RAD2DEG(scan->angle_min + scan->angle_increment * i);
        ROS_INFO(": [%f, %f]", degree, scan->ranges[i]);
    }
}

이를 통해, 점군 생성 함수를 다음과 같이 정의할 수 있다.

void scanToPoint(float radian, float dist, float& x, float& y)
{
    float dy = dist * cos(radian);
    float dx = dist * sin(radian);
    x = dx;
    y = dy;
}

void scanCallback(const sensor_msgs::LaserScan::ConstPtr& scan)

...

        float x, y;
        scanToPoint(radian, scan->ranges[i], x, y);
        ROS_INFO(": [%f, %f][%f, %f]", degree, scan->ranges[i], x, y);
...

다음과 같이 실행해 보면, 변환된 점 좌표가 출력된다.

rosrun rplidar_ros rplidarNodeClient

로그를 보면, 각 각도가 1도씩 증가되어 해당 거리가 표시되는 것을 알 수 있다. 그 뒤에 x, y값이 출력된다.

SLAM 처리

ROS SLAM 기본 패키지를 이용해 SLAM 처리를 한다. 다음과 같은 순서로 Hector SLAM을 실행해 본다. 참고로 Hector의미는 Heterogeneous Cooperating Team Of Robots 약자이다. 이 팀은 TU Darmstadt 컴퓨터 사이언스 및 기계 공학 간 연구자 협업으로 만들어졌다(참고).

cd ~/catkin_ws/src

git clone https://github.com/NickL77/RPLidar_Hector_SLAM

cd ..

catkin_make

아나콘다 설치 환경에서는 소스 빌드 시 몇몇 에러가 발생할 수 있는 데, 다음과 같이 처리한다.

sudo apt install qt4-default

sudo apt install ros-melodic-cv-bridge

sudo gedit /opt/ros/indigo/share/cv_bridge/cmake/cv_bridgeConfig.cmake

set(cv_bridge_FOUND_CATKIN_PROJECT TRUE)
if(NOT "include;/usr/local/include/opencv" STREQUAL " ")
set(cv_bridge_INCLUDE_DIRS "")
set(_include_dirs "/usr/local/include/opencv;/usr/include;/usr/local/include")

이제 다음과 같이 실행해 본다.

sudo chmod 666 /dev/ttyUSB0

roslaunch rplidar_ros rplidar.launch

roslaunch hector_slam_launch tutorial.launch

실행은 다음과 같이 엔비디아 나노에 연결된 라이다를 이동하며 스캔해 보았다.

결과는 다음과 같다.

Hector SLAM은 스캔된 점군 자체로 실시간 정합을 수행하는 SLAM이므로, 동일한 특징점을 발견하기 어려운 조건(스캔된 점군의 급격한 회전 및 이동), 특징점의 변화가 없는 구역(일직선 통로 등)에서 오류가 발생한다. 이를 보완하는 SLAM기법은 여러가지가 있다. 예를 들어, gmapping 은 odometry(주행계) 센서 값을 이용해 이를 보정한다. odometry센서는 다양하나 IMU가 지원되는 센서이면 가능하다(예. MPU6050). SLAM 알고리즘 성능 비교는 다음 링크를 참고한다.

IEEE, 2018, Comparison of Various SLAM Systems for Mobile Robot in an Indoor Environment

LIDAR에 대한 좀 더 상세한 내용은 다음 링크를 참고한다.

ROS Hector SLAM 분석

ROS에서 hector slam은 hector_mapping 노드에서 실행된다(참고). 이 노드는 레이저 스캔 프레임의 2차원 위치를 추정한다. 각 프레임과 옵션은 올바르게 설정되어 있어야 한다.

좌표계 프레임

다음 그림은 로봇이 거친 표면을 이동할 때, 로봇 플렛폼의 좌표계와 roll, pitch 모션 등을 보여준다. 좌표 프레임에 대한 상세 개념은 아래를 참고한다.

ROS기반 로봇 다중 좌표 프레임 개념 및 사용 방법

플랫폼 개념도

좌표 프레임은 odom을 사용하지 않는다면, 다음과 같이 설정된다.

이 경우, 퍼블리쉬 설정을 다음과 같이 처리한다.

패키지 노드 구성

노드는 다음과 같이 구성된다.

hector_mapping: The SLAM node.
hector_geotiff: Saving of map and robot trajectory to geotiff images files.
hector_trajectory_server: Saving of tf based trajectories(쾌적).

hector_mapping 노드는 LiDAR 기반 SLAM이며, odometry를 사용하지 않는다. ROS API는 옵션을 제공한다. 다음은 관련 토픽이다(참고).

scan: 레이저 스캔 데이터
syscommand: 시스템 명령. 예를 들어, 'reset'이면, 로봇 초기 위치등이 리셋됨
map_metadata: 토픽으로 부터 맵 메타데이터 획득
map: 토픽으로 부터 맵 데이터 획득
slam_out_pose: 로봇 위치 추정
poseupdate: 가우시안 기반 로봇 위치 추정

파라메터는 다음과 같다.

base_frame: 로봇 기본 프레임. 레이저 스캔 데이터의 localization, transformation에 사용됨
map_frame: 맵 프레임
odom_frame: odom 프레임
map_resolution: map 해상도. 기본은 0.025(m).
map_size: 맵 그리드 크기. 기본은 1024.
map_update_distance_thresh: default 0.4.
map_multi_res_levels: default 3

소스코드 분석

HECTOR SLAM 구조 역설계를 위해 github 해당 소스코드를 다운받아 분석해 본다.

roslaunch hector_slam_launch tutorial.launch 를 실행하면, launchh 에 있는 설정을 실행한다.

<node pkg="rviz" type="rviz" name="rviz"

args="-d $(find hector_slam_launch)/rviz_cfg/mapping_demo.rviz"/>

</include>

</launch>

여기서 hector_mapping 실제 동작은 해당 폴더의 launch에 있는 mapping_default.launch 를 통해 실행된다.

</node>

</launch>

전체 패키지는 13개로 구성되어 있다. 여기서 핵심은 hector_mapping 모듈이며, hector_slam, hector_slam_launch는 launch 패키지이다. hector_imi 패키지는 IMU센서 데이터를 사용해 odometry 값을 보정한다.

hector_mapping 모듈이 SLAM알고리즘을 구현하고 있다. 모듈 구조가 복잡하므로, UML 클래스 다이어그램으로 분석해 본다.

hector_mapping의 핵심 모듈은 HectorMappingRos이다. 이 모듈 코드 실행 순서는 다음과 같다.

slamProcessor = new hectorslam::HectorSlamProcessor(static_cast<float>(p_map_resolution_), p_map_size_, p_map_size_, Eigen::Vector2f(p_map_start_x_, p_map_start_y_), p_map_multi_res_levels_, hectorDrawings, debugInfoProvider);

slamProcessor->setUpdateFactorFree(p_update_factor_free_);

slamProcessor->setUpdateFactorOccupied(p_update_factor_occupied_);

slamProcessor->setMapUpdateMinDistDiff(p_map_update_distance_threshold_);

slamProcessor->setMapUpdateMinAngleDiff(p_map_update_angle_threshold_);

for (int i = 0; i < mapLevels; ++i)

{

mapPubContainer.push_back(MapPublisherContainer());

slamProcessor->addMapMutex(i, new HectorMapMutex());

...

}

scanSubscriber_ = node_.subscribe(p_scan_topic_, p_scan_subscriber_queue_size_, &HectorMappingRos::scanCallback, this);

sysMsgSubscriber_ = node_.subscribe(p_sys_msg_topic_, 2, &HectorMappingRos::sysMsgCallback, this);

poseUpdatePublisher_ = node_.advertise<geometry_msgs::PoseWithCovarianceStamped>(p_pose_update_topic_, 1, false);

posePublisher_ = node_.advertise<geometry_msgs::PoseStamped>("slam_out_pose", 1, false);

scan_point_cloud_publisher_ = node_.advertise<sensor_msgs::PointCloud>("slam_cloud",1,false);

}

void HectorMappingRos::scanCallback(const sensor_msgs::LaserScan& scan)

{

if (!p_use_tf_scan_transformation_) // TF 사용시

{

if (rosLaserScanToDataContainer(scan, laserScanContainer,slamProcessor->getScaleToMap())) // 점군 변환 후

{

slamProcessor->update(laserScanContainer,slamProcessor->getLastScanMatchPose()); // SLAM 처리

}

else // TF 사용 안하면

{

projector_.projectLaser(scan, laser_point_cloud_,30.0);

Eigen::Vector3f startEstimate(Eigen::Vector3f::Zero());

if(rosPointCloudToDataContainer(laser_point_cloud_, laserTransform, laserScanContainer, slamProcessor->getScaleToMap())) // 점군 변환 후

{

stamped_pose.getBasis().getEulerYPR(yaw, pitch, roll); // 스탬프 위치에서 YPR 획득

startEstimate = Eigen::Vector3f(stamped_pose.getOrigin().getX(),stamped_pose.getOrigin().getY(), yaw);

slamProcessor->update(laserScanContainer, startEstimate, true); // SLAM 처리

}

poseUpdatePublisher_.publish(poseInfoContainer_.getPoseWithCovarianceStamped());

posePublisher_.publish(poseInfoContainer_.getPoseStamped());

}

if(p_pub_odometry_)

{

tmp.header = poseInfoContainer_.getPoseWithCovarianceStamped().header;

tmp.child_frame_id = p_base_frame_;

odometryPublisher_.publish(tmp);

}

if (p_pub_map_odom_transform_)

{

tf::StampedTransform odom_to_base;

tf_.waitForTransform(p_odom_frame_, p_base_frame_, scan.header.stamp, ros::Duration(0.5));

tf_.lookupTransform(p_odom_frame_, p_base_frame_, scan.header.stamp, odom_to_base);

}

if (p_pub_map_scanmatch_transform_){

tfB_->sendTransform( tf::StampedTransform(poseInfoContainer_.getTfTransform(), scan.header.stamp, p_map_frame_, p_tf_map_scanmatch_transform_frame_name_));

}

void HectorMappingRos::publishMap(MapPublisherContainer& mapPublisher, const hectorslam::GridMap& gridMap, ros::Time timestamp, MapLockerInterface* mapMutex) // 맵 퍼블리쉬

{

mapPublisher.mapPublisher_.publish(map_.map);

}

bool HectorMappingRos::rosLaserScanToDataContainer(const sensor_msgs::LaserScan& scan, hectorslam::DataContainer& dataContainer, float scaleToMap) // 스캔 데이터를 점군으로 변환

{

for (size_t i = 0; i < size; ++i)

{

float dist = scan.ranges[i];

if ( (dist > scan.range_min) && (dist < maxRangeForContainer))

{

dist *= scaleToMap;

dataContainer.add(Eigen::Vector2f(cos(angle) * dist, sin(angle) * dist)); // x, y 점 좌표 변환

}

angle += scan.angle_increment;

}

HectorMappingRos은 HectorSlamProcessor의 update()를 호출한다. 이 모듈 코드 실행 순서는 다음과 같다.

class HectorSlamProcessor

{

void update(const DataContainer& dataContainer, const Eigen::Vector3f& poseHintWorld, bool map_without_matching = false)

{

Eigen::Vector3f newPoseEstimateWorld; // 위치 추정 벡터

if (!map_without_matching) // 매칭 없으면

{

newPoseEstimateWorld = (mapRep->matchData(poseHintWorld, dataContainer, lastScanMatchCov)); // 매칭 처리

}else{

newPoseEstimateWorld = poseHintWorld;

}

lastScanMatchPose = newPoseEstimateWorld;

if(util::poseDifferenceLargerThan(newPoseEstimateWorld, lastMapUpdatePose, paramMinDistanceDiffForMapUpdate, paramMinAngleDiffForMapUpdate) || map_without_matching)

{

mapRep->updateByScan(dataContainer, newPoseEstimateWorld); // 맵 데이터 업데이트

mapRep->onMapUpdated();

lastMapUpdatePose = newPoseEstimateWorld;

}

HectorSlamProcessor에서 호출된 matchData에서 점군을 매칭할 공분산을 리턴한다. 공분산은 해세 H 행렬이다.

class ScanMatcher

{

Eigen::Vector3f matchData(const Eigen::Vector3f& beginEstimateWorld, ConcreteOccGridMapUtil& gridMapUtil, const DataContainer& dataContainer, Eigen::Matrix3f& covMatrix, int maxIterations)

{

if (dataContainer.getSize() != 0) {

Eigen::Vector3f beginEstimateMap(gridMapUtil.getMapCoordsPose(beginEstimateWorld));

Eigen::Vector3f estimate(beginEstimateMap);

estimateTransformationLogLh(estimate, gridMapUtil, dataContainer); // 점군에서 위치 추정

int numIter = maxIterations;

for (int i = 0; i < numIter; ++i) {

estimateTransformationLogLh(estimate, gridMapUtil, dataContainer);

}

estimate[2] = util::normalize_angle(estimate[2]);

covMatrix = Eigen::Matrix3f::Zero();

covMatrix = H;

return gridMapUtil.getWorldCoordsPose(estimate);

}

return beginEstimateWorld;

}

bool estimateTransformationLogLh(Eigen::Vector3f& estimate, ConcreteOccGridMapUtil& gridMapUtil, const DataContainer& dataPoints)

{

gridMapUtil.getCompleteHessianDerivs(estimate, dataPoints, H, dTr); // 점군에서 위치 추정

if ((H(0, 0) != 0.0f) && (H(1, 1) != 0.0f)) {

Eigen::Vector3f searchDir (H.inverse() * dTr);

if (searchDir[2] > 0.2f) {

searchDir[2] = 0.2f;

std::cout << "SearchDir angle change too large\n";

} else if (searchDir[2] < -0.2f) {

searchDir[2] = -0.2f;

std::cout << "SearchDir angle change too large\n";

}

updateEstimatedPose(estimate, searchDir);

return true;

}

return false;

}

헤세 행렬은 getCompleteHessianDerivs 에서 계산된다.

class OccGridMapUtil

{

void getCompleteHessianDerivs(const Eigen::Vector3f& pose, const DataContainer& dataPoints, Eigen::Matrix3f& H, Eigen::Vector3f& dTr) // 헤세 Hessian행렬을 계산

{

int size = dataPoints.getSize();

Eigen::Affine2f transform(getTransformForState(pose));

float sinRot = sin(pose[2]);

float cosRot = cos(pose[2]);

H = Eigen::Matrix3f::Zero();

dTr = Eigen::Vector3f::Zero();

for (int i = 0; i < size; ++i) {

const Eigen::Vector2f& currPoint (dataPoints.getVecEntry(i));

Eigen::Vector3f transformedPointData(interpMapValueWithDerivatives(transform * currPoint));

float funVal = 1.0f - transformedPointData[0];

dTr[0] += transformedPointData[1] * funVal;

dTr[1] += transformedPointData[2] * funVal;

float rotDeriv = ((-sinRot * currPoint.x() - cosRot * currPoint.y()) * transformedPointData[1] + (cosRot * currPoint.x() - sinRot * currPoint.y()) * transformedPointData[2]); // 회전 편차

dTr[2] += rotDeriv * funVal;

H(0, 0) += util::sqr(transformedPointData[1]); // H 행렬

H(1, 1) += util::sqr(transformedPointData[2]);

H(2, 2) += util::sqr(rotDeriv);

H(0, 1) += transformedPointData[1] * transformedPointData[2];

H(0, 2) += transformedPointData[1] * rotDeriv;

H(1, 2) += transformedPointData[2] * rotDeriv;

}

H(1, 0) = H(0, 1);

H(2, 0) = H(0, 2);

H(2, 1) = H(1, 2);

}

앞서 hector slam 코드를 보면, 헤세 행렬 H을 구해 스캔 점군에 필요한 데이터를 얻는 것을 알 수 있다(참고 논문). 헤세 행렬을 구하기 위해서는 편미분 함수를 정의해야 한다. SLAM에서 지도 상에 주어진 점을 Pm이라 하면, 점유값 함수 M(Pm)을 정의하여 다음 수식을 세울 수 있다.

이중 선형 방법을 사용해 점유값을 근사화하면, P00, P10, P01, P11을 사용한 선형 보간은 다음과 같이 나타낼 수 있다.

그러므로, M 편미분 함수는 다음과 같이 근사화된다.

스캔 매칭은 문제는 지금까지 얻은 지도와 스캔된 점들 간 정렬을 위한 최적화 계산이 된다. 가우스-뉴턴 수치해석을 통해, 스캔 포인트 간에 정렬을 수행한다. 시작 추정 위치 ξ = (𝑝𝑥, 𝑝𝑦, 𝜓) T에서 스캔 매칭 목적은 M 함수를 최소화하는 것이된다.

여기서, 𝑆𝑖 (𝜉) 은 로봇 프레임에 수신된 스캔 데이터를 월드 좌표계로 변환하는 함수이다.

가우스 뉴턴 알고리즘은 비선형 최소 제곱 문제를 해결할 때 사용되고, 목표함수 𝑟𝑖는 다음과 같이 정의된다.

재귀 알고리즘으로 표현하면 다음과 같다.

로봇 위치의 움직임 Δ𝜉 이 매우 작다고 가정할 때, 가우스 뉴턴 알고리즘의 기울기 벡터 G는 다음과 같다.

이제, 2차 편미분을 무시하여, H 행렬을 정의한다.

𝑆𝑖(𝜉) 미분은 다음 행렬로 표시할 수 있다.

이제 최소값을 위한 Δ𝜉 를 계산한다.

헤토르 슬램의 비선형 근사 알고리즘을 정리하면 다음과 같다. 다만, 긴 복도 환경에서는 문제가 있다.

결국, 앞서 코드와 매치해 각 알고리즘 단계는 다음과 같이 수행되는 것을 알수 있다.

1. 헥터 스캔 매칭 함수(이전 점군, 현재 점군x, 현재 점군y, 점유 함수M)

void getCompleteHessianDerivs(const Eigen::Vector3f& pose, const DataContainer& dataPoints, Eigen::Matrix3f& H, Eigen::Vector3f& dTr)

2. 수신 데이터를 월드 좌표계로 계산

Eigen::Vector3f transformedPointData(interpMapValueWithDerivatives(transform * currPoint));

3. 최적해를 구하기 위한 구배함수 G 계산

float funVal = 1.0f - transformedPointData[0];

dTr[0] += transformedPointData[1] * funVal;

dTr[1] += transformedPointData[2] * funVal;

float rotDeriv = ((-sinRot * currPoint.x() - cosRot * currPoint.y()) * transformedPointData[1] + (cosRot * currPoint.x() - sinRot * currPoint.y()) * transformedPointData[2]);

dTr[2] += rotDeriv * funVal;

4. H 행렬 계산

H(0, 0) += util::sqr(transformedPointData[1]);

H(1, 1) += util::sqr(transformedPointData[2]);

H(2, 2) += util::sqr(rotDeriv);

H(0, 1) += transformedPointData[1] * transformedPointData[2];

H(0, 2) += transformedPointData[1] * rotDeriv;

H(1, 2) += transformedPointData[2] * rotDeriv;

}

5. 에러 편차 계산

Eigen::Vector3f searchDir (H.inverse() * dTr);

6. 편차 갱신

estimate += change;

7. 편차 리턴

참고로, 헤세 행렬은 편미분 함수로 f(x1, x2, x3, ..., xn) 에 대한 행렬을 구한것이다.

H는 이차미분함수(second derivative)이므로 대칭행렬이 된다. 그러므로, 고유벡터로 분해 가능하다. H는 함수 곡률을 나타내므로, 특징점(critical point) 종류를 판별하는데 사용할 수 있다. 어떤 함수의 일차 미분이 0이 되는 점은 함수의 극점이 된다. 어떤 함수의 특징점에서 계산한 Hessian 행렬의 모든 고유값이 음수이면 극대, 음 및 양의 고유값을 가지면 말안장 모양의 변곡점(saddle point)을 의미한다.

헤세 함수는 이미지 처리시 I(x, y)에서 픽셀 밝기에 대한 함수로 긴 모양의 패턴(모세혈관 등)을 검출하는 데 사용되기도 한다. 이 함수는 이미지 매칭에 사용되는 SURF알고리즘에도 사용된다.

스캔 테스트

작업된 2차원 라이다로 다음과 같이 간단히 패키징한 후, 영상과 같이 테스트해보았다.

스캔 거리는 약 5미터로 확인되었으며, 유리 같은 반사재질에는 데이터를 획득할 수 없었다. 스캔 거리가 멀수록 오차나 노이즈가 조금씩 심해진다. 당연하지만, 센서 수평을 유지하지 않으면 제대로 된 평면 포인트 클라우드를 얻을 수 없다. 가능한 가벼운 장비를 활용하였음에도 들고 다니기에는 손목이 아팠다.

부록: IMU 처리

나노에 IMU 센서를 연결하기 위해 MPU-9250를 사용한다.