How to correctly make a transformation?

[MagdaZal]

Hi! I want to add an INS to tf which will transform pointcloud from ORB SLAM2 to real world coordinate. My idea is to do a transformation so that the INS is at the rotational center of the robot.

My tf_tree looks like this: INS -> map -> base_link -> camera_link

My problem - despite the set transformation, the point cloud is saved in the orb slam system, not in the real coordinate system with INS. For my future work I have to get point cloud with world coordinates in ECEF.

Has anyone ever had this problem or can give me some advice?

1. Between INS and map I made a broadcaster that reads the data from IMU (x,y,z,w) and transforms it to ROS msg.

transform_msg.header.frame_id = "INS" transform_msg.child_frame_id = "map"

2. If I understand correctly, ORB SLAM2 transforms the point cloud from base_link to the map. Node.cc

//static parameters nodehandle.param(name_ofnode+ "/publish_pointcloud", publish_pointcloudparam, true); nodehandle.param(name_ofnode+ "/publish_pose", publish_poseparam, true); nodehandle.param(name_ofnode+ "/publish_tf", publish_tfparam, true); nodehandle.param(name_ofnode+ "/pointcloud_frame_id", map_frame_idparam, "INS"); nodehandle.param(name_ofnode+ "/camera_frame_id", camera_frame_idparam, "camera_link"); nodehandle.param(name_ofnode+ "/target_frame_id", target_frame_idparam, "base_link"); nodehandle.param(name_ofnode + "/map_file", map_file_nameparam, "map.bin"); nodehandle.param(name_ofnode + "/voc_file", voc_file_nameparam, "file_not_set"); nodehandle.param(name_ofnode + "/load_map", load_mapparam, false);

ros/launch/camera_mono.launch

!-- static parameters -- param name="load_map" type="bool" value="false" / param name="map_file" type="string" value="map.bin" / param name="voc_file" type="string" value="$(find orb_slam2_ros)/orb_slam2/Vocabulary/ORBvoc.txt" /

param name="pointcloud_frame_id" type="string" value="map" / param name="camera_frame_id" type="string" value="camera_link" / param name="min_num_kf_in_map" type="int" value="5" /

3. And between base_link and camera_link I have a static_transform_publisher.

node pkg="tf2_ros" type="static_transform_publisher" name="gopro_offset" args="0 0 0 0 0 0 base_link camera_link" / include file="$(find orb_slam2_ros)/ros/launch/GOPRO.launch" /