What can I do once it works?

[OdedHorowits]

Hi!

So, after making it work comes the real question -
How can use the results of the algorithm? Lets say I move my drone around and map the area.
How can I use that map?
Can I use it on-the-fly for navigation?

In details:
The ORB_SLAM3 app publishes into to topics:

ros2 topic echo /drone0/map_data
ros2 topic echo /drone0/map_points

ros2 topic info /drone0/map_data says it uses slam_msgs/msg/MapData and
ros2 topic info /drone0/map_points says it uses sensor_msgs/msg/PointCloud2

Trying to echo /drone0/map_points gave me nothing. I though that the reason is the msg is too big, but using rviz I get nothing also when adding a PointCloud that subscribes to this topic.
Echoing slam_msgs/msg/MapData however gives me some results.
The structure of the msg is a following:

std_msgs/Header header
	builtin_interfaces/Time stamp
		int32 sec
		uint32 nanosec
	string frame_id

#optimized graph
slam_msgs/MapGraph graph
	std_msgs/Header header
		builtin_interfaces/Time stamp
			int32 sec
			uint32 nanosec
		string frame_id
	int32[] poses_id
	geometry_msgs/PoseStamped[] poses
		std_msgs/Header header
			builtin_interfaces/Time stamp
				int32 sec
				uint32 nanosec
			string frame_id
		Pose pose
			Point position
				float64 x
				float64 y
				float64 z
			Quaternion orientation
				float64 x 0
				float64 y 0
				float64 z 0
				float64 w 1

#graph data
slam_msgs/KeyFrame[] nodes
	int32 id
	geometry_msgs/Point[] word_pts
		float64 x
		float64 y
		float64 z

Using a python subscriber I can extract the results, that looks like this:

====================================================================

[INFO] [1719309314.196997704] [map_data_subscriber]: Graph Header:
[INFO] [1719309314.197535030] [map_data_subscriber]: Frame ID: map
[INFO] [1719309314.198119681] [map_data_subscriber]: Timestamp: 0s 0ns

