로봇에 대한 Transformation Matrix는 Homogeneous Coordinate(homogeneous coordinates A coordinate system that algebraically treats all points in the projective plane (both Euclidean and ideal) equally. For example, the standard homogeneous coordinates [p1,p2,p3] of a point P in the projective plane are of the form [x,y,1] if P is a point in the Euclidean plane z=1 whose Cartesian coordinates are (x,y,1), or are of the form [a,b,0] if P is the ideal point – the point at infinity – associated to all lines in the Euclidean plane z=1 with direction numbers a,b,0. Homogeneous coordinates are so called because they treat Euclidean and ideal points in the same way.)이다.

node와 edge간에 관계를 이용하여서 Pose Graph 최적화를 이루어 낼때, Transformation matrix(R,T)을 가지고 계산하기에는 어렵다. 그 이유는 Rotation（+，- 로 계산 어려움, 곱셈으로 함）에 대한 Constraint이 있기 때문에 Gradient Decent를 가지고 global Minimal을 찾는게 쉽지 않다. 이에 Lie Aligebra(Targent space(로테이션에 대한 선형화), log)로 변환을 하여서 Rotation Matrix들을 Vector화를하면 “+,-“에 Non Constraint가 되기 때문에, 최적화를 쉽게 할 수 있다. 이에 최적화 작업이후에 대한 결과값들을 다시 Lie Group(Manifold space, exp)로 변환을 하여서 노드들에 대한 Transformation Matrix를 반영(업데이트))한다.

Reference

Graph SLAM CODE

Graph SLAM Bundle Adjustment 방법 및 코드 설명

Graph SLAM 설명 2

Graph SLAM CODE2

동기 신호 발생기(Sync Generator)에 고정시킨다는 뜻의 Generator lock의 합성어로 marster sync generator에 slave sync generator를 동기시키는 것을 말합니다. 일반적으로 동기신호의 주파수 및 위상을 맞춥니다.

동기화를 시키지 않는다면, 여러 센서를 가지고 오퍼레이션을 할때 각각 다른 프레임으로 센싱을 하기 때문에 time difference가 생기며, 만약 강체운동이 빠르다면, Time Difference로 생기는 센싱 차이로 인하여 문제가 생길 수 있다.

현재 카메라 같은 경어 Realsense 가 이 기능을 지원하고 있기때문에 손쉽게 할 수 있다.

External Sync -> trigger (Master - Slave Relationship)

실제 사용

Config 파일을 만들어서 realsense multi 카메라 launch하는 곳에 넣으면 끝!

마스터 그룹에다가 마스터에대한 yaml파일을 rosparam으로 설정할 수 있다.

이외에 슬레이브에 대해서 yaml파일을 만들고(multi_cam_param_slave 같은) 이 안에서 inter_cam_sync_mode를 2로 바꿔주기만 하면 된다.!(0은 Default)

<rosparam file="$(find vision_obstacle_pkg)/launch/realsense_config/multi_cam_param_master.yaml" />

# align_depth: True
# initial_reset: False
# enable_infra1: False
# enable_infra2: False
# filters: 'spatial,temporal,pointcloud'
# clip_distance: 3.0
# color_width: 848
# color_height: 480
# color_fps: 60.0
# depth_width: 848
# depth_height: 480
# depth_fps: 90.0
# enable_pointcloud: True
# dense_pointcloud: True
# enable_sync: True
# test
device_type: 'd455'
# rgb_camera: {
#     auto_exposure_priority: True,
#     backlight_compensation: True,
#     enable_auto_exposure: True,
#     enable_auto_white_balance: True,
#     frames_queue_size: 16,
#     power_line_frequency: 1,
#     saturation: 71,
#     sharpness: 74,
#     }
stereo_module:
    # emitter_always_on: False
    # emitter_enabled: 2
    # emitter_on_off: False
    # enable_auto_exposure: True
    # error_polling_enabled: True
    output_trigger_enabled: True
    frames_queue_size: 1
    inter_cam_sync_mode: 1
    laser_power: 150.0
    # visual_preset: 3
