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 node_handle_.param(name_of_node_+ "/publish_pointcloud", publish_pointcloud_param_, true); node_handle_.param(name_of_node_+ "/publish_pose", publish_pose_param_, true); node_handle_.param(name_of_node_+ "/publish_tf", publish_tf_param_, true); node_handle_.paramstd::string(name_of_node_+ "/pointcloud_frame_id", map_frame_id_param_, "INS"); node_handle_.paramstd::string(name_of_node_+ "/camera_frame_id", camera_frame_id_param_, "camera_link"); node_handle_.paramstd::string(name_of_node_+ "/target_frame_id", target_frame_id_param_, "base_link"); node_handle_.paramstd::string(name_of_node_ + "/map_file", map_file_name_param_, "map.bin"); node_handle_.paramstd::string(name_of_node_ + "/voc_file", voc_file_name_param_, "file_not_set"); node_handle_.param(name_of_node_ + "/load_map", load_map_param_, 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" /