[INFO] [1719309314.198958030] [map_data_subscriber]: Optimized Graph:
[INFO] [1719309314.199700617] [map_data_subscriber]:   Frame ID: 
[INFO] [1719309314.200628111] [map_data_subscriber]:   Timestamp: 0s 0ns
[INFO] [1719309314.201512677] [map_data_subscriber]: Poses ID: [0, 4, 1, 2, 3, 14, 38, 48, 62, 65, 67, 68, 73, 90, 84, 86, 70, 91, 89, 87, 88, 81]
[INFO] [1719309314.202609746] [map_data_subscriber]: Poses:
[INFO] [1719309314.203258511] [map_data_subscriber]:   Pose:
[INFO] [1719309314.203961693] [map_data_subscriber]:     Position: x=0.000, y=0.000, z=0.000
[INFO] [1719309314.205278722] [map_data_subscriber]:     Orientation: x=0.000, y=0.000, z=0.000, w=1.000
[INFO] [1719309314.206490162] [map_data_subscriber]:   Pose:
[INFO] [1719309314.209347471] [map_data_subscriber]:     Position: x=-0.013, y=0.002, z=2.565
[INFO] [1719309314.211288744] [map_data_subscriber]:     Orientation: x=0.002, y=-0.034, z=0.000, w=0.999
[INFO] [1719309314.212159562] [map_data_subscriber]:   Pose:
[INFO] [1719309314.212974634] [map_data_subscriber]:     Position: x=0.011, y=-0.000, z=1.097
[INFO] [1719309314.213891018] [map_data_subscriber]:     Orientation: x=0.003, y=-0.003, z=0.001, w=1.000
[INFO] [1719309314.214714715] [map_data_subscriber]:   Pose:
[INFO] [1719309314.215618662] [map_data_subscriber]:     Position: x=0.010, y=0.001, z=1.138
[INFO] [1719309314.216503208] [map_data_subscriber]:     Orientation: x=0.004, y=0.024, z=0.001, w=1.000
[INFO] [1719309314.217514307] [map_data_subscriber]:   Pose:
[INFO] [1719309314.218166416] [map_data_subscriber]:     Position: x=0.006, y=-0.005, z=1.731
[INFO] [1719309314.218971740] [map_data_subscriber]:     Orientation: x=-0.001, y=-0.032, z=0.001, w=1.000
[INFO] [1719309314.219560311] [map_data_subscriber]:   Pose:
[INFO] [1719309314.220058909] [map_data_subscriber]:     Position: x=-0.012, y=-0.019, z=3.652
[INFO] [1719309314.220551867] [map_data_subscriber]:     Orientation: x=-0.003, y=-0.037, z=0.002, w=0.999
[INFO] [1719309314.221097756] [map_data_subscriber]:   Pose:
[INFO] [1719309314.221565057] [map_data_subscriber]:     Position: x=-0.014, y=-0.002, z=3.654
[INFO] [1719309314.222104117] [map_data_subscriber]:     Orientation: x=-0.000, y=-0.026, z=0.001, w=1.000
[INFO] [1719309314.222826841] [map_data_subscriber]:   Pose:
[INFO] [1719309314.223358593] [map_data_subscriber]:     Position: x=-0.001, y=-0.032, z=3.603
[INFO] [1719309314.223897022] [map_data_subscriber]:     Orientation: x=0.001, y=0.029, z=0.000, w=1.000
[INFO] [1719309314.224433228] [map_data_subscriber]:   Pose:
[INFO] [1719309314.224994473] [map_data_subscriber]:     Position: x=-0.010, y=-0.024, z=3.634
[INFO] [1719309314.225489760] [map_data_subscriber]:     Orientation: x=0.002, y=-0.026, z=0.002, w=1.000
[INFO] [1719309314.226024291] [map_data_subscriber]:   Pose:
[INFO] [1719309314.226543916] [map_data_subscriber]:     Position: x=-0.029, y=0.005, z=3.665
[INFO] [1719309314.227078249] [map_data_subscriber]:     Orientation: x=0.006, y=-0.036, z=0.001, w=0.999
[INFO] [1719309314.227604373] [map_data_subscriber]:   Pose:
[INFO] [1719309314.228108954] [map_data_subscriber]:     Position: x=-0.028, y=-0.014, z=3.615
[INFO] [1719309314.228618967] [map_data_subscriber]:     Orientation: x=0.001, y=0.029, z=0.001, w=1.000
[INFO] [1719309314.229134493] [map_data_subscriber]:   Pose:
[INFO] [1719309314.229637485] [map_data_subscriber]:     Position: x=-0.008, y=-0.004, z=3.639
[INFO] [1719309314.230144051] [map_data_subscriber]:     Orientation: x=0.004, y=-0.029, z=0.002, w=1.000
[INFO] [1719309314.230628378] [map_data_subscriber]:   Pose:
[INFO] [1719309314.231133720] [map_data_subscriber]:     Position: x=-0.016, y=-0.002, z=3.632
[INFO] [1719309314.231777964] [map_data_subscriber]:     Orientation: x=0.002, y=-0.005, z=0.001, w=1.000
[INFO] [1719309314.232584568] [map_data_subscriber]:   Pose:
[INFO] [1719309314.233731438] [map_data_subscriber]:     Position: x=0.030, y=0.162, z=3.967
[INFO] [1719309314.234434491] [map_data_subscriber]:     Orientation: x=0.001, y=-0.019, z=-0.034, w=0.999
[INFO] [1719309314.234955198] [map_data_subscriber]:   Pose:
[INFO] [1719309314.235682070] [map_data_subscriber]:     Position: x=-0.038, y=-0.013, z=3.615
[INFO] [1719309314.236223849] [map_data_subscriber]:     Orientation: x=0.000, y=0.029, z=0.002, w=1.000
[INFO] [1719309314.236736824] [map_data_subscriber]:   Pose:
[INFO] [1719309314.237227303] [map_data_subscriber]:     Position: x=-0.020, y=-0.026, z=3.625
[INFO] [1719309314.237733662] [map_data_subscriber]:     Orientation: x=0.003, y=0.016, z=0.002, w=1.000
[INFO] [1719309314.240603471] [map_data_subscriber]:   Pose:
[INFO] [1719309314.242165247] [map_data_subscriber]:     Position: x=-0.034, y=-0.016, z=3.658
[INFO] [1719309314.243488185] [map_data_subscriber]:     Orientation: x=0.001, y=-0.022, z=0.001, w=1.000
[INFO] [1719309314.244487064] [map_data_subscriber]:   Pose:
[INFO] [1719309314.245269381] [map_data_subscriber]:     Position: x=0.027, y=-0.146, z=4.199
[INFO] [1719309314.246037474] [map_data_subscriber]:     Orientation: x=0.008, y=0.029, z=0.029, w=0.999
[INFO] [1719309314.246825802] [map_data_subscriber]:   Pose:
[INFO] [1719309314.247628231] [map_data_subscriber]:     Position: x=0.043, y=-0.192, z=4.195
[INFO] [1719309314.248443807] [map_data_subscriber]:     Orientation: x=0.002, y=0.023, z=0.032, w=0.999
[INFO] [1719309314.249202274] [map_data_subscriber]:   Pose:
[INFO] [1719309314.250213227] [map_data_subscriber]:     Position: x=0.004, y=-0.009, z=3.662
[INFO] [1719309314.251156173] [map_data_subscriber]:     Orientation: x=0.002, y=-0.025, z=0.001, w=1.000
[INFO] [1719309314.252336507] [map_data_subscriber]:   Pose:
[INFO] [1719309314.253064779] [map_data_subscriber]:     Position: x=0.011, y=-0.009, z=3.631
[INFO] [1719309314.253804713] [map_data_subscriber]:     Orientation: x=0.003, y=0.001, z=0.001, w=1.000
[INFO] [1719309314.254643644] [map_data_subscriber]:   Pose:
[INFO] [1719309314.255357690] [map_data_subscriber]:     Position: x=-0.013, y=-0.009, z=3.638
[INFO] [1719309314.256078394] [map_data_subscriber]:     Orientation: x=0.002, y=-0.020, z=0.002, w=1.000