# temporal:
#     filter_smooth_alpha: 0.14
#     filter_smooth_delta: 37
#     frames_queue_size: 16
#     holes_fill: 6
# spatial:
#     filter_magnitude: 3
#     filter_smooth_alpha: 0.54
#     filter_smooth_delta: 11
#     holes_fill: 3
#     frames_queue_size: 16
# pointcloud:
#     stream_filter: 2
#     stream_format_filter: 6
#     frames_queue_size: 16
#     filter_magnitude: 2
# allow_no_texture_points: False
decimation:
    filter_magnitude : 4

싱크가 제대로 먹었는지 확인

rosrun dynamic_reconfigure dynparam get /camera0/stereo_module

rosrun rqt_reconfigure rqt_reconfigure

확인을 하는 방법은 아래에서 아이디어를 받아서 체크를 해보는 것을 배웠다.

Hardware sync causes the timestamps of the Slave cameras to try to follow the timing of the Master camera.  The slave cameras are still responsible for generating their own streams though - it does not come from the Master camera.  So if the Slave cameras are experiencing No Frames Received errors then that would indicate an issue with the particular camera rather than the sync cable circuit.   If the Slave cameras are not detecting a recognisable sync pulse then they should be streaming independently (unsynced).

Are you able to stream the two Slave cameras withut the No Frames Received error if you set their Inter Cam Sync Mode to '0' (no sync) please?  If you still get No Frames Received when they are unsynced, test them at 60 FPS and 30 FPS til you can get a stream.  If you cannot get a stream when unsynched at 90 FPS but you can at a lower FPS, this would suggest that the error is unrelated to sync.

https://support.intelrealsense.com/hc/en-us/community/posts/360051490514-Syncing-two-D455-Cameras-one-as-master-and-one-as-slave-unsuccessful

Reference

// multi camera in hw sync for ros

https://blog.csdn.net/eye998/article/details/89456134

https://dev.intelrealsense.com/docs/multiple-depth-cameras-configuration?_ga=2.5716145.363849929.1638153068-1202039520.1637734583

https://dev.intelrealsense.com/docs/lidar-camera-l515-multi-camera-setup

good explanation https://github.com/IntelRealSense/realsense-ros/issues/984

good reference

https://github.com/imitrob/surftask/blob/73e16f007d8c7778a0b1f0542e0b7cda089af51e/traj_complete_ros/launch/vision.launch

https://support.intelrealsense.com/hc/en-us/community/posts/360051490514-Syncing-two-D455-Cameras-one-as-master-and-one-as-slave-unsuccessful

https://support.intelrealsense.com/hc/en-us/community/posts/360050894574-How-to-get-the-same-timestamp-data-from-two-cameras

realsense output_trigger_enabled!! important

http://wiki.ros.org/rqt_reconfigure

background running : Nohup

nohup <program_type> <file_name> &

응용

nohup python3 auto_trade.py > output.log & # save as log file.

or

nohup python3 auto_trade.py &

check background running process

ps aux list

or

ps aux list | grep .py

Kill process

because of running in background, we need to kill a process in terminal

if we use above command, then we can get PID, using PID that we want to kill, we can do below method.

 kill -9 PID

GPS는 위성으로 부터 위도(Latitude)와 경도(Longitude)와 고도(altitude)의 정보를 받게 된다.

지구가 둥글기 떄문에, 이를 우리가 볼 수 있는 지도(맵)으로 쉽게 표현하고자 UTM(Universal Transverse Mercator Coordinate System)으로 변환을 하여서, 맵상에 어디있는지 표현을 할 수 있다.(많은 어플리케이션이 이것을 사용함)

또한 특정 영역(Local Coordinate)를 기준으로 Targent Space로 Localization이 표현이 가능한데(ENU : East, north, up), map에 대한 초기값을 잡고 gps데이터로 부터 로컬에 대한 맵핑이 가능합니다.

지구중심지구고정 좌표계(ECEF, earth-centered earth-fixed frame)는 지구의 중심에 원점이 위치하며 지구에 고정되어 있어서 지구와 함께 자전하는 좌표계이다.

Since the entire ECEF reference frame rotates with the earth, this coordinate system is useful for positioning geo-stationary objects such as satellites. In fact, the Global Positioning System (GPS) uses ECEF as it’s primary coordinate system and derives all other coordinates from it

지구 중심 좌표로, 위치에 대한 정보 xyz를 구할 수 있다.

rosrun rqt_top rqt_top