[INFO] [1719309314.256807611] [map_data_subscriber]: Keyframes:

====================================================================

And some questions:
why do the optimized graph have no Frame ID?
why there are no keyframes?

Thanks

[suchetanrs]

Hi!
Thanks for the new issue.
No, ORB-SLAM3 cannot directly be used for navigation on the fly. Simply because it doesn't provide any 2D/3D occupancy map. What you can get out of this is an accurate localisation estimate and the optimised keyframe poses. These poses are the previously traversed areas of the robot. You'll need a separate mapping algorithm if you want to do this and later feed that map to a navigation stack.

[OdedHorowits]

Thank for your response.
I edited the former post, plz check it.
mainly, I ask -

• Why do the optimized graph have no Frame ID?
• Why there are no keyframes?
• Why can be the problem with the PointCloud?

And, if I start flying around, would the poses array keep grow? Does it store all the previously traversed areas?

[suchetanrs]

Hi,
So /drone0/map_points will have messages only if the ros_visualization parameter is set to true in the params file. This is still under development, I have not been able to test it.

To answer your questions.

1. The optimized graph poses don't have a frame ID because they are defined in the global frame. In this case, the map frame.
2. They keyframes are empty because I did not want to populate the tracked points for each keyframe. Maybe I should parametrize this, please feel free to let me know if you want to do this, maybe a raise PR for that if it works. However, the poses you can see from your python file are in fact, the keyframe poses.
3. Like I mentioned earlier, the problem could be the ros_visualization parameter. If it still does not show up with the value being true, I will need more information before proposing a solution.

[OdedHorowits]

Thank you so much for taking the time for thinking and answering.
Regarding the last question - if I start flying around, would the poses array keep grow? does it store all the previously traversed areas?

[suchetanrs]

Yes, you are right. The pose array will keep growing as long as the SLAM is running and it doesn't lose tracking. Yes, it is the previously traversed areas.

[OdedHorowits]

Hi
Does the behavior shown here is ok? doesn't seem so...

[OdedHorowits]

Hi! Thanks for the new issue. No, ORB-SLAM3 cannot directly be used for navigation on the fly. Simply because it doesn't provide any 2D/3D occupancy map. What you can get out of this is an accurate localisation estimate and the optimised keyframe poses. These poses are the previously traversed areas of the robot. You'll need a separate mapping algorithm if you want to do this and later feed that map to a navigation stack.

Looking at the ORB-SLAM3: Map Viewer app, I see the following after moving the drone in a rectangular path:

How do I get the data for those blue signs?
That is actually the localization data, where the drone thinks he is.

[suchetanrs]

Hi, regarding the TF Issue, I'll have a look at what's going on. Can you give me more information about your setup? Are you running with or without odometry? If the drone doesn't provide odometry data then use the wrapper with the parameter no_odometry_mode set to true.

For the second comment, you should be able to find this information on a topic named map_data. I think this was the topic name or should be a name similar to this. The data you see in the viewer is published at a fixed frequency on this topic.

[OdedHorowits]

regarding the TF Issue

so the setting is correct, i.e. no_odometry_mode: true.

Can you give me more information about your setup?

I am using Aerostack2. What information can i give that would help?

[suchetanrs]

Hi, I tried to recreate this issue. I was unable to do it.
I set no_odometry_mode to true and passed only two topics into the docker container. The rgb image and the depth image and it seems to run fine.
Can you please try the following?

• Make sure you have run colcon build --symlink-install after you have changed the parameter file. Try doing a clean build by deleting the build and install directories in the workspace.
• Try to open rviz inside the docker container and make sure under the TF section there is no other node publishing the transform between map and base_footprint.
• Make sure your parameter file looks like the following.

ORB_SLAM3_RGBD_ROS2:
  ros__parameters:
    robot_base_frame: base_footprint
    global_frame: map
    odom_frame: odom
    robot_x: 0.0
    robot_y: 0.0
    visualization: true
    ros_visualization: false
    no_odometry_mode: true

Sorry for the late reply, the issue is still in a blackbox :p I think more debugging might be needed.

[OdedHorowits]

I don't see any TF section in rviz :-(

---

Edit: ok found that: in rviz (Clicking the "Add" button, then select the "TF" display type)
That gave me:

---

What makes me think about the first part of the params file -

ORB_SLAM3_RGBD_ROS2:
  ros__parameters:
    robot_base_frame: base_footprint
    global_frame: map
    odom_frame: odom

Using ros2 run rqt_tf_tree rqt_tf_tree I can create the following image:

However, it also repeatedly prints the following errors to the shell where I run the rqt_tf_tree tool:

         at line 209 in ./src/buffer_core.cpp
Error:   TF_NO_CHILD_FRAME_ID: Ignoring transform from authority "default_authority" because child_frame_id not set 
         at line 217 in ./src/buffer_core.cpp
Error:   TF_NO_FRAME_ID: Ignoring transform with child_frame_id ""  from authority "default_authority" because frame_id not set
         at line 224 in ./src/buffer_core.cpp

That is very similar to the errors I get when launching the orb_slam wrapper.